diff options
Diffstat (limited to 'src')
53 files changed, 1155 insertions, 450 deletions
diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 52a667403..ec9d4ca09 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -118,11 +118,8 @@ ORB_DECLARE(output_pwm); /** start DSM bind */ #define DSM_BIND_START _IOC(_PWM_SERVO_BASE, 7) -/** stop DSM bind */ -#define DSM_BIND_STOP _IOC(_PWM_SERVO_BASE, 8) - /** Power up DSM receiver */ -#define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 9) +#define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 8) /** set a single servo to a specific value */ #define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo) diff --git a/src/drivers/md25/BlockSysIdent.cpp b/src/drivers/md25/BlockSysIdent.cpp new file mode 100644 index 000000000..23b0724d8 --- /dev/null +++ b/src/drivers/md25/BlockSysIdent.cpp @@ -0,0 +1,8 @@ +#include "BlockSysIdent.hpp" + +BlockSysIdent::BlockSysIdent() : + Block(NULL, "SYSID"), + _freq(this, "FREQ"), + _ampl(this, "AMPL") +{ +} diff --git a/src/drivers/md25/BlockSysIdent.hpp b/src/drivers/md25/BlockSysIdent.hpp new file mode 100644 index 000000000..270635f40 --- /dev/null +++ b/src/drivers/md25/BlockSysIdent.hpp @@ -0,0 +1,10 @@ +#include <controllib/block/Block.hpp> +#include <controllib/block/BlockParam.hpp> + +class BlockSysIdent : public control::Block { +public: + BlockSysIdent(); +private: + control::BlockParam<float> _freq; + control::BlockParam<float> _ampl; +}; diff --git a/src/drivers/md25/md25.cpp b/src/drivers/md25/md25.cpp index 71932ad65..d43e3aef9 100644 --- a/src/drivers/md25/md25.cpp +++ b/src/drivers/md25/md25.cpp @@ -45,9 +45,16 @@ #include "md25.hpp" #include <poll.h> #include <stdio.h> +#include <math.h> +#include <string.h> #include <systemlib/err.h> #include <arch/board/board.h> +#include <mavlink/mavlink_log.h> + +#include <controllib/uorb/UOrbPublication.hpp> +#include <uORB/topics/debug_key_value.h> +#include <drivers/drv_hrt.h> // registers enum { @@ -72,6 +79,9 @@ enum { REG_COMMAND_RW, }; +// File descriptors +static int mavlink_fd; + MD25::MD25(const char *deviceName, int bus, uint16_t address, uint32_t speed) : I2C("MD25", deviceName, bus, address, speed), @@ -106,7 +116,8 @@ MD25::MD25(const char *deviceName, int bus, setMotor2Speed(0); resetEncoders(); _setMode(MD25::MODE_UNSIGNED_SPEED); - setSpeedRegulation(true); + setSpeedRegulation(false); + setMotorAccel(10); setTimeout(true); } @@ -298,6 +309,12 @@ int MD25::setDeviceAddress(uint8_t address) return OK; } +int MD25::setMotorAccel(uint8_t accel) +{ + return _writeUint8(REG_ACCEL_RATE_RW, + accel); +} + int MD25::setMotor1Speed(float value) { return _writeUint8(REG_SPEED1_RW, @@ -451,12 +468,12 @@ int md25Test(const char *deviceName, uint8_t bus, uint8_t address) MD25 md25("/dev/md25", bus, address); // print status - char buf[200]; + char buf[400]; md25.status(buf, sizeof(buf)); printf("%s\n", buf); // setup for test - md25.setSpeedRegulation(true); + md25.setSpeedRegulation(false); md25.setTimeout(true); float dt = 0.1; float speed = 0.2; @@ -550,4 +567,68 @@ int md25Test(const char *deviceName, uint8_t bus, uint8_t address) return 0; } +int md25Sine(const char *deviceName, uint8_t bus, uint8_t address, float amplitude, float frequency) +{ + printf("md25 sine: starting\n"); + + // setup + MD25 md25("/dev/md25", bus, address); + + // print status + char buf[400]; + md25.status(buf, sizeof(buf)); + printf("%s\n", buf); + + // setup for test + md25.setSpeedRegulation(false); + md25.setTimeout(true); + float dt = 0.01; + float t_final = 60.0; + float prev_revolution = md25.getRevolutions1(); + + // debug publication + control::UOrbPublication<debug_key_value_s> debug_msg(NULL, + ORB_ID(debug_key_value)); + + // sine wave for motor 1 + md25.resetEncoders(); + while (true) { + + // input + uint64_t timestamp = hrt_absolute_time(); + float t = timestamp/1000000.0f; + + float input_value = amplitude*sinf(2*M_PI*frequency*t); + md25.setMotor1Speed(input_value); + + // output + md25.readData(); + float current_revolution = md25.getRevolutions1(); + + // send input message + //strncpy(debug_msg.key, "md25 in ", 10); + //debug_msg.timestamp_ms = 1000*timestamp; + //debug_msg.value = input_value; + //debug_msg.update(); + + // send output message + strncpy(debug_msg.key, "md25 out ", 10); + debug_msg.timestamp_ms = 1000*timestamp; + debug_msg.value = current_revolution; + debug_msg.update(); + + if (t > t_final) break; + + // update for next step + prev_revolution = current_revolution; + + // sleep + usleep(1000000 * dt); + } + md25.setMotor1Speed(0); + + printf("md25 sine complete\n"); + return 0; +} + // vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78 diff --git a/src/drivers/md25/md25.hpp b/src/drivers/md25/md25.hpp index e77511b16..1661f67f9 100644 --- a/src/drivers/md25/md25.hpp +++ b/src/drivers/md25/md25.hpp @@ -46,7 +46,7 @@ #include <poll.h> #include <stdio.h> -#include <controllib/block/UOrbSubscription.hpp> +#include <controllib/uorb/UOrbSubscription.hpp> #include <uORB/topics/actuator_controls.h> #include <drivers/device/i2c.h> @@ -213,6 +213,19 @@ public: int setDeviceAddress(uint8_t address); /** + * set motor acceleration + * @param accel + * controls motor speed change (1-10) + * accel rate | time for full fwd. to full rev. + * 1 | 6.375 s + * 2 | 1.6 s + * 3 | 0.675 s + * 5(default) | 1.275 s + * 10 | 0.65 s + */ + int setMotorAccel(uint8_t accel); + + /** * set motor 1 speed * @param normSpeed normalize speed between -1 and 1 * @return non-zero -> error @@ -290,4 +303,7 @@ private: // unit testing int md25Test(const char *deviceName, uint8_t bus, uint8_t address); +// sine testing +int md25Sine(const char *deviceName, uint8_t bus, uint8_t address, float amplitude, float frequency); + // vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78 diff --git a/src/drivers/md25/md25_main.cpp b/src/drivers/md25/md25_main.cpp index e62c46b0d..7e5904d05 100644 --- a/src/drivers/md25/md25_main.cpp +++ b/src/drivers/md25/md25_main.cpp @@ -82,7 +82,7 @@ usage(const char *reason) if (reason) fprintf(stderr, "%s\n", reason); - fprintf(stderr, "usage: md25 {start|stop|status|search|test|change_address}\n\n"); + fprintf(stderr, "usage: md25 {start|stop|read|status|search|test|change_address}\n\n"); exit(1); } @@ -136,6 +136,28 @@ int md25_main(int argc, char *argv[]) exit(0); } + if (!strcmp(argv[1], "sine")) { + + if (argc < 6) { + printf("usage: md25 sine bus address amp freq\n"); + exit(0); + } + + const char *deviceName = "/dev/md25"; + + uint8_t bus = strtoul(argv[2], nullptr, 0); + + uint8_t address = strtoul(argv[3], nullptr, 0); + + float amplitude = atof(argv[4]); + + float frequency = atof(argv[5]); + + md25Sine(deviceName, bus, address, amplitude, frequency); + + exit(0); + } + if (!strcmp(argv[1], "probe")) { if (argc < 4) { printf("usage: md25 probe bus address\n"); @@ -162,6 +184,29 @@ int md25_main(int argc, char *argv[]) exit(0); } + if (!strcmp(argv[1], "read")) { + if (argc < 4) { + printf("usage: md25 read bus address\n"); + exit(0); + } + + const char *deviceName = "/dev/md25"; + + uint8_t bus = strtoul(argv[2], nullptr, 0); + + uint8_t address = strtoul(argv[3], nullptr, 0); + + MD25 md25(deviceName, bus, address); + + // print status + char buf[400]; + md25.status(buf, sizeof(buf)); + printf("%s\n", buf); + + exit(0); + } + + if (!strcmp(argv[1], "search")) { if (argc < 3) { printf("usage: md25 search bus\n"); @@ -246,7 +291,7 @@ int md25_thread_main(int argc, char *argv[]) uint8_t address = strtoul(argv[4], nullptr, 0); // start - MD25 md25("/dev/md25", bus, address); + MD25 md25(deviceName, bus, address); thread_running = true; diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index ae56b70b3..fed83ea1a 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -89,21 +89,61 @@ #define PX4IO_SET_DEBUG _IOC(0xff00, 0) #define PX4IO_INAIR_RESTART_ENABLE _IOC(0xff00, 1) +/** + * The PX4IO class. + * + * Encapsulates PX4FMU to PX4IO communications modeled as file operations. + */ class PX4IO : public device::I2C { public: + /** + * Constructor. + * + * Initialize all class variables. + */ PX4IO(); + /** + * Destructor. + * + * Wait for worker thread to terminate. + */ virtual ~PX4IO(); + /** + * Initialize the PX4IO class. + * + * Initialize the physical I2C interface to PX4IO. Retrieve relevant initial system parameters. Initialize PX4IO registers. + */ virtual int init(); + /** + * IO Control handler. + * + * Handle all IOCTL calls to the PX4IO file descriptor. + * + * @param[in] filp file handle (not used). This function is always called directly through object reference + * @param[in] cmd the IOCTL command + * @param[in] the IOCTL command parameter (optional) + */ virtual int ioctl(file *filp, int cmd, unsigned long arg); + + /** + * write handler. + * + * Handle writes to the PX4IO file descriptor. + * + * @param[in] filp file handle (not used). This function is always called directly through object reference + * @param[in] buffer pointer to the data buffer to be written + * @param[in] len size in bytes to be written + * @return number of bytes written + */ virtual ssize_t write(file *filp, const char *buffer, size_t len); /** * Set the update rate for actuator outputs from FMU to IO. * - * @param rate The rate in Hz actuator outpus are sent to IO. + * @param[in] rate The rate in Hz actuator output are sent to IO. * Min 10 Hz, max 400 Hz */ int set_update_rate(int rate); @@ -111,29 +151,41 @@ public: /** * Set the battery current scaling and bias * - * @param amp_per_volt - * @param amp_bias + * @param[in] amp_per_volt + * @param[in] amp_bias */ void set_battery_current_scaling(float amp_per_volt, float amp_bias); /** * Push failsafe values to IO. * - * @param vals Failsafe control inputs: in us PPM (900 for zero, 1500 for centered, 2100 for full) - * @param len Number of channels, could up to 8 + * @param[in] vals Failsafe control inputs: in us PPM (900 for zero, 1500 for centered, 2100 for full) + * @param[in] len Number of channels, could up to 8 */ int set_failsafe_values(const uint16_t *vals, unsigned len); /** - * Print the current status of IO - */ + * Print IO status. + * + * Print all relevant IO status information + */ void print_status(); + /** + * Set the DSM VCC is controlled by relay one flag + * + * @param[in] enable true=DSM satellite VCC is controlled by relay1, false=DSM satellite VCC not controlled + */ inline void set_dsm_vcc_ctl(bool enable) { _dsm_vcc_ctl = enable; }; + /** + * Get the DSM VCC is controlled by relay one flag + * + * @return true=DSM satellite VCC is controlled by relay1, false=DSM satellite VCC not controlled + */ inline bool get_dsm_vcc_ctl() { return _dsm_vcc_ctl; @@ -141,52 +193,48 @@ public: private: // XXX - unsigned _max_actuators; - unsigned _max_controls; - unsigned _max_rc_input; - unsigned _max_relays; - unsigned _max_transfer; + unsigned _max_actuators; ///<Maximum # of actuators supported by PX4IO + unsigned _max_controls; ///<Maximum # of controls supported by PX4IO + unsigned _max_rc_input; ///<Maximum receiver channels supported by PX4IO + unsigned _max_relays; ///<Maximum relays supported by PX4IO + unsigned _max_transfer; ///<Maximum number of I2C transfers supported by PX4IO - unsigned _update_interval; ///< subscription interval limiting send rate + unsigned _update_interval; ///<Subscription interval limiting send rate - volatile int _task; ///< worker task - volatile bool _task_should_exit; + volatile int _task; ///<worker task id + volatile bool _task_should_exit; ///<worker terminate flag - int _mavlink_fd; + int _mavlink_fd; ///<mavlink file descriptor - perf_counter_t _perf_update; + perf_counter_t _perf_update; ///<local performance counter /* cached IO state */ - uint16_t _status; - uint16_t _alarms; + uint16_t _status; ///<Various IO status flags + uint16_t _alarms; ///<Various IO alarms /* subscribed topics */ - int _t_actuators; ///< actuator controls topic - int _t_armed; ///< system armed control topic - int _t_vstatus; ///< system / vehicle status - int _t_param; ///< parameter update topic + int _t_actuators; ///<actuator controls topic + int _t_armed; ///<system armed control topic + int _t_vstatus; ///<system / vehicle status + int _t_param; ///<parameter update topic /* advertised topics */ - orb_advert_t _to_input_rc; ///< rc inputs from io - orb_advert_t _to_actuators_effective; ///< effective actuator controls topic - orb_advert_t _to_outputs; ///< mixed servo outputs topic - orb_advert_t _to_battery; ///< battery status / voltage + orb_advert_t _to_input_rc; ///<rc inputs from IO topic + orb_advert_t _to_actuators_effective; ///<effective actuator controls topic + orb_advert_t _to_outputs; ///<mixed servo outputs topic + orb_advert_t _to_battery; ///<battery status / voltage topic - actuator_outputs_s _outputs; ///< mixed outputs - actuator_controls_effective_s _controls_effective; ///< effective controls + actuator_outputs_s _outputs; ///<mixed outputs + actuator_controls_effective_s _controls_effective; ///<effective controls - bool _primary_pwm_device; ///< true if we are the default PWM output + bool _primary_pwm_device; ///<true if we are the default PWM output - float _battery_amp_per_volt; - float _battery_amp_bias; - float _battery_mamphour_total; - uint64_t _battery_last_timestamp; + float _battery_amp_per_volt; ///<current sensor amps/volt + float _battery_amp_bias; ///<current sensor bias + float _battery_mamphour_total;///<amp hours consumed so far + uint64_t _battery_last_timestamp;///<last amp hour calculation timestamp - /** - * Relay1 is dedicated to controlling DSM receiver power - */ - - bool _dsm_vcc_ctl; + bool _dsm_vcc_ctl; ///<true if relay 1 controls DSM satellite RX power /** * Trampoline to the worker task @@ -570,9 +618,11 @@ void PX4IO::task_main() { hrt_abstime last_poll_time = 0; + int mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0); log("starting"); + /* * Subscribe to the appropriate PWM output topic based on whether we are the * primary PWM output or not. @@ -672,6 +722,25 @@ PX4IO::task_main() */ if (fds[3].revents & POLLIN) { parameter_update_s pupdate; + int32_t dsm_bind_val; + param_t dsm_bind_param; + + // See if bind parameter has been set, and reset it to 0 + param_get(dsm_bind_param = param_find("RC_DSM_BIND"), &dsm_bind_val); + if (dsm_bind_val) { + if (!(_status & PX4IO_P_STATUS_FLAGS_ARMED)) { + if ((dsm_bind_val == 1) || (dsm_bind_val == 2)) { + mavlink_log_info(mavlink_fd, "[IO] binding dsm%c rx", dsm_bind_val == 1 ? '2' : 'x'); + ioctl(nullptr, DSM_BIND_START, dsm_bind_val == 1 ? 3 : 7); + } else { + mavlink_log_info(mavlink_fd, "[IO] invalid bind type, bind request rejected"); + } + } else { + mavlink_log_info(mavlink_fd, "[IO] system armed, bind request rejected"); + } + dsm_bind_val = 0; + param_set(dsm_bind_param, &dsm_bind_val); + } /* copy to reset the notification */ orb_copy(ORB_ID(parameter_update), _t_param, &pupdate); @@ -1445,16 +1514,11 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down); usleep(500000); io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_set_rx_out); - usleep(1000); io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up); - usleep(100000); + usleep(50000); io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_send_pulses | (arg << 4)); - break; - - case DSM_BIND_STOP: - io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down); + usleep(50000); io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_reinit_uart); - usleep(500000); break; case DSM_BIND_POWER_UP: @@ -1603,7 +1667,8 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) } ssize_t -PX4IO::write(file *filp, const char *buffer, size_t len) +PX4IO::write(file * /*filp*/, const char *buffer, size_t len) +/* Make it obvious that file * isn't used here */ { unsigned count = len / 2; @@ -1696,30 +1761,12 @@ bind(int argc, char *argv[]) else errx(1, "unknown parameter %s, use dsm2 or dsmx", argv[2]); - /* Open console directly to grab CTRL-C signal */ - int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY); - if (!console) - errx(1, "failed opening console"); - warnx("This command will only bind DSM if satellite VCC (red wire) is controlled by relay 1."); - warnx("Press CTRL-C or 'c' when done."); g_dev->ioctl(nullptr, DSM_BIND_START, pulses); - for (;;) { - usleep(500000L); - /* Check if user wants to quit */ - char c; - if (read(console, &c, 1) == 1) { - if (c == 0x03 || c == 0x63) { - warnx("Done\n"); - g_dev->ioctl(nullptr, DSM_BIND_STOP, 0); - g_dev->ioctl(nullptr, DSM_BIND_POWER_UP, 0); - close(console); - exit(0); - } - } - } + exit(0); + } void diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c index 7ef3db970..83a1a1abb 100644 --- a/src/drivers/stm32/drv_hrt.c +++ b/src/drivers/stm32/drv_hrt.c @@ -70,7 +70,7 @@ #include "stm32_gpio.h" #include "stm32_tim.h" -#ifdef CONFIG_HRT_TIMER +#ifdef HRT_TIMER /* HRT configuration */ #if HRT_TIMER == 1 @@ -155,7 +155,7 @@ # error must not set CONFIG_STM32_TIM11=y and HRT_TIMER=11 # endif #else -# error HRT_TIMER must be set in board.h if CONFIG_HRT_TIMER=y +# error HRT_TIMER must be a value between 1 and 11 #endif /* @@ -275,7 +275,7 @@ static void hrt_call_invoke(void); /* * Specific registers and bits used by PPM sub-functions */ -#ifdef CONFIG_HRT_PPM +#ifdef HRT_PPM_CHANNEL /* * If the timer hardware doesn't support GTIM_CCER_CCxNP, then we will work around it. * @@ -326,7 +326,7 @@ static void hrt_call_invoke(void); # define CCER_PPM (GTIM_CCER_CC4E | GTIM_CCER_CC4P | GTIM_CCER_CC4NP) /* CC4, both edges */ # define CCER_PPM_FLIP GTIM_CCER_CC4P # else -# error HRT_PPM_CHANNEL must be a value between 1 and 4 if CONFIG_HRT_PPM is set +# error HRT_PPM_CHANNEL must be a value between 1 and 4 # endif /* @@ -377,7 +377,7 @@ static void hrt_ppm_decode(uint32_t status); # define CCMR1_PPM 0 # define CCMR2_PPM 0 # define CCER_PPM 0 -#endif /* CONFIG_HRT_PPM */ +#endif /* HRT_PPM_CHANNEL */ /* * Initialise the timer we are going to use. @@ -424,7 +424,7 @@ hrt_tim_init(void) up_enable_irq(HRT_TIMER_VECTOR); } -#ifdef CONFIG_HRT_PPM +#ifdef HRT_PPM_CHANNEL /* * Handle the PPM decoder state machine. */ @@ -526,7 +526,7 @@ error: ppm_decoded_channels = 0; } -#endif /* CONFIG_HRT_PPM */ +#endif /* HRT_PPM_CHANNEL */ /* * Handle the compare interupt by calling the callout dispatcher @@ -546,7 +546,7 @@ hrt_tim_isr(int irq, void *context) /* ack the interrupts we just read */ rSR = ~status; -#ifdef CONFIG_HRT_PPM +#ifdef HRT_PPM_CHANNEL /* was this a PPM edge? */ if (status & (SR_INT_PPM | SR_OVF_PPM)) { @@ -686,7 +686,7 @@ hrt_init(void) sq_init(&callout_queue); hrt_tim_init(); -#ifdef CONFIG_HRT_PPM +#ifdef HRT_PPM_CHANNEL /* configure the PPM input pin */ stm32_configgpio(GPIO_PPM_IN); #endif @@ -907,4 +907,4 @@ hrt_latency_update(void) } -#endif /* CONFIG_HRT_TIMER */ +#endif /* HRT_TIMER */ diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index 167ef30a8..2284be84d 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -117,10 +117,6 @@ #include <systemlib/err.h> -#ifndef CONFIG_HRT_TIMER -# error This driver requires CONFIG_HRT_TIMER -#endif - /* Tone alarm configuration */ #if TONE_ALARM_TIMER == 2 # define TONE_ALARM_BASE STM32_TIM2_BASE diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp index 49d0d157d..f01ee0355 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp @@ -47,8 +47,8 @@ #include <mathlib/mathlib.h> #include <controllib/blocks.hpp> #include <controllib/block/BlockParam.hpp> -#include <controllib/block/UOrbSubscription.hpp> -#include <controllib/block/UOrbPublication.hpp> +#include <controllib/uorb/UOrbSubscription.hpp> +#include <controllib/uorb/UOrbPublication.hpp> #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_global_position.h> diff --git a/src/modules/controllib/block/Block.cpp b/src/modules/controllib/block/Block.cpp index 5994d2315..b964d40a3 100644 --- a/src/modules/controllib/block/Block.cpp +++ b/src/modules/controllib/block/Block.cpp @@ -43,8 +43,8 @@ #include "Block.hpp" #include "BlockParam.hpp" -#include "UOrbSubscription.hpp" -#include "UOrbPublication.hpp" +#include "../uorb/UOrbSubscription.hpp" +#include "../uorb/UOrbPublication.hpp" namespace control { diff --git a/src/modules/controllib/blocks.hpp b/src/modules/controllib/blocks.hpp index 7a785d12e..fefe99702 100644 --- a/src/modules/controllib/blocks.hpp +++ b/src/modules/controllib/blocks.hpp @@ -42,6 +42,7 @@ #include <assert.h> #include <time.h> #include <stdlib.h> +#include <math.h> #include <mathlib/math/test/test.hpp> #include "block/Block.hpp" diff --git a/src/modules/controllib/module.mk b/src/modules/controllib/module.mk index 13d1069c7..d815a8feb 100644 --- a/src/modules/controllib/module.mk +++ b/src/modules/controllib/module.mk @@ -37,7 +37,7 @@ SRCS = test_params.c \ block/Block.cpp \ block/BlockParam.cpp \ - block/UOrbPublication.cpp \ - block/UOrbSubscription.cpp \ - blocks.cpp \ - fixedwing.cpp + uorb/UOrbPublication.cpp \ + uorb/UOrbSubscription.cpp \ + uorb/blocks.cpp \ + blocks.cpp diff --git a/src/modules/controllib/block/UOrbPublication.cpp b/src/modules/controllib/uorb/UOrbPublication.cpp index f69b39d90..f69b39d90 100644 --- a/src/modules/controllib/block/UOrbPublication.cpp +++ b/src/modules/controllib/uorb/UOrbPublication.cpp diff --git a/src/modules/controllib/block/UOrbPublication.hpp b/src/modules/controllib/uorb/UOrbPublication.hpp index 0a8ae2ff7..6f1f3fc1c 100644 --- a/src/modules/controllib/block/UOrbPublication.hpp +++ b/src/modules/controllib/uorb/UOrbPublication.hpp @@ -39,8 +39,8 @@ #pragma once #include <uORB/uORB.h> -#include "Block.hpp" -#include "List.hpp" +#include "../block/Block.hpp" +#include "../block/List.hpp" namespace control diff --git a/src/modules/controllib/block/UOrbSubscription.cpp b/src/modules/controllib/uorb/UOrbSubscription.cpp index 022cadd24..022cadd24 100644 --- a/src/modules/controllib/block/UOrbSubscription.cpp +++ b/src/modules/controllib/uorb/UOrbSubscription.cpp diff --git a/src/modules/controllib/block/UOrbSubscription.hpp b/src/modules/controllib/uorb/UOrbSubscription.hpp index 22cc2e114..d337d89a8 100644 --- a/src/modules/controllib/block/UOrbSubscription.hpp +++ b/src/modules/controllib/uorb/UOrbSubscription.hpp @@ -39,8 +39,8 @@ #pragma once #include <uORB/uORB.h> -#include "Block.hpp" -#include "List.hpp" +#include "../block/Block.hpp" +#include "../block/List.hpp" namespace control diff --git a/src/modules/controllib/uorb/blocks.cpp b/src/modules/controllib/uorb/blocks.cpp new file mode 100644 index 000000000..448a42a99 --- /dev/null +++ b/src/modules/controllib/uorb/blocks.cpp @@ -0,0 +1,101 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file uorb_blocks.cpp + * + * uorb block library code + */ + +#include "blocks.hpp" + +namespace control +{ + +BlockWaypointGuidance::BlockWaypointGuidance(SuperBlock *parent, const char *name) : + SuperBlock(parent, name), + _xtYawLimit(this, "XT2YAW"), + _xt2Yaw(this, "XT2YAW"), + _psiCmd(0) +{ +} + +BlockWaypointGuidance::~BlockWaypointGuidance() {}; + +void BlockWaypointGuidance::update(vehicle_global_position_s &pos, + vehicle_attitude_s &att, + vehicle_global_position_setpoint_s &posCmd, + vehicle_global_position_setpoint_s &lastPosCmd) +{ + + // heading to waypoint + float psiTrack = get_bearing_to_next_waypoint( + (double)pos.lat / (double)1e7d, + (double)pos.lon / (double)1e7d, + (double)posCmd.lat / (double)1e7d, + (double)posCmd.lon / (double)1e7d); + + // cross track + struct crosstrack_error_s xtrackError; + get_distance_to_line(&xtrackError, + (double)pos.lat / (double)1e7d, + (double)pos.lon / (double)1e7d, + (double)lastPosCmd.lat / (double)1e7d, + (double)lastPosCmd.lon / (double)1e7d, + (double)posCmd.lat / (double)1e7d, + (double)posCmd.lon / (double)1e7d); + + _psiCmd = _wrap_2pi(psiTrack - + _xtYawLimit.update(_xt2Yaw.update(xtrackError.distance))); +} + +BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name) : + SuperBlock(parent, name), + // subscriptions + _att(&getSubscriptions(), ORB_ID(vehicle_attitude), 20), + _attCmd(&getSubscriptions(), ORB_ID(vehicle_attitude_setpoint), 20), + _ratesCmd(&getSubscriptions(), ORB_ID(vehicle_rates_setpoint), 20), + _pos(&getSubscriptions() , ORB_ID(vehicle_global_position), 20), + _posCmd(&getSubscriptions(), ORB_ID(vehicle_global_position_set_triplet), 20), + _manual(&getSubscriptions(), ORB_ID(manual_control_setpoint), 20), + _status(&getSubscriptions(), ORB_ID(vehicle_status), 20), + _param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz + // publications + _actuators(&getPublications(), ORB_ID(actuator_controls_0)) +{ +} + +BlockUorbEnabledAutopilot::~BlockUorbEnabledAutopilot() {}; + +} // namespace control + diff --git a/src/modules/controllib/uorb/blocks.hpp b/src/modules/controllib/uorb/blocks.hpp new file mode 100644 index 000000000..9c0720aa5 --- /dev/null +++ b/src/modules/controllib/uorb/blocks.hpp @@ -0,0 +1,113 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file uorb_blocks.h + * + * uorb block library code + */ + +#pragma once + +#include <uORB/topics/vehicle_attitude_setpoint.h> +#include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/vehicle_rates_setpoint.h> +#include <uORB/topics/vehicle_global_position.h> +#include <uORB/topics/vehicle_global_position_set_triplet.h> +#include <uORB/topics/manual_control_setpoint.h> +#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/actuator_controls.h> +#include <uORB/topics/parameter_update.h> + +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <math.h> + +#include <drivers/drv_hrt.h> +#include <poll.h> + +extern "C" { +#include <systemlib/geo/geo.h> +} + +#include "../blocks.hpp" +#include "UOrbSubscription.hpp" +#include "UOrbPublication.hpp" + +namespace control +{ + +/** + * Waypoint Guidance block + */ +class __EXPORT BlockWaypointGuidance : public SuperBlock +{ +private: + BlockLimitSym _xtYawLimit; + BlockP _xt2Yaw; + float _psiCmd; +public: + BlockWaypointGuidance(SuperBlock *parent, const char *name); + virtual ~BlockWaypointGuidance(); + void update(vehicle_global_position_s &pos, + vehicle_attitude_s &att, + vehicle_global_position_setpoint_s &posCmd, + vehicle_global_position_setpoint_s &lastPosCmd); + float getPsiCmd() { return _psiCmd; } +}; + +/** + * UorbEnabledAutopilot + */ +class __EXPORT BlockUorbEnabledAutopilot : public SuperBlock +{ +protected: + // subscriptions + UOrbSubscription<vehicle_attitude_s> _att; + UOrbSubscription<vehicle_attitude_setpoint_s> _attCmd; + UOrbSubscription<vehicle_rates_setpoint_s> _ratesCmd; + UOrbSubscription<vehicle_global_position_s> _pos; + UOrbSubscription<vehicle_global_position_set_triplet_s> _posCmd; + UOrbSubscription<manual_control_setpoint_s> _manual; + UOrbSubscription<vehicle_status_s> _status; + UOrbSubscription<parameter_update_s> _param_update; + // publications + UOrbPublication<actuator_controls_s> _actuators; +public: + BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name); + virtual ~BlockUorbEnabledAutopilot(); +}; + +} // namespace control + diff --git a/src/modules/controllib/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp index 77b2ac806..f655a13bd 100644 --- a/src/modules/controllib/fixedwing.cpp +++ b/src/modules/fixedwing_backside/fixedwing.cpp @@ -86,61 +86,6 @@ void BlockStabilization::update(float pCmd, float qCmd, float rCmd, _yawDamper.update(rCmd, r, outputScale); } -BlockWaypointGuidance::BlockWaypointGuidance(SuperBlock *parent, const char *name) : - SuperBlock(parent, name), - _xtYawLimit(this, "XT2YAW"), - _xt2Yaw(this, "XT2YAW"), - _psiCmd(0) -{ -} - -BlockWaypointGuidance::~BlockWaypointGuidance() {}; - -void BlockWaypointGuidance::update(vehicle_global_position_s &pos, - vehicle_attitude_s &att, - vehicle_global_position_setpoint_s &posCmd, - vehicle_global_position_setpoint_s &lastPosCmd) -{ - - // heading to waypoint - float psiTrack = get_bearing_to_next_waypoint( - (double)pos.lat / (double)1e7d, - (double)pos.lon / (double)1e7d, - (double)posCmd.lat / (double)1e7d, - (double)posCmd.lon / (double)1e7d); - - // cross track - struct crosstrack_error_s xtrackError; - get_distance_to_line(&xtrackError, - (double)pos.lat / (double)1e7d, - (double)pos.lon / (double)1e7d, - (double)lastPosCmd.lat / (double)1e7d, - (double)lastPosCmd.lon / (double)1e7d, - (double)posCmd.lat / (double)1e7d, - (double)posCmd.lon / (double)1e7d); - - _psiCmd = _wrap_2pi(psiTrack - - _xtYawLimit.update(_xt2Yaw.update(xtrackError.distance))); -} - -BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name) : - SuperBlock(parent, name), - // subscriptions - _att(&getSubscriptions(), ORB_ID(vehicle_attitude), 20), - _attCmd(&getSubscriptions(), ORB_ID(vehicle_attitude_setpoint), 20), - _ratesCmd(&getSubscriptions(), ORB_ID(vehicle_rates_setpoint), 20), - _pos(&getSubscriptions() , ORB_ID(vehicle_global_position), 20), - _posCmd(&getSubscriptions(), ORB_ID(vehicle_global_position_set_triplet), 20), - _manual(&getSubscriptions(), ORB_ID(manual_control_setpoint), 20), - _status(&getSubscriptions(), ORB_ID(vehicle_status), 20), - _param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz - // publications - _actuators(&getPublications(), ORB_ID(actuator_controls_0)) -{ -} - -BlockUorbEnabledAutopilot::~BlockUorbEnabledAutopilot() {}; - BlockMultiModeBacksideAutopilot::BlockMultiModeBacksideAutopilot(SuperBlock *parent, const char *name) : BlockUorbEnabledAutopilot(parent, name), _stabilization(this, ""), // no name needed, already unique diff --git a/src/modules/controllib/fixedwing.hpp b/src/modules/fixedwing_backside/fixedwing.hpp index e4028c40d..3876e4630 100644 --- a/src/modules/controllib/fixedwing.hpp +++ b/src/modules/fixedwing_backside/fixedwing.hpp @@ -39,31 +39,8 @@ #pragma once -#include <uORB/topics/vehicle_attitude_setpoint.h> -#include <uORB/topics/vehicle_attitude.h> -#include <uORB/topics/vehicle_rates_setpoint.h> -#include <uORB/topics/vehicle_global_position.h> -#include <uORB/topics/vehicle_global_position_set_triplet.h> -#include <uORB/topics/manual_control_setpoint.h> -#include <uORB/topics/vehicle_status.h> -#include <uORB/topics/actuator_controls.h> -#include <uORB/topics/parameter_update.h> - -#include <stdio.h> -#include <stdlib.h> -#include <string.h> -#include <math.h> - -#include <drivers/drv_hrt.h> -#include <poll.h> - -#include "blocks.hpp" -#include "block/UOrbSubscription.hpp" -#include "block/UOrbPublication.hpp" - -extern "C" { -#include <systemlib/geo/geo.h> -} +#include <controllib/blocks.hpp> +#include <controllib/uorb/blocks.hpp> namespace control { @@ -251,47 +228,6 @@ public: */ /** - * Waypoint Guidance block - */ -class __EXPORT BlockWaypointGuidance : public SuperBlock -{ -private: - BlockLimitSym _xtYawLimit; - BlockP _xt2Yaw; - float _psiCmd; -public: - BlockWaypointGuidance(SuperBlock *parent, const char *name); - virtual ~BlockWaypointGuidance(); - void update(vehicle_global_position_s &pos, - vehicle_attitude_s &att, - vehicle_global_position_setpoint_s &posCmd, - vehicle_global_position_setpoint_s &lastPosCmd); - float getPsiCmd() { return _psiCmd; } -}; - -/** - * UorbEnabledAutopilot - */ -class __EXPORT BlockUorbEnabledAutopilot : public SuperBlock -{ -protected: - // subscriptions - UOrbSubscription<vehicle_attitude_s> _att; - UOrbSubscription<vehicle_attitude_setpoint_s> _attCmd; - UOrbSubscription<vehicle_rates_setpoint_s> _ratesCmd; - UOrbSubscription<vehicle_global_position_s> _pos; - UOrbSubscription<vehicle_global_position_set_triplet_s> _posCmd; - UOrbSubscription<manual_control_setpoint_s> _manual; - UOrbSubscription<vehicle_status_s> _status; - UOrbSubscription<parameter_update_s> _param_update; - // publications - UOrbPublication<actuator_controls_s> _actuators; -public: - BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name); - virtual ~BlockUorbEnabledAutopilot(); -}; - -/** * Multi-mode Autopilot */ class __EXPORT BlockMultiModeBacksideAutopilot : public BlockUorbEnabledAutopilot diff --git a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp index 4803a526e..f4ea05088 100644 --- a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp +++ b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp @@ -45,12 +45,13 @@ #include <stdlib.h> #include <string.h> #include <systemlib/systemlib.h> -#include <controllib/fixedwing.hpp> #include <systemlib/param/param.h> #include <systemlib/err.h> #include <drivers/drv_hrt.h> #include <math.h> +#include "fixedwing.hpp" + static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int deamon_task; /**< Handle of deamon task / thread */ diff --git a/src/modules/fixedwing_backside/module.mk b/src/modules/fixedwing_backside/module.mk index ec958d7cb..133728781 100644 --- a/src/modules/fixedwing_backside/module.mk +++ b/src/modules/fixedwing_backside/module.mk @@ -38,4 +38,5 @@ MODULE_COMMAND = fixedwing_backside SRCS = fixedwing_backside_main.cpp \ + fixedwing.cpp \ params.c diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 43d96fb06..fbd82a4c6 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -300,6 +300,8 @@ controls_tick() { } else { r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE; } + } else { + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE; } } diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index ab6e3fec4..745cdfa40 100644 --- a/src/modules/px4iofirmware/dsm.c +++ b/src/modules/px4iofirmware/dsm.c @@ -48,156 +48,44 @@ #include <drivers/drv_hrt.h> -#define DEBUG - #include "px4io.h" -#define DSM_FRAME_SIZE 16 -#define DSM_FRAME_CHANNELS 7 - -static int dsm_fd = -1; - -static hrt_abstime last_rx_time; -static hrt_abstime last_frame_time; - -static uint8_t frame[DSM_FRAME_SIZE]; - -static unsigned partial_frame_count; -static unsigned channel_shift; - -unsigned dsm_frame_drops; - -static bool dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value); -static void dsm_guess_format(bool reset); -static bool dsm_decode(hrt_abstime now, uint16_t *values, uint16_t *num_values); - -int -dsm_init(const char *device) -{ - if (dsm_fd < 0) - dsm_fd = open(device, O_RDONLY | O_NONBLOCK); - - if (dsm_fd >= 0) { - struct termios t; - - /* 115200bps, no parity, one stop bit */ - tcgetattr(dsm_fd, &t); - cfsetspeed(&t, 115200); - t.c_cflag &= ~(CSTOPB | PARENB); - tcsetattr(dsm_fd, TCSANOW, &t); - - /* initialise the decoder */ - partial_frame_count = 0; - last_rx_time = hrt_absolute_time(); - - /* reset the format detector */ - dsm_guess_format(true); - - debug("DSM: ready"); - - } else { - debug("DSM: open failed"); - } +#define DSM_FRAME_SIZE 16 /**<DSM frame size in bytes*/ +#define DSM_FRAME_CHANNELS 7 /**<Max supported DSM channels*/ - return dsm_fd; -} - -void -dsm_bind(uint16_t cmd, int pulses) -{ - const uint32_t usart1RxAsOutp = GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN10; - - if (dsm_fd < 0) - return; - - switch (cmd) { - case dsm_bind_power_down: - // power down DSM satellite - POWER_RELAY1(0); - break; - case dsm_bind_power_up: - POWER_RELAY1(1); - dsm_guess_format(true); - break; - case dsm_bind_set_rx_out: - stm32_configgpio(usart1RxAsOutp); - break; - case dsm_bind_send_pulses: - for (int i = 0; i < pulses; i++) { - stm32_gpiowrite(usart1RxAsOutp, false); - up_udelay(50); - stm32_gpiowrite(usart1RxAsOutp, true); - up_udelay(50); - } - break; - case dsm_bind_reinit_uart: - // Restore USART rx pin - stm32_configgpio(GPIO_USART1_RX); - break; - } -} - -bool -dsm_input(uint16_t *values, uint16_t *num_values) -{ - ssize_t ret; - hrt_abstime now; - - /* - * The DSM* protocol doesn't provide any explicit framing, - * so we detect frame boundaries by the inter-frame delay. - * - * The minimum frame spacing is 11ms; with 16 bytes at 115200bps - * frame transmission time is ~1.4ms. - * - * We expect to only be called when bytes arrive for processing, - * and if an interval of more than 5ms passes between calls, - * the first byte we read will be the first byte of a frame. - * - * In the case where byte(s) are dropped from a frame, this also - * provides a degree of protection. Of course, it would be better - * if we didn't drop bytes... - */ - now = hrt_absolute_time(); - - if ((now - last_rx_time) > 5000) { - if (partial_frame_count > 0) { - dsm_frame_drops++; - partial_frame_count = 0; - } - } - - /* - * Fetch bytes, but no more than we would need to complete - * the current frame. - */ - ret = read(dsm_fd, &frame[partial_frame_count], DSM_FRAME_SIZE - partial_frame_count); - - /* if the read failed for any reason, just give up here */ - if (ret < 1) - return false; - - last_rx_time = now; - - /* - * Add bytes to the current frame - */ - partial_frame_count += ret; - - /* - * If we don't have a full frame, return - */ - if (partial_frame_count < DSM_FRAME_SIZE) - return false; - - /* - * Great, it looks like we might have a frame. Go ahead and - * decode it. - */ - partial_frame_count = 0; - return dsm_decode(now, values, num_values); -} +static int dsm_fd = -1; /**<File handle to the DSM UART*/ +static hrt_abstime dsm_last_rx_time; /**<Timestamp when we last received*/ +static hrt_abstime dsm_last_frame_time; /**<Timestamp for start of last dsm frame*/ +static uint8_t dsm_frame[DSM_FRAME_SIZE]; /**<DSM dsm frame receive buffer*/ +static unsigned dsm_partial_frame_count; /**<Count of bytes received for current dsm frame*/ +static unsigned dsm_channel_shift; /**<Channel resolution, 0=unknown, 1=10 bit, 2=11 bit*/ +static unsigned dsm_frame_drops; /**<Count of incomplete DSM frames*/ +/** + * Attempt to decode a single channel raw channel datum + * + * The DSM* protocol doesn't provide any explicit framing, + * so we detect dsm frame boundaries by the inter-dsm frame delay. + * + * The minimum dsm frame spacing is 11ms; with 16 bytes at 115200bps + * dsm frame transmission time is ~1.4ms. + * + * We expect to only be called when bytes arrive for processing, + * and if an interval of more than 5ms passes between calls, + * the first byte we read will be the first byte of a dsm frame. + * + * In the case where byte(s) are dropped from a dsm frame, this also + * provides a degree of protection. Of course, it would be better + * if we didn't drop bytes... + * + * Upon receiving a full dsm frame we attempt to decode it + * + * @param[in] raw 16 bit raw channel value from dsm frame + * @param[in] shift position of channel number in raw data + * @param[out] channel pointer to returned channel number + * @param[out] value pointer to returned channel value + * @return true=raw value successfully decoded + */ static bool dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value) { @@ -215,6 +103,11 @@ dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *va return true; } +/** + * Attempt to guess if receiving 10 or 11 bit channel values + * + * @param[in] reset true=reset the 10/11 bit state to unknown + */ static void dsm_guess_format(bool reset) { @@ -227,14 +120,14 @@ dsm_guess_format(bool reset) cs10 = 0; cs11 = 0; samples = 0; - channel_shift = 0; + dsm_channel_shift = 0; return; } - /* scan the channels in the current frame in both 10- and 11-bit mode */ + /* scan the channels in the current dsm_frame in both 10- and 11-bit mode */ for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) { - uint8_t *dp = &frame[2 + (2 * i)]; + uint8_t *dp = &dsm_frame[2 + (2 * i)]; uint16_t raw = (dp[0] << 8) | dp[1]; unsigned channel, value; @@ -245,10 +138,10 @@ dsm_guess_format(bool reset) if (dsm_decode_channel(raw, 11, &channel, &value) && (channel < 31)) cs11 |= (1 << channel); - /* XXX if we cared, we could look for the phase bit here to decide 1 vs. 2-frame format */ + /* XXX if we cared, we could look for the phase bit here to decide 1 vs. 2-dsm_frame format */ } - /* wait until we have seen plenty of frames - 2 should normally be enough */ + /* wait until we have seen plenty of frames - 5 should normally be enough */ if (samples++ < 5) return; @@ -284,13 +177,13 @@ dsm_guess_format(bool reset) } if ((votes11 == 1) && (votes10 == 0)) { - channel_shift = 11; + dsm_channel_shift = 11; debug("DSM: 11-bit format"); return; } if ((votes10 == 1) && (votes11 == 0)) { - channel_shift = 10; + dsm_channel_shift = 10; debug("DSM: 10-bit format"); return; } @@ -300,27 +193,131 @@ dsm_guess_format(bool reset) dsm_guess_format(true); } +/** + * Initialize the DSM receive functionality + * + * Open the UART for receiving DSM frames and configure it appropriately + * + * @param[in] device Device name of DSM UART + */ +int +dsm_init(const char *device) +{ + if (dsm_fd < 0) + dsm_fd = open(device, O_RDONLY | O_NONBLOCK); + + if (dsm_fd >= 0) { + + struct termios t; + + /* 115200bps, no parity, one stop bit */ + tcgetattr(dsm_fd, &t); + cfsetspeed(&t, 115200); + t.c_cflag &= ~(CSTOPB | PARENB); + tcsetattr(dsm_fd, TCSANOW, &t); + + /* initialise the decoder */ + dsm_partial_frame_count = 0; + dsm_last_rx_time = hrt_absolute_time(); + + /* reset the format detector */ + dsm_guess_format(true); + + debug("DSM: ready"); + + } else { + + debug("DSM: open failed"); + + } + + return dsm_fd; +} + +/** + * Handle DSM satellite receiver bind mode handler + * + * @param[in] cmd commands - dsm_bind_power_down, dsm_bind_power_up, dsm_bind_set_rx_out, dsm_bind_send_pulses, dsm_bind_reinit_uart + * @param[in] pulses Number of pulses for dsm_bind_send_pulses command + */ +void +dsm_bind(uint16_t cmd, int pulses) +{ + const uint32_t usart1RxAsOutp = + GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | GPIO_OUTPUT_SET | GPIO_PORTA | GPIO_PIN10; + + if (dsm_fd < 0) + return; + + switch (cmd) { + + case dsm_bind_power_down: + + /*power down DSM satellite*/ + POWER_RELAY1(0); + break; + + case dsm_bind_power_up: + + /*power up DSM satellite*/ + POWER_RELAY1(1); + dsm_guess_format(true); + break; + + case dsm_bind_set_rx_out: + + /*Set UART RX pin to active output mode*/ + stm32_configgpio(usart1RxAsOutp); + break; + + case dsm_bind_send_pulses: + + /*Pulse RX pin a number of times*/ + for (int i = 0; i < pulses; i++) { + stm32_gpiowrite(usart1RxAsOutp, false); + up_udelay(25); + stm32_gpiowrite(usart1RxAsOutp, true); + up_udelay(25); + } + break; + + case dsm_bind_reinit_uart: + + /*Restore USART RX pin to RS232 receive mode*/ + stm32_configgpio(GPIO_USART1_RX); + break; + + } +} + +/** + * Decode the entire dsm frame (all contained channels) + * + * @param[in] frame_time timestamp when this dsm frame was received. Used to detect RX loss in order to reset 10/11 bit guess. + * @param[out] values pointer to per channel array of decoded values + * @param[out] num_values pointer to number of raw channel values returned + * @return true=DSM frame successfully decoded, false=no update + */ static bool dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) { - /* - debug("DSM frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x", - frame[0], frame[1], frame[2], frame[3], frame[4], frame[5], frame[6], frame[7], - frame[8], frame[9], frame[10], frame[11], frame[12], frame[13], frame[14], frame[15]); + debug("DSM dsm_frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x", + dsm_frame[0], dsm_frame[1], dsm_frame[2], dsm_frame[3], dsm_frame[4], dsm_frame[5], dsm_frame[6], dsm_frame[7], + dsm_frame[8], dsm_frame[9], dsm_frame[10], dsm_frame[11], dsm_frame[12], dsm_frame[13], dsm_frame[14], dsm_frame[15]); */ /* * If we have lost signal for at least a second, reset the * format guessing heuristic. */ - if (((frame_time - last_frame_time) > 1000000) && (channel_shift != 0)) + if (((frame_time - dsm_last_frame_time) > 1000000) && (dsm_channel_shift != 0)) dsm_guess_format(true); - /* we have received something we think is a frame */ - last_frame_time = frame_time; + /* we have received something we think is a dsm_frame */ + dsm_last_frame_time = frame_time; - /* if we don't know the frame format, update the guessing state machine */ - if (channel_shift == 0) { + /* if we don't know the dsm_frame format, update the guessing state machine */ + if (dsm_channel_shift == 0) { dsm_guess_format(false); return false; } @@ -332,17 +329,17 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) * Each channel is a 16-bit unsigned value containing either a 10- * or 11-bit channel value and a 4-bit channel number, shifted * either 10 or 11 bits. The MSB may also be set to indicate the - * second frame in variants of the protocol where more than + * second dsm_frame in variants of the protocol where more than * seven channels are being transmitted. */ for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) { - uint8_t *dp = &frame[2 + (2 * i)]; + uint8_t *dp = &dsm_frame[2 + (2 * i)]; uint16_t raw = (dp[0] << 8) | dp[1]; unsigned channel, value; - if (!dsm_decode_channel(raw, channel_shift, &channel, &value)) + if (!dsm_decode_channel(raw, dsm_channel_shift, &channel, &value)) continue; /* ignore channels out of range */ @@ -354,7 +351,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) *num_values = channel + 1; /* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */ - if (channel_shift == 11) + if (dsm_channel_shift == 11) value /= 2; value += 998; @@ -385,7 +382,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) values[channel] = value; } - if (channel_shift == 11) + if (dsm_channel_shift == 11) *num_values |= 0x8000; /* @@ -393,3 +390,70 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) */ return true; } + +/** + * Called periodically to check for input data from the DSM UART + * + * The DSM* protocol doesn't provide any explicit framing, + * so we detect dsm frame boundaries by the inter-dsm frame delay. + * The minimum dsm frame spacing is 11ms; with 16 bytes at 115200bps + * dsm frame transmission time is ~1.4ms. + * We expect to only be called when bytes arrive for processing, + * and if an interval of more than 5ms passes between calls, + * the first byte we read will be the first byte of a dsm frame. + * In the case where byte(s) are dropped from a dsm frame, this also + * provides a degree of protection. Of course, it would be better + * if we didn't drop bytes... + * Upon receiving a full dsm frame we attempt to decode it. + * + * @param[out] values pointer to per channel array of decoded values + * @param[out] num_values pointer to number of raw channel values returned + * @return true=decoded raw channel values updated, false=no update + */ +bool +dsm_input(uint16_t *values, uint16_t *num_values) +{ + ssize_t ret; + hrt_abstime now; + + /* + */ + now = hrt_absolute_time(); + + if ((now - dsm_last_rx_time) > 5000) { + if (dsm_partial_frame_count > 0) { + dsm_frame_drops++; + dsm_partial_frame_count = 0; + } + } + + /* + * Fetch bytes, but no more than we would need to complete + * the current dsm frame. + */ + ret = read(dsm_fd, &dsm_frame[dsm_partial_frame_count], DSM_FRAME_SIZE - dsm_partial_frame_count); + + /* if the read failed for any reason, just give up here */ + if (ret < 1) + return false; + + dsm_last_rx_time = now; + + /* + * Add bytes to the current dsm frame + */ + dsm_partial_frame_count += ret; + + /* + * If we don't have a full dsm frame, return + */ + if (dsm_partial_frame_count < DSM_FRAME_SIZE) + return false; + + /* + * Great, it looks like we might have a dsm frame. Go ahead and + * decode it. + */ + dsm_partial_frame_count = 0; + return dsm_decode(now, values, num_values); +} diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 83feeb9b6..57cffcc23 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -184,7 +184,7 @@ extern void controls_init(void); extern void controls_tick(void); extern int dsm_init(const char *device); extern bool dsm_input(uint16_t *values, uint16_t *num_values); -extern void dsm_bind(uint16_t cmd, int pulses); +extern void dsm_bind(uint16_t cmd, int pulses); extern int sbus_init(const char *device); extern bool sbus_input(uint16_t *values, uint16_t *num_values); @@ -192,9 +192,9 @@ extern bool sbus_input(uint16_t *values, uint16_t *num_values); extern volatile uint8_t debug_level; /* send a debug message to the console */ -extern void isr_debug(uint8_t level, const char *fmt, ...); +extern void isr_debug(uint8_t level, const char *fmt, ...); #ifdef CONFIG_STM32_I2C1 -void i2c_dump(void); -void i2c_reset(void); +void i2c_dump(void); +void i2c_reset(void); #endif diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 3713e0b30..ba7cdd91c 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -604,6 +604,15 @@ int sdlog2_thread_main(int argc, char *argv[]) errx(1, "unable to create logging folder, exiting."); } + const char *converter_in = "/etc/logging/conv.zip"; + char* converter_out = malloc(150); + sprintf(converter_out, "%s/conv.zip", folder_path); + + if (file_copy(converter_in, converter_out)) { + errx(1, "unable to copy conversion scripts, exiting."); + } + free(converter_out); + /* only print logging path, important to find log file later */ warnx("logging to directory: %s", folder_path); @@ -1256,7 +1265,7 @@ int file_copy(const char *file_old, const char *file_new) fclose(source); fclose(target); - return ret; + return OK; } void handle_command(struct vehicle_command_s *cmd) diff --git a/src/modules/segway/BlockSegwayController.cpp b/src/modules/segway/BlockSegwayController.cpp new file mode 100644 index 000000000..b1dc39445 --- /dev/null +++ b/src/modules/segway/BlockSegwayController.cpp @@ -0,0 +1,57 @@ +#include "BlockSegwayController.hpp" + +void BlockSegwayController::update() { + // wait for a sensor update, check for exit condition every 100 ms + if (poll(&_attPoll, 1, 100) < 0) return; // poll error + + uint64_t newTimeStamp = hrt_absolute_time(); + float dt = (newTimeStamp - _timeStamp) / 1.0e6f; + _timeStamp = newTimeStamp; + + // check for sane values of dt + // to prevent large control responses + if (dt > 1.0f || dt < 0) return; + + // set dt for all child blocks + setDt(dt); + + // check for new updates + if (_param_update.updated()) updateParams(); + + // get new information from subscriptions + updateSubscriptions(); + + // default all output to zero unless handled by mode + for (unsigned i = 2; i < NUM_ACTUATOR_CONTROLS; i++) + _actuators.control[i] = 0.0f; + + // only update guidance in auto mode + if (_status.state_machine == SYSTEM_STATE_AUTO) { + // update guidance + } + + // compute speed command + float spdCmd = -th2v.update(_att.pitch) - q2v.update(_att.pitchspeed); + + // handle autopilot modes + if (_status.state_machine == SYSTEM_STATE_AUTO || + _status.state_machine == SYSTEM_STATE_STABILIZED) { + _actuators.control[0] = spdCmd; + _actuators.control[1] = spdCmd; + + } else if (_status.state_machine == SYSTEM_STATE_MANUAL) { + if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { + _actuators.control[CH_LEFT] = _manual.throttle; + _actuators.control[CH_RIGHT] = _manual.pitch; + + } else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) { + _actuators.control[0] = spdCmd; + _actuators.control[1] = spdCmd; + } + } + + // update all publications + updatePublications(); + +} + diff --git a/src/modules/segway/BlockSegwayController.hpp b/src/modules/segway/BlockSegwayController.hpp new file mode 100644 index 000000000..4a01f785c --- /dev/null +++ b/src/modules/segway/BlockSegwayController.hpp @@ -0,0 +1,27 @@ +#pragma once + +#include <controllib/uorb/blocks.hpp> + +using namespace control; + +class BlockSegwayController : public control::BlockUorbEnabledAutopilot { +public: + BlockSegwayController() : + BlockUorbEnabledAutopilot(NULL,"SEG"), + th2v(this, "TH2V"), + q2v(this, "Q2V"), + _attPoll(), + _timeStamp(0) + { + _attPoll.fd = _att.getHandle(); + _attPoll.events = POLLIN; + } + void update(); +private: + enum {CH_LEFT, CH_RIGHT}; + BlockPI th2v; + BlockP q2v; + struct pollfd _attPoll; + uint64_t _timeStamp; +}; + diff --git a/src/modules/segway/module.mk b/src/modules/segway/module.mk new file mode 100644 index 000000000..d5da85601 --- /dev/null +++ b/src/modules/segway/module.mk @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# segway controller +# + +MODULE_COMMAND = segway + +SRCS = segway_main.cpp \ + BlockSegwayController.cpp \ + params.c diff --git a/src/modules/segway/params.c b/src/modules/segway/params.c new file mode 100644 index 000000000..d72923717 --- /dev/null +++ b/src/modules/segway/params.c @@ -0,0 +1,8 @@ +#include <systemlib/param/param.h> + +// 16 is max name length +PARAM_DEFINE_FLOAT(SEG_TH2V_P, 10.0f); // pitch to voltage +PARAM_DEFINE_FLOAT(SEG_TH2V_I, 0.0f); // pitch integral to voltage +PARAM_DEFINE_FLOAT(SEG_TH2V_I_MAX, 0.0f); // integral limiter +PARAM_DEFINE_FLOAT(SEG_Q2V, 1.0f); // pitch rate to voltage + diff --git a/src/modules/segway/segway_main.cpp b/src/modules/segway/segway_main.cpp new file mode 100644 index 000000000..061fbf9b9 --- /dev/null +++ b/src/modules/segway/segway_main.cpp @@ -0,0 +1,157 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Author: James Goppert + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file segway_main.cpp + * @author James Goppert + * + * Segway controller using control library + */ + +#include <nuttx/config.h> +#include <unistd.h> +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <systemlib/systemlib.h> +#include <systemlib/param/param.h> +#include <systemlib/err.h> +#include <drivers/drv_hrt.h> +#include <math.h> + +#include "BlockSegwayController.hpp" + +static bool thread_should_exit = false; /**< Deamon exit flag */ +static bool thread_running = false; /**< Deamon status flag */ +static int deamon_task; /**< Handle of deamon task / thread */ + +/** + * Deamon management function. + */ +extern "C" __EXPORT int segway_main(int argc, char *argv[]); + +/** + * Mainloop of deamon. + */ +int segway_thread_main(int argc, char *argv[]); + +/** + * Print the correct usage. + */ +static void usage(const char *reason); + +static void +usage(const char *reason) +{ + if (reason) + fprintf(stderr, "%s\n", reason); + + fprintf(stderr, "usage: segway {start|stop|status} [-p <additional params>]\n\n"); + exit(1); +} + +/** + * The deamon app only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_create(). + */ +int segway_main(int argc, char *argv[]) +{ + + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + warnx("already running"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + + deamon_task = task_spawn_cmd("segway", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 10, + 5120, + segway_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + thread_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + warnx("is running"); + + } else { + warnx("not started"); + } + + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + +int segway_thread_main(int argc, char *argv[]) +{ + + warnx("starting"); + + using namespace control; + + BlockSegwayController autopilot; + + thread_running = true; + + while (!thread_should_exit) { + autopilot.update(); + } + + warnx("exiting."); + + thread_running = false; + + return 0; +} diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 133cda8d6..bd431c9eb 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -156,6 +156,7 @@ PARAM_DEFINE_FLOAT(RC14_DZ, 0.0f); PARAM_DEFINE_INT32(RC_TYPE, 1); /** 1 = FUTABA, 2 = Spektrum, 3 = Graupner HoTT, 4 = Turnigy 9x */ PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */ +PARAM_DEFINE_INT32(RC_DSM_BIND, 0); /* 0 = Idle, 1 = Start DSM2 bind, 2 = Start DSMX bind */ /* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */ PARAM_DEFINE_FLOAT(BAT_V_SCALING, (3.3f * 52.0f / 5.0f / 4095.0f)); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index f7b41b120..42268b971 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -257,8 +257,6 @@ private: float battery_voltage_scaling; - int rc_rl1_DSM_VCC_control; - } _parameters; /**< local copies of interesting parameters */ struct { @@ -308,8 +306,6 @@ private: param_t battery_voltage_scaling; - param_t rc_rl1_DSM_VCC_control; - } _parameter_handles; /**< handles for interesting parameters */ @@ -544,9 +540,6 @@ Sensors::Sensors() : _parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING"); - /* DSM VCC relay control */ - _parameter_handles.rc_rl1_DSM_VCC_control = param_find("RC_RL1_DSM_VCC"); - /* fetch initial parameter values */ parameters_update(); } @@ -738,11 +731,6 @@ Sensors::parameters_update() warnx("Failed updating voltage scaling param"); } - /* relay 1 DSM VCC control */ - if (param_get(_parameter_handles.rc_rl1_DSM_VCC_control, &(_parameters.rc_rl1_DSM_VCC_control)) != OK) { - warnx("Failed updating relay 1 DSM VCC control"); - } - return OK; } diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index ae5fc6c61..301cfa255 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -37,6 +37,10 @@ * Common object definitions without a better home. */ +/** + * @defgroup topics List of all uORB topics. + */ + #include <nuttx/config.h> #include <drivers/drv_orb_dev.h> diff --git a/src/modules/uORB/topics/actuator_controls.h b/src/modules/uORB/topics/actuator_controls.h index b7c4196c0..a27095be5 100644 --- a/src/modules/uORB/topics/actuator_controls.h +++ b/src/modules/uORB/topics/actuator_controls.h @@ -52,11 +52,20 @@ #define NUM_ACTUATOR_CONTROLS 8 #define NUM_ACTUATOR_CONTROL_GROUPS 4 /**< for sanity checking */ +/** + * @addtogroup topics + * @{ + */ + struct actuator_controls_s { uint64_t timestamp; float control[NUM_ACTUATOR_CONTROLS]; }; +/** + * @} + */ + /* actuator control sets; this list can be expanded as more controllers emerge */ ORB_DECLARE(actuator_controls_0); ORB_DECLARE(actuator_controls_1); diff --git a/src/modules/uORB/topics/actuator_controls_effective.h b/src/modules/uORB/topics/actuator_controls_effective.h index 088c4fc8f..d7b404ad4 100644 --- a/src/modules/uORB/topics/actuator_controls_effective.h +++ b/src/modules/uORB/topics/actuator_controls_effective.h @@ -53,11 +53,20 @@ #define NUM_ACTUATOR_CONTROLS_EFFECTIVE NUM_ACTUATOR_CONTROLS #define NUM_ACTUATOR_CONTROL_GROUPS_EFFECTIVE NUM_ACTUATOR_CONTROL_GROUPS /**< for sanity checking */ +/** + * @addtogroup topics + * @{ + */ + struct actuator_controls_effective_s { uint64_t timestamp; float control_effective[NUM_ACTUATOR_CONTROLS_EFFECTIVE]; }; +/** + * @} + */ + /* actuator control sets; this list can be expanded as more controllers emerge */ ORB_DECLARE(actuator_controls_effective_0); ORB_DECLARE(actuator_controls_effective_1); diff --git a/src/modules/uORB/topics/actuator_outputs.h b/src/modules/uORB/topics/actuator_outputs.h index bbe429073..30895ca83 100644 --- a/src/modules/uORB/topics/actuator_outputs.h +++ b/src/modules/uORB/topics/actuator_outputs.h @@ -52,12 +52,21 @@ #define NUM_ACTUATOR_OUTPUTS 16 #define NUM_ACTUATOR_OUTPUT_GROUPS 4 /**< for sanity checking */ +/** + * @addtogroup topics + * @{ + */ + struct actuator_outputs_s { uint64_t timestamp; /**< output timestamp in us since system boot */ float output[NUM_ACTUATOR_OUTPUTS]; /**< output data, in natural output units */ int noutputs; /**< valid outputs */ }; +/** + * @} + */ + /* actuator output sets; this list can be expanded as more drivers emerge */ ORB_DECLARE(actuator_outputs_0); ORB_DECLARE(actuator_outputs_1); diff --git a/src/modules/uORB/topics/debug_key_value.h b/src/modules/uORB/topics/debug_key_value.h index a9d1b83fd..9253c787d 100644 --- a/src/modules/uORB/topics/debug_key_value.h +++ b/src/modules/uORB/topics/debug_key_value.h @@ -47,6 +47,7 @@ /** * @addtogroup topics + * @{ */ /** diff --git a/src/modules/uORB/topics/esc_status.h b/src/modules/uORB/topics/esc_status.h index 00cf59b28..11332d7a7 100644 --- a/src/modules/uORB/topics/esc_status.h +++ b/src/modules/uORB/topics/esc_status.h @@ -52,10 +52,6 @@ #include "../uORB.h" /** - * @addtogroup topics @{ - */ - -/** * The number of ESCs supported. * Current (Q2/2013) we support 8 ESCs, */ @@ -76,7 +72,12 @@ enum ESC_CONNECTION_TYPE { }; /** - * + * @addtogroup topics + * @{ + */ + +/** + * Electronic speed controller status. */ struct esc_status_s { diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index 253f444b3..978a3383a 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -46,11 +46,6 @@ #include <stdbool.h> #include "../uORB.h" -/** - * @addtogroup topics - * @{ - */ - enum NAV_CMD { NAV_CMD_WAYPOINT = 0, NAV_CMD_LOITER_TURN_COUNT, @@ -62,6 +57,11 @@ enum NAV_CMD { }; /** + * @addtogroup topics + * @{ + */ + +/** * Global position setpoint in WGS84 coordinates. * * This is the position the MAV is heading towards. If it of type loiter, diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h index 2e895c59c..7901b930a 100644 --- a/src/modules/uORB/topics/offboard_control_setpoint.h +++ b/src/modules/uORB/topics/offboard_control_setpoint.h @@ -44,11 +44,6 @@ #include "../uORB.h" /** - * @addtogroup topics - * @{ - */ - -/** * Off-board control inputs. * * Typically sent by a ground control station / joystick or by @@ -66,6 +61,11 @@ enum OFFBOARD_CONTROL_MODE OFFBOARD_CONTROL_MODE_MULTIROTOR_SIMPLE = 7, /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */ }; +/** + * @addtogroup topics + * @{ + */ + struct offboard_control_setpoint_s { uint64_t timestamp; diff --git a/src/modules/uORB/topics/omnidirectional_flow.h b/src/modules/uORB/topics/omnidirectional_flow.h index 8f4be3b3f..a6ad8a131 100644 --- a/src/modules/uORB/topics/omnidirectional_flow.h +++ b/src/modules/uORB/topics/omnidirectional_flow.h @@ -46,6 +46,7 @@ /** * @addtogroup topics + * @{ */ /** diff --git a/src/modules/uORB/topics/parameter_update.h b/src/modules/uORB/topics/parameter_update.h index 300e895c7..68964deb0 100644 --- a/src/modules/uORB/topics/parameter_update.h +++ b/src/modules/uORB/topics/parameter_update.h @@ -42,11 +42,20 @@ #include <stdint.h> #include "../uORB.h" +/** + * @addtogroup topics + * @{ + */ + struct parameter_update_s { /** time at which the latest parameter was updated */ uint64_t timestamp; }; +/** + * @} + */ + ORB_DECLARE(parameter_update); #endif
\ No newline at end of file diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index 9dd54df91..e69335b3d 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -46,11 +46,6 @@ #include "../uORB.h" /** - * @addtogroup topics - * @{ - */ - -/** * The number of RC channel inputs supported. * Current (Q1/2013) radios support up to 18 channels, * leaving at a sane value of 14. @@ -83,6 +78,11 @@ enum RC_CHANNELS_FUNCTION RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */ }; +/** + * @addtogroup topics + * @{ + */ + struct rc_channels_s { uint64_t timestamp; /**< In microseconds since boot time. */ diff --git a/src/modules/uORB/topics/sensor_combined.h b/src/modules/uORB/topics/sensor_combined.h index 9a76b5182..ad164555e 100644 --- a/src/modules/uORB/topics/sensor_combined.h +++ b/src/modules/uORB/topics/sensor_combined.h @@ -46,11 +46,6 @@ #include <stdbool.h> #include "../uORB.h" -/** - * @addtogroup topics - * @{ - */ - enum MAGNETOMETER_MODE { MAGNETOMETER_MODE_NORMAL = 0, MAGNETOMETER_MODE_POSITIVE_BIAS, @@ -58,6 +53,11 @@ enum MAGNETOMETER_MODE { }; /** + * @addtogroup topics + * @{ + */ + +/** * Sensor readings in raw and SI-unit form. * * These values are read from the sensors. Raw values are in sensor-specific units, diff --git a/src/modules/uORB/topics/subsystem_info.h b/src/modules/uORB/topics/subsystem_info.h index c415e832e..cfe0bf69e 100644 --- a/src/modules/uORB/topics/subsystem_info.h +++ b/src/modules/uORB/topics/subsystem_info.h @@ -50,10 +50,6 @@ #include <stdbool.h> #include "../uORB.h" -/** - * @addtogroup topics - */ - enum SUBSYSTEM_TYPE { SUBSYSTEM_TYPE_GYRO = 1, @@ -76,6 +72,10 @@ enum SUBSYSTEM_TYPE }; /** + * @addtogroup topics + */ + +/** * State of individual sub systems */ struct subsystem_info_s { diff --git a/src/modules/uORB/topics/telemetry_status.h b/src/modules/uORB/topics/telemetry_status.h index f30852de5..828fb31cc 100644 --- a/src/modules/uORB/topics/telemetry_status.h +++ b/src/modules/uORB/topics/telemetry_status.h @@ -50,6 +50,11 @@ enum TELEMETRY_STATUS_RADIO_TYPE { TELEMETRY_STATUS_RADIO_TYPE_WIRE }; +/** + * @addtogroup topics + * @{ + */ + struct telemetry_status_s { uint64_t timestamp; enum TELEMETRY_STATUS_RADIO_TYPE type; /**< type of the radio hardware */ @@ -62,6 +67,10 @@ struct telemetry_status_s { uint8_t txbuf; /**< how full the tx buffer is as a percentage */ }; +/** + * @} + */ + ORB_DECLARE(telemetry_status); #endif /* TOPIC_TELEMETRY_STATUS_H */
\ No newline at end of file diff --git a/src/modules/uORB/topics/vehicle_attitude.h b/src/modules/uORB/topics/vehicle_attitude.h index c31c81d0c..4380a5ee7 100755 --- a/src/modules/uORB/topics/vehicle_attitude.h +++ b/src/modules/uORB/topics/vehicle_attitude.h @@ -48,6 +48,7 @@ /** * @addtogroup topics + * @{ */ /** diff --git a/src/modules/uORB/topics/vehicle_command.h b/src/modules/uORB/topics/vehicle_command.h index fac571659..31ff014de 100644 --- a/src/modules/uORB/topics/vehicle_command.h +++ b/src/modules/uORB/topics/vehicle_command.h @@ -46,11 +46,6 @@ #include "../uORB.h" /** - * @addtogroup topics - * @{ - */ - -/** * Commands for commander app. * * Should contain all commands from MAVLink's VEHICLE_CMD ENUM, @@ -110,6 +105,10 @@ enum VEHICLE_CMD_RESULT VEHICLE_CMD_RESULT_ENUM_END=5, /* | */ }; +/** + * @addtogroup topics + * @{ + */ struct vehicle_command_s { diff --git a/src/modules/uORB/topics/vehicle_global_position_set_triplet.h b/src/modules/uORB/topics/vehicle_global_position_set_triplet.h index 318abba89..8516b263f 100644 --- a/src/modules/uORB/topics/vehicle_global_position_set_triplet.h +++ b/src/modules/uORB/topics/vehicle_global_position_set_triplet.h @@ -60,8 +60,8 @@ */ struct vehicle_global_position_set_triplet_s { - bool previous_valid; - bool next_valid; + bool previous_valid; /**< flag indicating previous position is valid */ + bool next_valid; /**< flag indicating next position is valid */ struct vehicle_global_position_setpoint_s previous; struct vehicle_global_position_setpoint_s current; diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index c7c1048f6..94068a9ac 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -54,10 +54,6 @@ #include <stdbool.h> #include "../uORB.h" -/** - * @addtogroup topics @{ - */ - /* State Machine */ typedef enum { @@ -137,6 +133,10 @@ enum VEHICLE_BATTERY_WARNING { VEHICLE_BATTERY_WARNING_ALERT /**< aleting of low voltage 2. stage */ }; +/** + * @addtogroup topics + * @{ + */ /** * state machine / state of vehicle. diff --git a/src/systemcmds/tests/test_hrt.c b/src/systemcmds/tests/test_hrt.c index f21dd115b..f6e540401 100644 --- a/src/systemcmds/tests/test_hrt.c +++ b/src/systemcmds/tests/test_hrt.c @@ -94,7 +94,7 @@ extern uint16_t ppm_pulse_history[]; int test_ppm(int argc, char *argv[]) { -#ifdef CONFIG_HRT_PPM +#ifdef HRT_PPM_CHANNEL unsigned i; printf("channels: %u\n", ppm_decoded_channels); |