From e7f2c053c241849e3ea035fcd22a0e29945b3415 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 5 Nov 2012 16:04:45 +0100 Subject: Quickly separated low-level raw RC from mapped / scaled RC, supports FMU PPM and IO PPM / Spektrum now --- apps/drivers/drv_rc_input.h | 22 ++++- apps/drivers/px4io/px4io.cpp | 20 +++- apps/drivers/stm32/drv_hrt.c | 2 +- apps/mavlink/mavlink_receiver.c | 4 - apps/mavlink/orb_listener.c | 35 +++++-- apps/mavlink/orb_topics.h | 2 + apps/sensors/sensors.cpp | 198 +++++++++++++++++++++++----------------- apps/uORB/topics/rc_channels.h | 14 +-- 8 files changed, 180 insertions(+), 117 deletions(-) (limited to 'apps') diff --git a/apps/drivers/drv_rc_input.h b/apps/drivers/drv_rc_input.h index 532e95fb5..927406108 100644 --- a/apps/drivers/drv_rc_input.h +++ b/apps/drivers/drv_rc_input.h @@ -57,15 +57,23 @@ #define RC_INPUT_DEVICE_PATH "/dev/input_rc" /** - * Maximum number of R/C input channels in the system. + * Maximum number of R/C input channels in the system. S.Bus has up to 18 channels. */ -#define RC_INPUT_MAX_CHANNELS 16 +#define RC_INPUT_MAX_CHANNELS 18 /** * Input signal type, value is a control position from zero to 100 * percent. */ -typedef uint8_t rc_input_t; +typedef uint16_t rc_input_t; + +enum RC_INPUT_SOURCE { + RC_INPUT_SOURCE_UNKNOWN = 0, + RC_INPUT_SOURCE_PX4FMU_PPM, + RC_INPUT_SOURCE_PX4IO_PPM, + RC_INPUT_SOURCE_PX4IO_SPEKTRUM, + RC_INPUT_SOURCE_PX4IO_SBUS +}; /** * R/C input status structure. @@ -74,10 +82,16 @@ typedef uint8_t rc_input_t; * on the board involved. */ struct rc_input_values { + /** decoding time */ + uint64_t timestamp; + /** number of channels actually being seen */ uint32_t channel_count; - /** desired pulse widths for each of the supported channels */ + /** Input source */ + enum RC_INPUT_SOURCE input_source; + + /** measured pulse widths for each of the supported channels */ rc_input_t values[RC_INPUT_MAX_CHANNELS]; }; diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 9e2025231..4687df2aa 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -61,9 +61,10 @@ #include #include #include -#include +#include #include +#include #include #include #include @@ -105,6 +106,9 @@ private: int _t_armed; ///< system armed control topic actuator_armed_s _armed; ///< system armed state + orb_advert_t _to_input_rc; ///< rc inputs from io + rc_input_values _input_rc; ///< rc input values + orb_advert_t _t_outputs; ///< mixed outputs topic actuator_outputs_s _outputs; ///< mixed outputs @@ -317,9 +321,14 @@ PX4IO::task_main() orb_set_interval(_t_armed, 200); /* 5Hz update rate */ /* advertise the mixed control outputs */ + memset(&_outputs, 0, sizeof(_outputs)); _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), &_outputs); + /* advertise the rc inputs */ + memset(&_input_rc, 0, sizeof(_input_rc)); + _to_input_rc = orb_advertise(ORB_ID(input_rc), &_input_rc); + /* poll descriptor */ pollfd fds[3]; fds[0].fd = _serial_fd; @@ -456,7 +465,14 @@ PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received) } _connected = true; - /* XXX handle R/C inputs here ... needs code sharing/library */ + /* publish raw rc channel values from IO */ + _input_rc.timestamp = hrt_absolute_time(); + for (int i = 0; i < rep->channel_count; i++) + { + _input_rc.values[i] = rep->rc_channel[i]; + } + + orb_publish(ORB_ID(input_rc), _to_input_rc, &_input_rc); /* remember the latched arming switch state */ _switch_armed = rep->armed; diff --git a/apps/drivers/stm32/drv_hrt.c b/apps/drivers/stm32/drv_hrt.c index 6ac46092b..cd9cb45b1 100644 --- a/apps/drivers/stm32/drv_hrt.c +++ b/apps/drivers/stm32/drv_hrt.c @@ -338,7 +338,7 @@ static void hrt_call_invoke(void); /* decoded PPM buffer */ #define PPM_MAX_CHANNELS 12 __EXPORT uint16_t ppm_buffer[PPM_MAX_CHANNELS]; -__EXPORT unsigned ppm_decoded_channels; +__EXPORT unsigned ppm_decoded_channels = 0; __EXPORT uint64_t ppm_last_valid_decode = 0; /* PPM edge history */ diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c index 0dff743bc..39e574c04 100644 --- a/apps/mavlink/mavlink_receiver.c +++ b/apps/mavlink/mavlink_receiver.c @@ -397,10 +397,6 @@ handle_message(mavlink_message_t *msg) rc_hil.timestamp = hrt_absolute_time(); rc_hil.chan_count = 4; - rc_hil.chan[0].raw = 1500 + man.x / 2; - rc_hil.chan[1].raw = 1500 + man.y / 2; - rc_hil.chan[2].raw = 1500 + man.r / 2; - rc_hil.chan[3].raw = 1500 + man.z / 2; rc_hil.chan[0].scaled = man.x / 1000.0f; rc_hil.chan[1].scaled = man.y / 1000.0f; diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index 4b66ee438..683964a0d 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -67,6 +67,7 @@ struct vehicle_global_position_s global_pos; struct vehicle_local_position_s local_pos; struct vehicle_status_s v_status; struct rc_channels_s rc; +struct rc_input_values rc_raw; struct actuator_armed_s armed; struct mavlink_subscriptions mavlink_subs; @@ -99,6 +100,7 @@ static void l_vehicle_attitude(struct listener *l); static void l_vehicle_gps_position(struct listener *l); static void l_vehicle_status(struct listener *l); static void l_rc_channels(struct listener *l); +static void l_input_rc(struct listener *l); static void l_global_position(struct listener *l); static void l_local_position(struct listener *l); static void l_global_position_setpoint(struct listener *l); @@ -116,6 +118,7 @@ struct listener listeners[] = { {l_vehicle_gps_position, &mavlink_subs.gps_sub, 0}, {l_vehicle_status, &status_sub, 0}, {l_rc_channels, &rc_sub, 0}, + {l_input_rc, &mavlink_subs.input_rc_sub, 0}, {l_global_position, &mavlink_subs.global_pos_sub, 0}, {l_local_position, &mavlink_subs.local_pos_sub, 0}, {l_global_position_setpoint, &mavlink_subs.spg_sub, 0}, @@ -274,21 +277,29 @@ l_rc_channels(struct listener *l) { /* copy rc channels into local buffer */ orb_copy(ORB_ID(rc_channels), rc_sub, &rc); + // XXX Add RC channels scaled message here +} + +void +l_input_rc(struct listener *l) +{ + /* copy rc channels into local buffer */ + orb_copy(ORB_ID(input_rc), mavlink_subs.input_rc_sub, &rc_raw); if (gcs_link) /* Channels are sent in MAVLink main loop at a fixed interval */ mavlink_msg_rc_channels_raw_send(chan, - rc.timestamp / 1000, + rc_raw.timestamp / 1000, 0, - rc.chan[0].raw, - rc.chan[1].raw, - rc.chan[2].raw, - rc.chan[3].raw, - rc.chan[4].raw, - rc.chan[5].raw, - rc.chan[6].raw, - rc.chan[7].raw, - rc.rssi); + (rc_raw.channel_count > 0) ? rc_raw.values[0] : UINT16_MAX, + (rc_raw.channel_count > 1) ? rc_raw.values[1] : UINT16_MAX, + (rc_raw.channel_count > 2) ? rc_raw.values[2] : UINT16_MAX, + (rc_raw.channel_count > 3) ? rc_raw.values[3] : UINT16_MAX, + (rc_raw.channel_count > 4) ? rc_raw.values[4] : UINT16_MAX, + (rc_raw.channel_count > 5) ? rc_raw.values[5] : UINT16_MAX, + (rc_raw.channel_count > 6) ? rc_raw.values[6] : UINT16_MAX, + (rc_raw.channel_count > 7) ? rc_raw.values[7] : UINT16_MAX, + 255); } void @@ -584,6 +595,10 @@ uorb_receive_start(void) rc_sub = orb_subscribe(ORB_ID(rc_channels)); orb_set_interval(rc_sub, 100); /* 10Hz updates */ + /* --- RC RAW VALUE --- */ + mavlink_subs.input_rc_sub = orb_subscribe(ORB_ID(input_rc)); + orb_set_interval(mavlink_subs.input_rc_sub, 100); + /* --- GLOBAL POS VALUE --- */ mavlink_subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); orb_set_interval(mavlink_subs.global_pos_sub, 1000); /* 1Hz active updates */ diff --git a/apps/mavlink/orb_topics.h b/apps/mavlink/orb_topics.h index f041e7c4c..6860702d2 100644 --- a/apps/mavlink/orb_topics.h +++ b/apps/mavlink/orb_topics.h @@ -57,6 +57,7 @@ #include #include #include +#include struct mavlink_subscriptions { int sensor_sub; @@ -75,6 +76,7 @@ struct mavlink_subscriptions { int spl_sub; int spg_sub; int debug_key_value; + int input_rc_sub; }; extern struct mavlink_subscriptions mavlink_subs; diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index 453502b05..3e9f35eaf 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -58,11 +58,13 @@ #include #include #include +#include #include #include #include #include + #include #include @@ -143,6 +145,7 @@ private: int _gyro_sub; /**< raw gyro data subscription */ int _accel_sub; /**< raw accel data subscription */ int _mag_sub; /**< raw mag data subscription */ + int _rc_sub; /**< raw rc channels data subscription */ int _baro_sub; /**< raw baro data subscription */ int _vstatus_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ @@ -162,6 +165,7 @@ private: 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]; float mag_offset[3]; @@ -331,6 +335,7 @@ Sensors::Sensors() : _gyro_sub(-1), _accel_sub(-1), _mag_sub(-1), + _rc_sub(-1), _baro_sub(-1), _vstatus_sub(-1), _params_sub(-1), @@ -464,14 +469,13 @@ Sensors::parameters_update() warnx("Failed getting exponential gain for chan %d", i); } - _rc.chan[i].scaling_factor = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]); + _parameters.scaling_factor[i] = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]); /* handle blowup in the scaling factor calculation */ - if (isnan(_rc.chan[i].scaling_factor) || isinf(_rc.chan[i].scaling_factor)) { - _rc.chan[i].scaling_factor = 0; + if (isnan(_parameters.scaling_factor[i]) || isinf(_parameters.scaling_factor[i])) { + _parameters.scaling_factor[i] = 0; } - _rc.chan[i].mid = _parameters.trim[i]; } /* update RC function mappings */ @@ -856,98 +860,125 @@ Sensors::adc_poll(struct sensor_combined_s &raw) void Sensors::ppm_poll() { - struct manual_control_setpoint_s manual_control; - - /* check to see whether a new frame has been decoded */ - if (_ppm_last_valid == ppm_last_valid_decode) - return; - /* require at least two chanels to consider the signal valid */ - if (ppm_decoded_channels < 4) - return; - - unsigned channel_limit = ppm_decoded_channels; - if (channel_limit > _rc_max_chan_count) - channel_limit = _rc_max_chan_count; - - /* we are accepting this decode */ - _ppm_last_valid = ppm_last_valid_decode; - - /* Read out values from HRT */ - for (unsigned int i = 0; i < channel_limit; i++) { - _rc.chan[i].raw = ppm_buffer[i]; - - /* scale around the mid point differently for lower and upper range */ - if (ppm_buffer[i] > (_parameters.trim[i] + _parameters.dz[i])) { - _rc.chan[i].scaled = (ppm_buffer[i] - _parameters.trim[i]) / (float)(_parameters.max[i] - _parameters.trim[i]); - } else if (ppm_buffer[i] < (_parameters.trim[i] - _parameters.dz[i])) { - /* division by zero impossible for trim == min (as for throttle), as this falls in the above if clause */ - _rc.chan[i].scaled = -((_parameters.trim[i] - ppm_buffer[i]) / (float)(_parameters.trim[i] - _parameters.min[i])); - - } else { - /* in the configured dead zone, output zero */ - _rc.chan[i].scaled = 0.0f; + /* fake low-level driver, directly pulling from driver variables */ + static orb_advert_t rc_input_pub = -1; + struct rc_input_values raw; + + raw.timestamp = ppm_last_valid_decode; + + if (ppm_decoded_channels > 1) { + + for (int i = 0; i < ppm_decoded_channels; i++) { + raw.values[i] = ppm_buffer[i]; } - /* reverse channel if required */ - if (i == _rc.function[THROTTLE]) { - if ((int)_parameters.rev[i] == -1) { - _rc.chan[i].scaled = 1.0f + -1.0f * _rc.chan[i].scaled; - } + raw.channel_count = ppm_decoded_channels; + + /* publish to object request broker */ + if (rc_input_pub <= 0) { + rc_input_pub = orb_advertise(ORB_ID(input_rc), &raw); } else { - _rc.chan[i].scaled *= _parameters.rev[i]; + orb_publish(ORB_ID(input_rc), rc_input_pub, &raw); } + } - /* handle any parameter-induced blowups */ - if (isnan(_rc.chan[i].scaled) || isinf(_rc.chan[i].scaled)) - _rc.chan[i].scaled = 0.0f; - //_rc.chan[i].scaled = (ppm_buffer[i] - _rc.chan[i].mid) * _rc.chan[i].scaling_factor; - } + /* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */ + bool rc_updated; + orb_check(_rc_sub, &rc_updated); - _rc.chan_count = ppm_decoded_channels; - _rc.timestamp = ppm_last_valid_decode; + if (rc_updated) { + struct rc_input_values rc_input; - manual_control.timestamp = ppm_last_valid_decode; + orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); - /* roll input - rolling right is stick-wise and rotation-wise positive */ - manual_control.roll = _rc.chan[_rc.function[ROLL]].scaled; - if (manual_control.roll < -1.0f) manual_control.roll = -1.0f; - if (manual_control.roll > 1.0f) manual_control.roll = 1.0f; - if (!isnan(_parameters.rc_scale_roll) || !isinf(_parameters.rc_scale_roll)) { - manual_control.roll *= _parameters.rc_scale_roll; - } + struct manual_control_setpoint_s manual_control; - /* - * pitch input - stick down is negative, but stick down is pitching up (pos) in NED, - * so reverse sign. - */ - manual_control.pitch = -1.0f * _rc.chan[_rc.function[PITCH]].scaled; - if (manual_control.pitch < -1.0f) manual_control.pitch = -1.0f; - if (manual_control.pitch > 1.0f) manual_control.pitch = 1.0f; - if (!isnan(_parameters.rc_scale_pitch) || !isinf(_parameters.rc_scale_pitch)) { - manual_control.pitch *= _parameters.rc_scale_pitch; - } + /* require at least two chanels to consider the signal valid */ + if (rc_input.channel_count < 2) + return; - /* yaw input - stick right is positive and positive rotation */ - manual_control.yaw = _rc.chan[_rc.function[YAW]].scaled * _parameters.rc_scale_yaw; - if (manual_control.yaw < -1.0f) manual_control.yaw = -1.0f; - if (manual_control.yaw > 1.0f) manual_control.yaw = 1.0f; - if (!isnan(_parameters.rc_scale_yaw) || !isinf(_parameters.rc_scale_yaw)) { - manual_control.yaw *= _parameters.rc_scale_yaw; - } - - /* throttle input */ - manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled; - if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f; - if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f; + unsigned channel_limit = rc_input.channel_count; + if (channel_limit > _rc_max_chan_count) + channel_limit = _rc_max_chan_count; - /* mode switch input */ - manual_control.override_mode_switch = _rc.chan[_rc.function[OVERRIDE]].scaled; - if (manual_control.override_mode_switch < -1.0f) manual_control.override_mode_switch = -1.0f; - if (manual_control.override_mode_switch > 1.0f) manual_control.override_mode_switch = 1.0f; + /* we are accepting this message */ + _ppm_last_valid = rc_input.timestamp; - orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc); - orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control); + /* Read out values from raw message */ + for (unsigned int i = 0; i < channel_limit; i++) { + + /* scale around the mid point differently for lower and upper range */ + if (rc_input.values[i] > (_parameters.trim[i] + _parameters.dz[i])) { + _rc.chan[i].scaled = (rc_input.values[i] - _parameters.trim[i]) / (float)(_parameters.max[i] - _parameters.trim[i]); + } else if (rc_input.values[i] < (_parameters.trim[i] - _parameters.dz[i])) { + /* division by zero impossible for trim == min (as for throttle), as this falls in the above if clause */ + _rc.chan[i].scaled = -((_parameters.trim[i] - rc_input.values[i]) / (float)(_parameters.trim[i] - _parameters.min[i])); + + } else { + /* in the configured dead zone, output zero */ + _rc.chan[i].scaled = 0.0f; + } + + /* reverse channel if required */ + if (i == _rc.function[THROTTLE]) { + if ((int)_parameters.rev[i] == -1) { + _rc.chan[i].scaled = 1.0f + -1.0f * _rc.chan[i].scaled; + } + } else { + _rc.chan[i].scaled *= _parameters.rev[i]; + } + + /* handle any parameter-induced blowups */ + if (isnan(_rc.chan[i].scaled) || isinf(_rc.chan[i].scaled)) + _rc.chan[i].scaled = 0.0f; + } + + _rc.chan_count = rc_input.channel_count; + _rc.timestamp = rc_input.timestamp; + + manual_control.timestamp = rc_input.timestamp; + + /* roll input - rolling right is stick-wise and rotation-wise positive */ + manual_control.roll = _rc.chan[_rc.function[ROLL]].scaled; + if (manual_control.roll < -1.0f) manual_control.roll = -1.0f; + if (manual_control.roll > 1.0f) manual_control.roll = 1.0f; + if (!isnan(_parameters.rc_scale_roll) || !isinf(_parameters.rc_scale_roll)) { + manual_control.roll *= _parameters.rc_scale_roll; + } + + /* + * pitch input - stick down is negative, but stick down is pitching up (pos) in NED, + * so reverse sign. + */ + manual_control.pitch = -1.0f * _rc.chan[_rc.function[PITCH]].scaled; + if (manual_control.pitch < -1.0f) manual_control.pitch = -1.0f; + if (manual_control.pitch > 1.0f) manual_control.pitch = 1.0f; + if (!isnan(_parameters.rc_scale_pitch) || !isinf(_parameters.rc_scale_pitch)) { + manual_control.pitch *= _parameters.rc_scale_pitch; + } + + /* yaw input - stick right is positive and positive rotation */ + manual_control.yaw = _rc.chan[_rc.function[YAW]].scaled * _parameters.rc_scale_yaw; + if (manual_control.yaw < -1.0f) manual_control.yaw = -1.0f; + if (manual_control.yaw > 1.0f) manual_control.yaw = 1.0f; + if (!isnan(_parameters.rc_scale_yaw) || !isinf(_parameters.rc_scale_yaw)) { + manual_control.yaw *= _parameters.rc_scale_yaw; + } + + /* throttle input */ + manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled; + if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f; + if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f; + + /* mode switch input */ + manual_control.override_mode_switch = _rc.chan[_rc.function[OVERRIDE]].scaled; + if (manual_control.override_mode_switch < -1.0f) manual_control.override_mode_switch = -1.0f; + if (manual_control.override_mode_switch > 1.0f) manual_control.override_mode_switch = 1.0f; + + orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc); + orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control); + } } #endif @@ -979,6 +1010,7 @@ Sensors::task_main() _gyro_sub = orb_subscribe(ORB_ID(sensor_gyro)); _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); _mag_sub = orb_subscribe(ORB_ID(sensor_mag)); + _rc_sub = orb_subscribe(ORB_ID(input_rc)); _baro_sub = orb_subscribe(ORB_ID(sensor_baro)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); diff --git a/apps/uORB/topics/rc_channels.h b/apps/uORB/topics/rc_channels.h index 2c487cf3b..fef6ef2b3 100644 --- a/apps/uORB/topics/rc_channels.h +++ b/apps/uORB/topics/rc_channels.h @@ -50,14 +50,6 @@ * @{ */ -enum RC_CHANNELS_STATUS -{ - UNKNOWN = 0, - KNOWN = 1, - SIGNAL = 2, - TIMEOUT = 3 -}; - /** * This defines the mapping of the RC functions. * The value assigned to the specific function corresponds to the entry of @@ -85,12 +77,7 @@ struct rc_channels_s { uint64_t timestamp; /**< In microseconds since boot time. */ uint64_t timestamp_last_valid; /**< timestamp of last valid RC signal. */ struct { - uint16_t mid; /**< midpoint (0). */ - float scaling_factor; /**< scaling factor from raw counts to -1..+1 */ - uint16_t raw; /**< current raw value */ float scaled; /**< Scaled to -1..1 (throttle: 0..1) */ - uint16_t override; - enum RC_CHANNELS_STATUS status; /**< status of the channel */ } chan[RC_CHANNELS_FUNCTION_MAX]; uint8_t chan_count; /**< maximum number of valid channels */ @@ -98,6 +85,7 @@ struct rc_channels_s { char function_name[RC_CHANNELS_FUNCTION_MAX][20]; uint8_t function[RC_CHANNELS_FUNCTION_MAX]; uint8_t rssi; /**< Overall receive signal strength */ + bool is_valid; /**< Inputs are valid, no timeout */ }; /**< radio control channels. */ /** -- cgit v1.2.3