diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-08-18 12:04:07 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-08-18 12:04:07 +0200 |
commit | e597f979824074e971aef19814951d87c8b6a8b5 (patch) | |
tree | 3b4c767b37c2799a75074d287d66ba31918e32e7 /src/modules | |
parent | eda528157a04185cbb1342c152c4ac715f67771c (diff) | |
parent | 061be7f7fed430d1c235809f1e1dce61e8b7aa01 (diff) | |
download | px4-firmware-e597f979824074e971aef19814951d87c8b6a8b5.tar.gz px4-firmware-e597f979824074e971aef19814951d87c8b6a8b5.tar.bz2 px4-firmware-e597f979824074e971aef19814951d87c8b6a8b5.zip |
Merged master
Diffstat (limited to 'src/modules')
22 files changed, 478 insertions, 225 deletions
diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index 4925ced54..206e27db5 100644 --- a/src/modules/px4iofirmware/dsm.c +++ b/src/modules/px4iofirmware/dsm.c @@ -48,161 +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"); - } - - 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; - -#ifdef CONFIG_ARCH_BOARD_PX4IO_V2 - // XXX implement - #warning DSM BIND NOT IMPLEMENTED ON PX4IO V2 -#else - 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(25); - stm32_gpiowrite(usart1RxAsOutp, true); - up_udelay(25); - } - break; - case dsm_bind_reinit_uart: - // Restore USART rx pin - stm32_configgpio(GPIO_USART1_RX); - break; - } -#endif -} - -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; - } - } +#define DSM_FRAME_SIZE 16 /**<DSM frame size in bytes*/ +#define DSM_FRAME_CHANNELS 7 /**<Max supported DSM channels*/ - /* - * 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) { @@ -220,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) { @@ -232,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; @@ -250,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; @@ -289,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; } @@ -305,27 +193,136 @@ 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; + +#ifdef CONFIG_ARCH_BOARD_PX4IO_V2 + // XXX implement + #warning DSM BIND NOT IMPLEMENTED ON PX4IO V2 +#else + 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; + + } +#endif +} + +/** + * 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; } @@ -337,17 +334,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 */ @@ -359,7 +356,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; @@ -390,7 +387,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; /* @@ -398,3 +395,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 d6cf22b21..dea67043e 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -198,7 +198,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); diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index a5fc34184..d19a7aec3 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -70,6 +70,9 @@ PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f); PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0); +PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0); +PARAM_DEFINE_INT32(SENS_EXT_MAG_ROT, 0); + PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f); PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f); PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 22374a1fe..a6204c9fa 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -50,6 +50,7 @@ #include <stdio.h> #include <errno.h> #include <math.h> +#include <mathlib/mathlib.h> #include <nuttx/analog/adc.h> @@ -138,6 +139,77 @@ #define limit_minus_one_to_one(arg) (arg < -1.0f) ? -1.0f : ((arg > 1.0f) ? 1.0f : arg) /** + * Enum for board and external compass rotations. + * This enum maps from board attitude to airframe attitude. + */ +enum Rotation { + ROTATION_NONE = 0, + ROTATION_YAW_45 = 1, + ROTATION_YAW_90 = 2, + ROTATION_YAW_135 = 3, + ROTATION_YAW_180 = 4, + ROTATION_YAW_225 = 5, + ROTATION_YAW_270 = 6, + ROTATION_YAW_315 = 7, + ROTATION_ROLL_180 = 8, + ROTATION_ROLL_180_YAW_45 = 9, + ROTATION_ROLL_180_YAW_90 = 10, + ROTATION_ROLL_180_YAW_135 = 11, + ROTATION_PITCH_180 = 12, + ROTATION_ROLL_180_YAW_225 = 13, + ROTATION_ROLL_180_YAW_270 = 14, + ROTATION_ROLL_180_YAW_315 = 15, + ROTATION_ROLL_90 = 16, + ROTATION_ROLL_90_YAW_45 = 17, + ROTATION_ROLL_90_YAW_90 = 18, + ROTATION_ROLL_90_YAW_135 = 19, + ROTATION_ROLL_270 = 20, + ROTATION_ROLL_270_YAW_45 = 21, + ROTATION_ROLL_270_YAW_90 = 22, + ROTATION_ROLL_270_YAW_135 = 23, + ROTATION_PITCH_90 = 24, + ROTATION_PITCH_270 = 25, + ROTATION_MAX +}; + +typedef struct +{ + uint16_t roll; + uint16_t pitch; + uint16_t yaw; +} rot_lookup_t; + +const rot_lookup_t rot_lookup[] = +{ + { 0, 0, 0 }, + { 0, 0, 45 }, + { 0, 0, 90 }, + { 0, 0, 135 }, + { 0, 0, 180 }, + { 0, 0, 225 }, + { 0, 0, 270 }, + { 0, 0, 315 }, + {180, 0, 0 }, + {180, 0, 45 }, + {180, 0, 90 }, + {180, 0, 135 }, + { 0, 180, 0 }, + {180, 0, 225 }, + {180, 0, 270 }, + {180, 0, 315 }, + { 90, 0, 0 }, + { 90, 0, 45 }, + { 90, 0, 90 }, + { 90, 0, 135 }, + {270, 0, 0 }, + {270, 0, 45 }, + {270, 0, 90 }, + {270, 0, 135 }, + { 0, 90, 0 }, + { 0, 270, 0 } +}; + +/** * Sensor app start / stop handling function * * @ingroup apps @@ -189,8 +261,8 @@ private: int _mag_sub; /**< raw mag data subscription */ int _rc_sub; /**< raw rc channels data subscription */ int _baro_sub; /**< raw baro data subscription */ - int _airspeed_sub; /**< airspeed subscription */ - int _diff_pres_sub; /**< raw differential pressure subscription */ + int _airspeed_sub; /**< airspeed subscription */ + int _diff_pres_sub; /**< raw differential pressure subscription */ int _vcontrol_mode_sub; /**< vehicle control mode subscription */ int _params_sub; /**< notification of parameter updates */ int _manual_control_sub; /**< notification of manual control updates */ @@ -210,13 +282,15 @@ private: struct differential_pressure_s _diff_pres; struct airspeed_s _airspeed; + math::Matrix _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ + math::Matrix _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */ + struct { float min[_rc_max_chan_count]; float trim[_rc_max_chan_count]; float max[_rc_max_chan_count]; float rev[_rc_max_chan_count]; float dz[_rc_max_chan_count]; - // float ex[_rc_max_chan_count]; float scaling_factor[_rc_max_chan_count]; float gyro_offset[3]; @@ -227,6 +301,9 @@ private: float accel_scale[3]; float diff_pres_offset_pa; + int board_rotation; + int external_mag_rotation; + int rc_type; int rc_map_roll; @@ -264,7 +341,6 @@ private: param_t max[_rc_max_chan_count]; param_t rev[_rc_max_chan_count]; param_t dz[_rc_max_chan_count]; - // param_t ex[_rc_max_chan_count]; param_t rc_type; param_t rc_demix; @@ -304,6 +380,9 @@ private: param_t battery_voltage_scaling; + param_t board_rotation; + param_t external_mag_rotation; + } _parameter_handles; /**< handles for interesting parameters */ @@ -313,6 +392,11 @@ private: int parameters_update(); /** + * Get the rotation matrices + */ + void get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix); + + /** * Do accel-related initialisation. */ void accel_init(); @@ -448,7 +532,10 @@ Sensors::Sensors() : _diff_pres_pub(-1), /* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")) + _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")), + + _board_rotation(3,3), + _external_mag_rotation(3,3) { /* basic r/c parameters */ @@ -538,6 +625,10 @@ Sensors::Sensors() : _parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING"); + /* rotations */ + _parameter_handles.board_rotation = param_find("SENS_BOARD_ROT"); + _parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT"); + /* fetch initial parameter values */ parameters_update(); } @@ -724,10 +815,34 @@ Sensors::parameters_update() warnx("Failed updating voltage scaling param"); } + param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation)); + param_get(_parameter_handles.external_mag_rotation, &(_parameters.external_mag_rotation)); + + get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation); + get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation); + return OK; } void +Sensors::get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix) +{ + /* first set to zero */ + rot_matrix->Matrix::zero(3,3); + + float roll = M_DEG_TO_RAD_F * (float)rot_lookup[rot].roll; + float pitch = M_DEG_TO_RAD_F * (float)rot_lookup[rot].pitch; + float yaw = M_DEG_TO_RAD_F * (float)rot_lookup[rot].yaw; + + math::EulerAngles euler(roll, pitch, yaw); + + math::Dcm R(euler); + + for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) + (*rot_matrix)(i,j) = R(i, j); +} + +void Sensors::accel_init() { int fd; @@ -750,7 +865,7 @@ Sensors::accel_init() /* set the driver to poll at 1000Hz */ ioctl(fd, SENSORIOCSPOLLRATE, 1000); - #else + #elif CONFIG_ARCH_BOARD_PX4FMU_V2 /* set the accel internal sampling rate up to at leat 800Hz */ ioctl(fd, ACCELIOCSSAMPLERATE, 800); @@ -758,6 +873,9 @@ Sensors::accel_init() /* set the driver to poll at 800Hz */ ioctl(fd, SENSORIOCSPOLLRATE, 800); + #else + #error Need a board configuration, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2 + #endif warnx("using system accel"); @@ -867,9 +985,12 @@ Sensors::accel_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report); - raw.accelerometer_m_s2[0] = accel_report.x; - raw.accelerometer_m_s2[1] = accel_report.y; - raw.accelerometer_m_s2[2] = accel_report.z; + math::Vector3 vect = {accel_report.x, accel_report.y, accel_report.z}; + vect = _board_rotation*vect; + + raw.accelerometer_m_s2[0] = vect(0); + raw.accelerometer_m_s2[1] = vect(1); + raw.accelerometer_m_s2[2] = vect(2); raw.accelerometer_raw[0] = accel_report.x_raw; raw.accelerometer_raw[1] = accel_report.y_raw; @@ -890,9 +1011,12 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report); - raw.gyro_rad_s[0] = gyro_report.x; - raw.gyro_rad_s[1] = gyro_report.y; - raw.gyro_rad_s[2] = gyro_report.z; + math::Vector3 vect = {gyro_report.x, gyro_report.y, gyro_report.z}; + vect = _board_rotation*vect; + + raw.gyro_rad_s[0] = vect(0); + raw.gyro_rad_s[1] = vect(1); + raw.gyro_rad_s[2] = vect(2); raw.gyro_raw[0] = gyro_report.x_raw; raw.gyro_raw[1] = gyro_report.y_raw; @@ -913,9 +1037,14 @@ Sensors::mag_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report); - raw.magnetometer_ga[0] = mag_report.x; - raw.magnetometer_ga[1] = mag_report.y; - raw.magnetometer_ga[2] = mag_report.z; + // XXX TODO add support for external mag orientation + + math::Vector3 vect = {mag_report.x, mag_report.y, mag_report.z}; + vect = _board_rotation*vect; + + raw.magnetometer_ga[0] = vect(0); + raw.magnetometer_ga[1] = vect(1); + raw.magnetometer_ga[2] = vect(2); raw.magnetometer_raw[0] = mag_report.x_raw; raw.magnetometer_raw[1] = mag_report.y_raw; diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index ed77bb7ef..f35d1fe9b 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 26b967237..ace77438c 100644 --- a/src/modules/uORB/topics/actuator_controls.h +++ b/src/modules/uORB/topics/actuator_controls.h @@ -55,15 +55,25 @@ /* control sets with pre-defined applications */ #define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0) + +/** + * @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); ORB_DECLARE(actuator_controls_2); ORB_DECLARE(actuator_controls_3); -#endif
\ No newline at end of file +#endif 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 2167e44a2..5a8580143 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 15. @@ -85,6 +80,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 4d49316c3..a7eb8e6bf 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -159,6 +159,10 @@ enum VEHICLE_BATTERY_WARNING { VEHICLE_BATTERY_WARNING_ALERT /**< alerting of low voltage 2. stage */ }; +/** + * @addtogroup topics + * @{ + */ /** * state machine / state of vehicle. |