From 367ce63b86b863d72a3f6b4250d9da780a85f40b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 3 Apr 2014 11:45:57 +0400 Subject: 'signal_lost' flag added to manual_control_setpoint and rc_channels topics to indicate signal loss immediately --- src/modules/sensors/sensors.cpp | 224 +++++++++++++++++++++------------------- 1 file changed, 118 insertions(+), 106 deletions(-) (limited to 'src/modules/sensors') diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index f890c4c7f..9f816f5e1 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -474,6 +474,7 @@ Sensors::Sensors() : _battery_discharged(0), _battery_current_timestamp(0) { + memset(&_rc, 0, sizeof(_rc)); /* basic r/c parameters */ for (unsigned i = 0; i < _rc_max_chan_count; i++) { @@ -1283,12 +1284,6 @@ Sensors::rc_poll() if (rc_updated) { /* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */ struct rc_input_values rc_input; - - orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); - - if (rc_input.rc_lost) - return; - struct manual_control_setpoint_s manual_control; struct actuator_controls_s actuator_group_3; @@ -1311,19 +1306,28 @@ Sensors::rc_poll() manual_control.aux4 = NAN; manual_control.aux5 = NAN; - /* require at least four channels to consider the signal valid */ - if (rc_input.channel_count < 4) - return; + manual_control.signal_lost = true; - /* failsafe check */ - if (_parameters.rc_fs_ch != 0) { - if (_parameters.rc_fs_mode == 0) { - if (rc_input.values[_parameters.rc_fs_ch - 1] < _parameters.rc_fs_thr) - return; + orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); + + /* detect RC signal loss */ + /* check flags and require at least four channels to consider the signal valid */ + if (!(rc_input.rc_lost || rc_input.rc_failsafe || rc_input.channel_count < 4)) { + /* signal looks good */ + manual_control.signal_lost = false; + + /* check throttle failsafe */ + if (_parameters.rc_fs_ch != 0) { + if (_parameters.rc_fs_mode == 0) { + if (rc_input.values[_parameters.rc_fs_ch - 1] < _parameters.rc_fs_thr) { + manual_control.signal_lost = true; + } - } else if (_parameters.rc_fs_mode == 1) { - if (rc_input.values[_parameters.rc_fs_ch - 1] > _parameters.rc_fs_thr) - return; + } else if (_parameters.rc_fs_mode == 1) { + if (rc_input.values[_parameters.rc_fs_ch - 1] > _parameters.rc_fs_thr) { + manual_control.signal_lost = true; + } + } } } @@ -1332,10 +1336,7 @@ Sensors::rc_poll() if (channel_limit > _rc_max_chan_count) channel_limit = _rc_max_chan_count; - /* we are accepting this message */ - _rc_last_valid = rc_input.timestamp_last_signal; - - /* Read out values from raw message */ + /* read out and scale values from raw message even if signal is invalid */ for (unsigned int i = 0; i < channel_limit; i++) { /* @@ -1382,105 +1383,124 @@ Sensors::rc_poll() } _rc.chan_count = rc_input.channel_count; - _rc.timestamp = rc_input.timestamp_last_signal; + _rc.rssi = rc_input.rssi; + _rc.signal_lost = manual_control.signal_lost; - manual_control.timestamp = rc_input.timestamp_last_signal; + if (!manual_control.signal_lost) { + _rc_last_valid = rc_input.timestamp_last_signal; + } - /* roll input - rolling right is stick-wise and rotation-wise positive */ - manual_control.roll = limit_minus_one_to_one(_rc.chan[_rc.function[ROLL]].scaled); - /* - * pitch input - stick down is negative, but stick down is pitching up (pos) in NED, - * so reverse sign. - */ - manual_control.pitch = limit_minus_one_to_one(-1.0f * _rc.chan[_rc.function[PITCH]].scaled); - /* yaw input - stick right is positive and positive rotation */ - manual_control.yaw = limit_minus_one_to_one(_rc.chan[_rc.function[YAW]].scaled); - /* throttle input */ - manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled; + _rc.timestamp = _rc_last_valid; - if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f; + manual_control.timestamp = _rc_last_valid; - if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f; + if (!manual_control.signal_lost) { + /* fill values in manual_control_setpoint topic only if signal is valid */ - /* scale output */ - if (isfinite(_parameters.rc_scale_roll) && _parameters.rc_scale_roll > 0.0f) { - manual_control.roll *= _parameters.rc_scale_roll; - } + /* roll input - rolling right is stick-wise and rotation-wise positive */ + manual_control.roll = limit_minus_one_to_one(_rc.chan[_rc.function[ROLL]].scaled); + /* + * pitch input - stick down is negative, but stick down is pitching up (pos) in NED, + * so reverse sign. + */ + manual_control.pitch = limit_minus_one_to_one(-1.0f * _rc.chan[_rc.function[PITCH]].scaled); + /* yaw input - stick right is positive and positive rotation */ + manual_control.yaw = limit_minus_one_to_one(_rc.chan[_rc.function[YAW]].scaled); + /* throttle input */ + manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled; - if (isfinite(_parameters.rc_scale_pitch) && _parameters.rc_scale_pitch > 0.0f) { - manual_control.pitch *= _parameters.rc_scale_pitch; - } + if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f; - if (isfinite(_parameters.rc_scale_yaw) && _parameters.rc_scale_yaw > 0.0f) { - manual_control.yaw *= _parameters.rc_scale_yaw; - } + if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f; - /* flaps */ - if (_rc.function[FLAPS] >= 0) { + /* scale output */ + if (isfinite(_parameters.rc_scale_roll) && _parameters.rc_scale_roll > 0.0f) { + manual_control.roll *= _parameters.rc_scale_roll; + } - manual_control.flaps = limit_minus_one_to_one(_rc.chan[_rc.function[FLAPS]].scaled); + if (isfinite(_parameters.rc_scale_pitch) && _parameters.rc_scale_pitch > 0.0f) { + manual_control.pitch *= _parameters.rc_scale_pitch; + } - if (isfinite(_parameters.rc_scale_flaps) && _parameters.rc_scale_flaps > 0.0f) { - manual_control.flaps *= _parameters.rc_scale_flaps; + if (isfinite(_parameters.rc_scale_yaw) && _parameters.rc_scale_yaw > 0.0f) { + manual_control.yaw *= _parameters.rc_scale_yaw; } - } - /* mode switch input */ - if (_rc.function[MODE] >= 0) { - manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled); - } + /* flaps */ + if (_rc.function[FLAPS] >= 0) { - /* assisted switch input */ - if (_rc.function[ASSISTED] >= 0) { - manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled); - } + manual_control.flaps = limit_minus_one_to_one(_rc.chan[_rc.function[FLAPS]].scaled); - /* mission switch input */ - if (_rc.function[MISSION] >= 0) { - manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled); - } + if (isfinite(_parameters.rc_scale_flaps) && _parameters.rc_scale_flaps > 0.0f) { + manual_control.flaps *= _parameters.rc_scale_flaps; + } + } - /* return switch input */ - if (_rc.function[RETURN] >= 0) { - manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled); - } + /* mode switch input */ + if (_rc.function[MODE] >= 0) { + manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled); + } -// if (_rc.function[OFFBOARD_MODE] >= 0) { -// manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled); -// } + /* assisted switch input */ + if (_rc.function[ASSISTED] >= 0) { + manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled); + } - /* aux functions, only assign if valid mapping is present */ - if (_rc.function[AUX_1] >= 0) { - manual_control.aux1 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_1]].scaled); - } + /* mission switch input */ + if (_rc.function[MISSION] >= 0) { + manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled); + } - if (_rc.function[AUX_2] >= 0) { - manual_control.aux2 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_2]].scaled); - } + /* return switch input */ + if (_rc.function[RETURN] >= 0) { + manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled); + } - if (_rc.function[AUX_3] >= 0) { - manual_control.aux3 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_3]].scaled); - } + // if (_rc.function[OFFBOARD_MODE] >= 0) { + // manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled); + // } - if (_rc.function[AUX_4] >= 0) { - manual_control.aux4 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_4]].scaled); - } + /* aux functions, only assign if valid mapping is present */ + if (_rc.function[AUX_1] >= 0) { + manual_control.aux1 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_1]].scaled); + } + + if (_rc.function[AUX_2] >= 0) { + manual_control.aux2 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_2]].scaled); + } + + if (_rc.function[AUX_3] >= 0) { + manual_control.aux3 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_3]].scaled); + } - if (_rc.function[AUX_5] >= 0) { - manual_control.aux5 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_5]].scaled); + if (_rc.function[AUX_4] >= 0) { + manual_control.aux4 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_4]].scaled); + } + + if (_rc.function[AUX_5] >= 0) { + manual_control.aux5 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_5]].scaled); + } + + /* copy from mapped manual control to control group 3 */ + actuator_group_3.control[0] = manual_control.roll; + actuator_group_3.control[1] = manual_control.pitch; + actuator_group_3.control[2] = manual_control.yaw; + actuator_group_3.control[3] = manual_control.throttle; + actuator_group_3.control[4] = manual_control.flaps; + actuator_group_3.control[5] = manual_control.aux1; + actuator_group_3.control[6] = manual_control.aux2; + actuator_group_3.control[7] = manual_control.aux3; + + /* publish actuator_controls_3 topic only if control signal is valid */ + if (_actuator_group_3_pub > 0) { + orb_publish(ORB_ID(actuator_controls_3), _actuator_group_3_pub, &actuator_group_3); + + } else { + _actuator_group_3_pub = orb_advertise(ORB_ID(actuator_controls_3), &actuator_group_3); + } } - /* copy from mapped manual control to control group 3 */ - actuator_group_3.control[0] = manual_control.roll; - actuator_group_3.control[1] = manual_control.pitch; - actuator_group_3.control[2] = manual_control.yaw; - actuator_group_3.control[3] = manual_control.throttle; - actuator_group_3.control[4] = manual_control.flaps; - actuator_group_3.control[5] = manual_control.aux1; - actuator_group_3.control[6] = manual_control.aux2; - actuator_group_3.control[7] = manual_control.aux3; - - /* check if ready for publishing */ + /* publish rc_channels topic even if signal is invalid, for debug */ if (_rc_pub > 0) { orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc); @@ -1489,21 +1509,13 @@ Sensors::rc_poll() _rc_pub = orb_advertise(ORB_ID(rc_channels), &_rc); } - /* check if ready for publishing */ + /* publish manual_control_setpoint topic even if signal is not valid to update 'signal_lost' flag */ if (_manual_control_pub > 0) { orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control); } else { _manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control); } - - /* check if ready for publishing */ - if (_actuator_group_3_pub > 0) { - orb_publish(ORB_ID(actuator_controls_3), _actuator_group_3_pub, &actuator_group_3); - - } else { - _actuator_group_3_pub = orb_advertise(ORB_ID(actuator_controls_3), &actuator_group_3); - } } } -- cgit v1.2.3 From 1d5f62d890d1d85cef5e0f8e282d8e9e70717d46 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 3 Apr 2014 17:26:07 +0400 Subject: sensors: use enum for switches position and -1..1 for values in 'manual_control_setpoint' topic --- src/modules/sensors/sensors.cpp | 165 ++++++++++------------ src/modules/uORB/topics/manual_control_setpoint.h | 37 ++--- 2 files changed, 93 insertions(+), 109 deletions(-) (limited to 'src/modules/sensors') diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 9f816f5e1..e08d8618f 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -135,7 +135,7 @@ */ #define PCB_TEMP_ESTIMATE_DEG 5.0f -#define limit_minus_one_to_one(arg) (arg < -1.0f) ? -1.0f : ((arg > 1.0f) ? 1.0f : arg) +#define STICK_ON_OFF_LIMIT 0.75f /** * Sensor app start / stop handling function @@ -169,6 +169,16 @@ private: hrt_abstime _rc_last_valid; /**< last time we got a valid RC signal */ + /** + * Get and limit value for specified RC function. Returns NAN if not mapped. + */ + float get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max_value); + + /** + * Get switch position for specified function. + */ + switch_pos_t get_rc_switch_position(enum RC_CHANNELS_FUNCTION func); + /** * Gather and publish RC input data. */ @@ -1275,6 +1285,45 @@ Sensors::adc_poll(struct sensor_combined_s &raw) } } +float +Sensors::get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max_value) +{ + if (_rc.function[func] >= 0) { + float value = _rc.chan[_rc.function[func]].scaled; + if (value < min_value) { + return min_value; + + } else if (value > max_value) { + return max_value; + + } else { + return value; + } + } else { + return NAN; + } +} + +switch_pos_t +Sensors::get_rc_switch_position(enum RC_CHANNELS_FUNCTION func) +{ + if (_rc.function[func] >= 0) { + float value = _rc.chan[_rc.function[func]].scaled; + if (value > STICK_ON_OFF_LIMIT) { + return SWITCH_POS_ON; + + } else if (value < STICK_ON_OFF_LIMIT) { + return SWITCH_POS_OFF; + + } else { + return SWITCH_POS_MIDDLE; + } + + } else { + return SWITCH_POS_NONE; + } +} + void Sensors::rc_poll() { @@ -1292,13 +1341,6 @@ Sensors::rc_poll() manual_control.pitch = NAN; manual_control.yaw = NAN; manual_control.throttle = NAN; - - manual_control.mode_switch = NAN; - manual_control.return_switch = NAN; - manual_control.assisted_switch = NAN; - manual_control.mission_switch = NAN; -// manual_control.auto_offboard_input_switch = NAN; - manual_control.flaps = NAN; manual_control.aux1 = NAN; manual_control.aux2 = NAN; @@ -1306,6 +1348,11 @@ Sensors::rc_poll() manual_control.aux4 = NAN; manual_control.aux5 = NAN; + manual_control.mode_switch = SWITCH_POS_NONE; + manual_control.return_switch = SWITCH_POS_NONE; + manual_control.assisted_switch = SWITCH_POS_NONE; + manual_control.mission_switch = SWITCH_POS_NONE; + manual_control.signal_lost = true; orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); @@ -1316,7 +1363,7 @@ Sensors::rc_poll() /* signal looks good */ manual_control.signal_lost = false; - /* check throttle failsafe */ + /* check for throttle failsafe */ if (_parameters.rc_fs_ch != 0) { if (_parameters.rc_fs_mode == 0) { if (rc_input.values[_parameters.rc_fs_ch - 1] < _parameters.rc_fs_thr) { @@ -1397,89 +1444,23 @@ Sensors::rc_poll() if (!manual_control.signal_lost) { /* fill values in manual_control_setpoint topic only if signal is valid */ - /* roll input - rolling right is stick-wise and rotation-wise positive */ - manual_control.roll = limit_minus_one_to_one(_rc.chan[_rc.function[ROLL]].scaled); - /* - * pitch input - stick down is negative, but stick down is pitching up (pos) in NED, - * so reverse sign. - */ - manual_control.pitch = limit_minus_one_to_one(-1.0f * _rc.chan[_rc.function[PITCH]].scaled); - /* yaw input - stick right is positive and positive rotation */ - manual_control.yaw = limit_minus_one_to_one(_rc.chan[_rc.function[YAW]].scaled); - /* 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; - - /* scale output */ - if (isfinite(_parameters.rc_scale_roll) && _parameters.rc_scale_roll > 0.0f) { - manual_control.roll *= _parameters.rc_scale_roll; - } - - if (isfinite(_parameters.rc_scale_pitch) && _parameters.rc_scale_pitch > 0.0f) { - manual_control.pitch *= _parameters.rc_scale_pitch; - } - - if (isfinite(_parameters.rc_scale_yaw) && _parameters.rc_scale_yaw > 0.0f) { - manual_control.yaw *= _parameters.rc_scale_yaw; - } - - /* flaps */ - if (_rc.function[FLAPS] >= 0) { - - manual_control.flaps = limit_minus_one_to_one(_rc.chan[_rc.function[FLAPS]].scaled); - - if (isfinite(_parameters.rc_scale_flaps) && _parameters.rc_scale_flaps > 0.0f) { - manual_control.flaps *= _parameters.rc_scale_flaps; - } - } - - /* mode switch input */ - if (_rc.function[MODE] >= 0) { - manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled); - } - - /* assisted switch input */ - if (_rc.function[ASSISTED] >= 0) { - manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled); - } - - /* mission switch input */ - if (_rc.function[MISSION] >= 0) { - manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled); - } - - /* return switch input */ - if (_rc.function[RETURN] >= 0) { - manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled); - } - - // if (_rc.function[OFFBOARD_MODE] >= 0) { - // manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled); - // } - - /* aux functions, only assign if valid mapping is present */ - if (_rc.function[AUX_1] >= 0) { - manual_control.aux1 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_1]].scaled); - } - - if (_rc.function[AUX_2] >= 0) { - manual_control.aux2 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_2]].scaled); - } - - if (_rc.function[AUX_3] >= 0) { - manual_control.aux3 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_3]].scaled); - } - - if (_rc.function[AUX_4] >= 0) { - manual_control.aux4 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_4]].scaled); - } - - if (_rc.function[AUX_5] >= 0) { - manual_control.aux5 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_5]].scaled); - } + /* limit controls */ + manual_control.roll = get_rc_value(ROLL, -1.0, 1.0); + manual_control.pitch = get_rc_value(PITCH, -1.0, 1.0); + manual_control.yaw = get_rc_value(YAW, -1.0, 1.0); + manual_control.throttle = get_rc_value(THROTTLE, 0.0, 1.0); + manual_control.flaps = get_rc_value(FLAPS, -1.0, 1.0); + manual_control.aux1 = get_rc_value(AUX_1, -1.0, 1.0); + manual_control.aux2 = get_rc_value(AUX_2, -1.0, 1.0); + manual_control.aux3 = get_rc_value(AUX_3, -1.0, 1.0); + manual_control.aux4 = get_rc_value(AUX_4, -1.0, 1.0); + manual_control.aux5 = get_rc_value(AUX_5, -1.0, 1.0); + + /* mode switches */ + manual_control.mode_switch = get_rc_switch_position(MODE); + manual_control.assisted_switch = get_rc_switch_position(ASSISTED); + manual_control.mission_switch = get_rc_switch_position(MISSION); + manual_control.return_switch = get_rc_switch_position(RETURN); /* copy from mapped manual control to control group 3 */ actuator_group_3.control[0] = manual_control.roll; diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index 5d1384380..985d3923f 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -43,6 +43,16 @@ #include #include "../uORB.h" +/** + * Switch position + */ +typedef enum { + SWITCH_POS_NONE = 0, /**< switch is not mapped */ + SWITCH_POS_ON, /**< switch activated (value = 1) */ + SWITCH_POS_MIDDLE, /**< middle position (value = 0) */ + SWITCH_POS_OFF /**< switch not activated (value = -1) */ +} switch_pos_t; + /** * @addtogroup topics * @{ @@ -53,32 +63,25 @@ struct manual_control_setpoint_s { bool signal_lost; /**< control signal lost, should be checked together with topic timeout */ - float roll; /**< ailerons roll / roll rate input */ - float pitch; /**< elevator / pitch / pitch rate */ - float yaw; /**< rudder / yaw rate / yaw */ - float throttle; /**< throttle / collective thrust / altitude */ - - float mode_switch; /**< mode 3 position switch (mandatory): manual, assisted, auto */ - float return_switch; /**< land 2 position switch (mandatory): land, no effect */ - float assisted_switch; /**< assisted 2 position switch (optional): seatbelt, simple */ - float mission_switch; /**< mission 2 position switch (optional): mission, loiter */ - /** - * Any of the channels below may not be available and be set to NaN + * Any of the channels may not be available and be set to NaN * to indicate that it does not contain valid data. */ - - // XXX needed or parameter? - //float auto_offboard_input_switch; /**< controller setpoint source (0 = onboard, 1 = offboard) */ - - float flaps; /**< flap position */ - + float roll; /**< ailerons roll / roll rate input, -1..1 */ + float pitch; /**< elevator / pitch / pitch rate, -1..1 */ + float yaw; /**< rudder / yaw rate / yaw, -1..1 */ + float throttle; /**< throttle / collective thrust / altitude, 0..1 */ + float flaps; /**< flap position */ float aux1; /**< default function: camera yaw / azimuth */ float aux2; /**< default function: camera pitch / tilt */ float aux3; /**< default function: camera trigger */ float aux4; /**< default function: camera roll */ float aux5; /**< default function: payload drop */ + switch_pos_t mode_switch; /**< mode 3 position switch (mandatory): manual, assisted, auto */ + switch_pos_t return_switch; /**< land 2 position switch (mandatory): land, no effect */ + switch_pos_t assisted_switch; /**< assisted 2 position switch (optional): seatbelt, simple */ + switch_pos_t mission_switch; /**< mission 2 position switch (optional): mission, loiter */ }; /**< manual control inputs */ /** -- cgit v1.2.3 From e2ac5222d812bdbfaf33fc2d320ee22ab861d433 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 3 Apr 2014 20:54:28 +0400 Subject: mc_att_control, mc_pos_control: update manual_control_setpoint usage --- src/modules/mc_att_control/mc_att_control_main.cpp | 54 +++++++++++----------- src/modules/mc_att_control/mc_att_control_params.c | 29 ++++++++++++ src/modules/mc_pos_control/mc_pos_control_main.cpp | 14 +----- src/modules/sensors/sensor_params.c | 22 --------- src/modules/sensors/sensors.cpp | 19 -------- 5 files changed, 57 insertions(+), 81 deletions(-) (limited to 'src/modules/sensors') diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 9cb8e8344..01f1631a9 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -157,7 +157,9 @@ private: param_t yaw_rate_d; param_t yaw_ff; - param_t rc_scale_yaw; + param_t man_scale_roll; + param_t man_scale_pitch; + param_t man_scale_yaw; } _params_handles; /**< handles for interesting parameters */ struct { @@ -167,7 +169,9 @@ private: math::Vector<3> rate_d; /**< D gain for angular rate error */ float yaw_ff; /**< yaw control feed-forward */ - float rc_scale_yaw; + float man_scale_roll; + float man_scale_pitch; + float man_scale_yaw; } _params; /** @@ -295,7 +299,9 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params_handles.yaw_rate_d = param_find("MC_YAWRATE_D"); _params_handles.yaw_ff = param_find("MC_YAW_FF"); - _params_handles.rc_scale_yaw = param_find("RC_SCALE_YAW"); + _params_handles.man_scale_roll = param_find("MC_SCALE_ROLL"); + _params_handles.man_scale_pitch = param_find("MC_SCALE_PITCH"); + _params_handles.man_scale_yaw = param_find("MC_SCALE_YAW"); /* fetch initial parameter values */ parameters_update(); @@ -330,7 +336,7 @@ MulticopterAttitudeControl::parameters_update() { float v; - /* roll */ + /* roll gains */ param_get(_params_handles.roll_p, &v); _params.att_p(0) = v; param_get(_params_handles.roll_rate_p, &v); @@ -340,7 +346,7 @@ MulticopterAttitudeControl::parameters_update() param_get(_params_handles.roll_rate_d, &v); _params.rate_d(0) = v; - /* pitch */ + /* pitch gains */ param_get(_params_handles.pitch_p, &v); _params.att_p(1) = v; param_get(_params_handles.pitch_rate_p, &v); @@ -350,7 +356,7 @@ MulticopterAttitudeControl::parameters_update() param_get(_params_handles.pitch_rate_d, &v); _params.rate_d(1) = v; - /* yaw */ + /* yaw gains */ param_get(_params_handles.yaw_p, &v); _params.att_p(2) = v; param_get(_params_handles.yaw_rate_p, &v); @@ -362,7 +368,14 @@ MulticopterAttitudeControl::parameters_update() param_get(_params_handles.yaw_ff, &_params.yaw_ff); - param_get(_params_handles.rc_scale_yaw, &_params.rc_scale_yaw); + /* manual control scale */ + param_get(_params_handles.man_scale_roll, &_params.man_scale_roll); + param_get(_params_handles.man_scale_pitch, &_params.man_scale_pitch); + param_get(_params_handles.man_scale_yaw, &_params.man_scale_yaw); + + _params.man_scale_roll *= M_PI / 180.0; + _params.man_scale_pitch *= M_PI / 180.0; + _params.man_scale_yaw *= M_PI / 180.0; return OK; } @@ -404,7 +417,6 @@ MulticopterAttitudeControl::vehicle_manual_poll() orb_check(_manual_control_sp_sub, &updated); if (updated) { - orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sp_sub, &_manual_control_sp); } } @@ -483,24 +495,10 @@ MulticopterAttitudeControl::control_attitude(float dt) // reset_yaw_sp = true; //} } else { - float yaw_dz_scaled = YAW_DEADZONE * _params.rc_scale_yaw; - - if (_params.rc_scale_yaw > 0.001f && fabs(_manual_control_sp.yaw) > yaw_dz_scaled) { - /* move yaw setpoint */ - yaw_sp_move_rate = _manual_control_sp.yaw / _params.rc_scale_yaw; - - if (_manual_control_sp.yaw > 0.0f) { - yaw_sp_move_rate -= YAW_DEADZONE; - - } else { - yaw_sp_move_rate += YAW_DEADZONE; - } - - yaw_sp_move_rate *= _params.rc_scale_yaw; - _v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt); - _v_att_sp.R_valid = false; - publish_att_sp = true; - } + /* move yaw setpoint */ + _v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + _manual_control_sp.yaw * _params.man_scale_yaw * dt); + _v_att_sp.R_valid = false; + publish_att_sp = true; } /* reset yaw setpint to current position if needed */ @@ -513,8 +511,8 @@ MulticopterAttitudeControl::control_attitude(float dt) if (!_v_control_mode.flag_control_velocity_enabled) { /* update attitude setpoint if not in position control mode */ - _v_att_sp.roll_body = _manual_control_sp.roll; - _v_att_sp.pitch_body = _manual_control_sp.pitch; + _v_att_sp.roll_body = _manual_control_sp.roll * _params.man_scale_roll; + _v_att_sp.pitch_body = -_manual_control_sp.pitch * _params.man_scale_pitch; _v_att_sp.R_valid = false; publish_att_sp = true; } diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index 488107d58..9acf8bfa3 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -173,3 +173,32 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); * @group Multicopter Attitude Control */ PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f); + +/** + * Manual control scaling factor for roll + * + * @unit deg + * @min 0.0 + * @max 90.0 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_SCALE_ROLL, 35.0f); + +/** + * Manual control scaling factor for pitch + * + * @unit deg + * @min 0.0 + * @max 90.0 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_SCALE_PITCH, 35.0f); + +/** + * Manual control scaling factor for yaw + * + * @unit deg/s + * @min 0.0 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_SCALE_YAW, 120.0f); diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 78d06ba5b..2bd2d356a 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -148,9 +148,6 @@ private: param_t tilt_max; param_t land_speed; param_t land_tilt_max; - - param_t rc_scale_pitch; - param_t rc_scale_roll; } _params_handles; /**< handles for interesting parameters */ struct { @@ -160,9 +157,6 @@ private: float land_speed; float land_tilt_max; - float rc_scale_pitch; - float rc_scale_roll; - math::Vector<3> pos_p; math::Vector<3> vel_p; math::Vector<3> vel_i; @@ -306,8 +300,6 @@ MulticopterPositionControl::MulticopterPositionControl() : _params_handles.tilt_max = param_find("MPC_TILT_MAX"); _params_handles.land_speed = param_find("MPC_LAND_SPEED"); _params_handles.land_tilt_max = param_find("MPC_LAND_TILT"); - _params_handles.rc_scale_pitch = param_find("RC_SCALE_PITCH"); - _params_handles.rc_scale_roll = param_find("RC_SCALE_ROLL"); /* fetch initial parameter values */ parameters_update(true); @@ -354,8 +346,6 @@ MulticopterPositionControl::parameters_update(bool force) param_get(_params_handles.tilt_max, &_params.tilt_max); param_get(_params_handles.land_speed, &_params.land_speed); param_get(_params_handles.land_tilt_max, &_params.land_tilt_max); - param_get(_params_handles.rc_scale_pitch, &_params.rc_scale_pitch); - param_get(_params_handles.rc_scale_roll, &_params.rc_scale_roll); float v; param_get(_params_handles.xy_p, &v); @@ -608,8 +598,8 @@ MulticopterPositionControl::task_main() reset_lat_lon_sp(); /* move position setpoint with roll/pitch stick */ - sp_move_rate(0) = scale_control(-_manual.pitch / _params.rc_scale_pitch, 1.0f, pos_ctl_dz); - sp_move_rate(1) = scale_control(_manual.roll / _params.rc_scale_roll, 1.0f, pos_ctl_dz); + sp_move_rate(0) = _manual.pitch; + sp_move_rate(1) = _manual.roll; } /* limit setpoint move rate */ diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 288c6e00a..09afaf949 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -647,28 +647,6 @@ PARAM_DEFINE_INT32(RC_MAP_AUX2, 0); /**< default function: camera roll */ PARAM_DEFINE_INT32(RC_MAP_AUX3, 0); -/** - * Roll scaling factor - * - * @group Radio Calibration - */ -PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.6f); - -/** - * Pitch scaling factor - * - * @group Radio Calibration - */ -PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.6f); - -/** - * Yaw scaling factor - * - * @group Radio Calibration - */ -PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 2.0f); - - /** * Failsafe channel mapping. * diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index e08d8618f..8f350751c 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -268,11 +268,6 @@ private: int rc_map_aux4; int rc_map_aux5; - float rc_scale_roll; - float rc_scale_pitch; - float rc_scale_yaw; - float rc_scale_flaps; - int rc_fs_ch; int rc_fs_mode; float rc_fs_thr; @@ -318,11 +313,6 @@ private: param_t rc_map_aux4; param_t rc_map_aux5; - param_t rc_scale_roll; - param_t rc_scale_pitch; - param_t rc_scale_yaw; - param_t rc_scale_flaps; - param_t rc_fs_ch; param_t rc_fs_mode; param_t rc_fs_thr; @@ -536,11 +526,6 @@ Sensors::Sensors() : _parameter_handles.rc_map_aux4 = param_find("RC_MAP_AUX4"); _parameter_handles.rc_map_aux5 = param_find("RC_MAP_AUX5"); - _parameter_handles.rc_scale_roll = param_find("RC_SCALE_ROLL"); - _parameter_handles.rc_scale_pitch = param_find("RC_SCALE_PITCH"); - _parameter_handles.rc_scale_yaw = param_find("RC_SCALE_YAW"); - _parameter_handles.rc_scale_flaps = param_find("RC_SCALE_FLAPS"); - /* RC failsafe */ _parameter_handles.rc_fs_ch = param_find("RC_FS_CH"); _parameter_handles.rc_fs_mode = param_find("RC_FS_MODE"); @@ -696,10 +681,6 @@ Sensors::parameters_update() param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3)); param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4)); param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5)); - param_get(_parameter_handles.rc_scale_roll, &(_parameters.rc_scale_roll)); - param_get(_parameter_handles.rc_scale_pitch, &(_parameters.rc_scale_pitch)); - param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw)); - param_get(_parameter_handles.rc_scale_flaps, &(_parameters.rc_scale_flaps)); param_get(_parameter_handles.rc_fs_ch, &(_parameters.rc_fs_ch)); param_get(_parameter_handles.rc_fs_mode, &(_parameters.rc_fs_mode)); param_get(_parameter_handles.rc_fs_thr, &(_parameters.rc_fs_thr)); -- cgit v1.2.3 From 3641faed0c696f1a88f88a17b5666ed5c3ba8b1c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 3 Apr 2014 23:18:43 +0400 Subject: sensors: publish last valid manual control values when signal lost --- src/modules/sensors/sensors.cpp | 113 ++++++++++++++++++---------------------- 1 file changed, 51 insertions(+), 62 deletions(-) (limited to 'src/modules/sensors') diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 8f350751c..669f4e174 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -167,8 +167,6 @@ public: private: static const unsigned _rc_max_chan_count = RC_INPUT_MAX_CHANNELS; /**< maximum number of r/c channels we handle */ - hrt_abstime _rc_last_valid; /**< last time we got a valid RC signal */ - /** * Get and limit value for specified RC function. Returns NAN if not mapped. */ @@ -216,6 +214,7 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ struct rc_channels_s _rc; /**< r/c channel data */ + struct manual_control_setpoint_s _manual; /**< manual control setpoint */ struct battery_status_s _battery_status; /**< battery status */ struct baro_report _barometer; /**< barometer data */ struct differential_pressure_s _diff_pres; @@ -438,8 +437,6 @@ Sensors *g_sensors = nullptr; } Sensors::Sensors() : - _rc_last_valid(0), - _fd_adc(-1), _last_adc(0), @@ -475,6 +472,21 @@ Sensors::Sensors() : _battery_current_timestamp(0) { memset(&_rc, 0, sizeof(_rc)); + memset(&_manual, 0, sizeof(_manual)); + + /* set NANs instead of zeroes */ + _manual.roll = NAN; + _manual.pitch = NAN; + _manual.yaw = NAN; + _manual.throttle = NAN; + _manual.flaps = NAN; + _manual.aux1 = NAN; + _manual.aux2 = NAN; + _manual.aux3 = NAN; + _manual.aux4 = NAN; + _manual.aux5 = NAN; + + _manual.signal_lost = true; /* basic r/c parameters */ for (unsigned i = 0; i < _rc_max_chan_count; i++) { @@ -1314,27 +1326,8 @@ Sensors::rc_poll() if (rc_updated) { /* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */ struct rc_input_values rc_input; - struct manual_control_setpoint_s manual_control; - struct actuator_controls_s actuator_group_3; - - /* initialize to default values */ - manual_control.roll = NAN; - manual_control.pitch = NAN; - manual_control.yaw = NAN; - manual_control.throttle = NAN; - manual_control.flaps = NAN; - manual_control.aux1 = NAN; - manual_control.aux2 = NAN; - manual_control.aux3 = NAN; - manual_control.aux4 = NAN; - manual_control.aux5 = NAN; - - manual_control.mode_switch = SWITCH_POS_NONE; - manual_control.return_switch = SWITCH_POS_NONE; - manual_control.assisted_switch = SWITCH_POS_NONE; - manual_control.mission_switch = SWITCH_POS_NONE; - - manual_control.signal_lost = true; + + _manual.signal_lost = true; orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); @@ -1342,18 +1335,18 @@ Sensors::rc_poll() /* check flags and require at least four channels to consider the signal valid */ if (!(rc_input.rc_lost || rc_input.rc_failsafe || rc_input.channel_count < 4)) { /* signal looks good */ - manual_control.signal_lost = false; + _manual.signal_lost = false; /* check for throttle failsafe */ if (_parameters.rc_fs_ch != 0) { if (_parameters.rc_fs_mode == 0) { if (rc_input.values[_parameters.rc_fs_ch - 1] < _parameters.rc_fs_thr) { - manual_control.signal_lost = true; + _manual.signal_lost = true; } } else if (_parameters.rc_fs_mode == 1) { if (rc_input.values[_parameters.rc_fs_ch - 1] > _parameters.rc_fs_thr) { - manual_control.signal_lost = true; + _manual.signal_lost = true; } } } @@ -1412,46 +1405,42 @@ Sensors::rc_poll() _rc.chan_count = rc_input.channel_count; _rc.rssi = rc_input.rssi; - _rc.signal_lost = manual_control.signal_lost; - - if (!manual_control.signal_lost) { - _rc_last_valid = rc_input.timestamp_last_signal; - } - - _rc.timestamp = _rc_last_valid; + _rc.signal_lost = _manual.signal_lost; - manual_control.timestamp = _rc_last_valid; - - if (!manual_control.signal_lost) { + if (!_manual.signal_lost) { /* fill values in manual_control_setpoint topic only if signal is valid */ + _manual.timestamp = rc_input.timestamp_last_signal; + _rc.timestamp = rc_input.timestamp_last_signal; /* limit controls */ - manual_control.roll = get_rc_value(ROLL, -1.0, 1.0); - manual_control.pitch = get_rc_value(PITCH, -1.0, 1.0); - manual_control.yaw = get_rc_value(YAW, -1.0, 1.0); - manual_control.throttle = get_rc_value(THROTTLE, 0.0, 1.0); - manual_control.flaps = get_rc_value(FLAPS, -1.0, 1.0); - manual_control.aux1 = get_rc_value(AUX_1, -1.0, 1.0); - manual_control.aux2 = get_rc_value(AUX_2, -1.0, 1.0); - manual_control.aux3 = get_rc_value(AUX_3, -1.0, 1.0); - manual_control.aux4 = get_rc_value(AUX_4, -1.0, 1.0); - manual_control.aux5 = get_rc_value(AUX_5, -1.0, 1.0); + _manual.roll = get_rc_value(ROLL, -1.0, 1.0); + _manual.pitch = get_rc_value(PITCH, -1.0, 1.0); + _manual.yaw = get_rc_value(YAW, -1.0, 1.0); + _manual.throttle = get_rc_value(THROTTLE, 0.0, 1.0); + _manual.flaps = get_rc_value(FLAPS, -1.0, 1.0); + _manual.aux1 = get_rc_value(AUX_1, -1.0, 1.0); + _manual.aux2 = get_rc_value(AUX_2, -1.0, 1.0); + _manual.aux3 = get_rc_value(AUX_3, -1.0, 1.0); + _manual.aux4 = get_rc_value(AUX_4, -1.0, 1.0); + _manual.aux5 = get_rc_value(AUX_5, -1.0, 1.0); /* mode switches */ - manual_control.mode_switch = get_rc_switch_position(MODE); - manual_control.assisted_switch = get_rc_switch_position(ASSISTED); - manual_control.mission_switch = get_rc_switch_position(MISSION); - manual_control.return_switch = get_rc_switch_position(RETURN); + _manual.mode_switch = get_rc_switch_position(MODE); + _manual.assisted_switch = get_rc_switch_position(ASSISTED); + _manual.mission_switch = get_rc_switch_position(MISSION); + _manual.return_switch = get_rc_switch_position(RETURN); /* copy from mapped manual control to control group 3 */ - actuator_group_3.control[0] = manual_control.roll; - actuator_group_3.control[1] = manual_control.pitch; - actuator_group_3.control[2] = manual_control.yaw; - actuator_group_3.control[3] = manual_control.throttle; - actuator_group_3.control[4] = manual_control.flaps; - actuator_group_3.control[5] = manual_control.aux1; - actuator_group_3.control[6] = manual_control.aux2; - actuator_group_3.control[7] = manual_control.aux3; + struct actuator_controls_s actuator_group_3; + + actuator_group_3.control[0] = _manual.roll; + actuator_group_3.control[1] = _manual.pitch; + actuator_group_3.control[2] = _manual.yaw; + actuator_group_3.control[3] = _manual.throttle; + actuator_group_3.control[4] = _manual.flaps; + actuator_group_3.control[5] = _manual.aux1; + actuator_group_3.control[6] = _manual.aux2; + actuator_group_3.control[7] = _manual.aux3; /* publish actuator_controls_3 topic only if control signal is valid */ if (_actuator_group_3_pub > 0) { @@ -1473,10 +1462,10 @@ Sensors::rc_poll() /* publish manual_control_setpoint topic even if signal is not valid to update 'signal_lost' flag */ if (_manual_control_pub > 0) { - orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control); + orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &_manual); } else { - _manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control); + _manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &_manual); } } -- cgit v1.2.3 From 60355b4e6c0a6916084442d0557cacd74b008b82 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 3 Apr 2014 23:47:09 +0400 Subject: sensors: switch position reading bug fixed --- src/modules/sensors/sensors.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/sensors') diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 669f4e174..79b53a79c 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1305,7 +1305,7 @@ Sensors::get_rc_switch_position(enum RC_CHANNELS_FUNCTION func) if (value > STICK_ON_OFF_LIMIT) { return SWITCH_POS_ON; - } else if (value < STICK_ON_OFF_LIMIT) { + } else if (value < -STICK_ON_OFF_LIMIT) { return SWITCH_POS_OFF; } else { -- cgit v1.2.3 From 77190f5052405ba8ef10c89f3193802d3d59e66f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 5 Apr 2014 15:42:23 +0400 Subject: sensors: bug fixed --- src/modules/sensors/sensors.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) (limited to 'src/modules/sensors') diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 2c1b1258c..37255c4bf 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1327,9 +1327,10 @@ Sensors::rc_poll() /* check flags and require at least four channels to consider the signal valid */ if (!(rc_input.rc_lost || rc_input.rc_failsafe || rc_input.channel_count < 4)) { /* signal looks good, but check for throttle failsafe */ - if (_parameters.rc_fs_thr == 0 || - !((_parameters.rc_fs_thr < _parameters.min[i] && rc_input.values[_rc.function[THROTTLE]] < _parameters.rc_fs_thr) || - (_parameters.rc_fs_thr > _parameters.max[i] && rc_input.values[_rc.function[THROTTLE]] > _parameters.rc_fs_thr))) { + int8_t thr_ch = _rc.function[THROTTLE]; + if (_parameters.rc_fs_thr == 0 || thr_ch < 0 || + !((_parameters.rc_fs_thr < _parameters.min[thr_ch] && rc_input.values[thr_ch] < _parameters.rc_fs_thr) || + (_parameters.rc_fs_thr > _parameters.max[thr_ch] && rc_input.values[thr_ch] > _parameters.rc_fs_thr))) { /* valid signal, throttle failsafe not configured or not triggered */ signal_lost = false; } -- cgit v1.2.3 From b00b70aeb2b7f20e74ca185e357e796f54031640 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 5 Apr 2014 16:06:10 +0400 Subject: manual_control_setpoint: signal_lost flag removed, sensors: bugs fixed --- src/modules/sensors/sensors.cpp | 36 +++++++---------------- src/modules/uORB/topics/manual_control_setpoint.h | 2 -- 2 files changed, 10 insertions(+), 28 deletions(-) (limited to 'src/modules/sensors') diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 37255c4bf..5b826a919 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -214,7 +214,6 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ struct rc_channels_s _rc; /**< r/c channel data */ - struct manual_control_setpoint_s _manual; /**< manual control setpoint */ struct battery_status_s _battery_status; /**< battery status */ struct baro_report _barometer; /**< barometer data */ struct differential_pressure_s _diff_pres; @@ -468,21 +467,6 @@ Sensors::Sensors() : _battery_current_timestamp(0) { memset(&_rc, 0, sizeof(_rc)); - memset(&_manual, 0, sizeof(_manual)); - - /* set NANs instead of zeroes */ - _manual.roll = NAN; - _manual.pitch = NAN; - _manual.yaw = NAN; - _manual.throttle = NAN; - _manual.flaps = NAN; - _manual.aux1 = NAN; - _manual.aux2 = NAN; - _manual.aux3 = NAN; - _manual.aux4 = NAN; - _manual.aux5 = NAN; - - _manual.signal_lost = true; /* basic r/c parameters */ for (unsigned i = 0; i < _rc_max_chan_count; i++) { @@ -1427,10 +1411,10 @@ Sensors::rc_poll() /* publish manual_control_setpoint topic */ if (_manual_control_pub > 0) { - orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &_manual); + orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual); } else { - _manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &_manual); + _manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual); } /* copy from mapped manual control to control group 3 */ @@ -1439,14 +1423,14 @@ Sensors::rc_poll() actuator_group_3.timestamp = rc_input.timestamp_publication; - actuator_group_3.control[0] = _manual.roll; - actuator_group_3.control[1] = _manual.pitch; - actuator_group_3.control[2] = _manual.yaw; - actuator_group_3.control[3] = _manual.throttle; - actuator_group_3.control[4] = _manual.flaps; - actuator_group_3.control[5] = _manual.aux1; - actuator_group_3.control[6] = _manual.aux2; - actuator_group_3.control[7] = _manual.aux3; + actuator_group_3.control[0] = manual.roll; + actuator_group_3.control[1] = manual.pitch; + actuator_group_3.control[2] = manual.yaw; + actuator_group_3.control[3] = manual.throttle; + actuator_group_3.control[4] = manual.flaps; + actuator_group_3.control[5] = manual.aux1; + actuator_group_3.control[6] = manual.aux2; + actuator_group_3.control[7] = manual.aux3; /* publish actuator_controls_3 topic */ if (_actuator_group_3_pub > 0) { diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index 985d3923f..2b3a337b2 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -61,8 +61,6 @@ typedef enum { struct manual_control_setpoint_s { uint64_t timestamp; - bool signal_lost; /**< control signal lost, should be checked together with topic timeout */ - /** * Any of the channels may not be available and be set to NaN * to indicate that it does not contain valid data. -- cgit v1.2.3 From eb5cd54023c39d3adc266d68e02c1f10e77ac62a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 7 Apr 2014 14:32:37 +0400 Subject: sensors: lost signal detection rewritten to be more clear --- src/modules/sensors/sensors.cpp | 25 +++++++++++++++++-------- 1 file changed, 17 insertions(+), 8 deletions(-) (limited to 'src/modules/sensors') diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index f20c328d3..91bf92da6 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1307,17 +1307,26 @@ Sensors::rc_poll() orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); /* detect RC signal loss */ - bool signal_lost = true; + bool signal_lost; /* check flags and require at least four channels to consider the signal valid */ - if (!(rc_input.rc_lost || rc_input.rc_failsafe || rc_input.channel_count < 4)) { - /* signal looks good, but check for throttle failsafe */ + if (rc_input.rc_lost || rc_input.rc_failsafe || rc_input.channel_count < 4) { + /* signal is lost or no enough channels */ + signal_lost = true; + + } else { + /* signal looks good */ + signal_lost = false; + + /* check throttle failsafe */ int8_t thr_ch = _rc.function[THROTTLE]; - if (_parameters.rc_fs_thr == 0 || thr_ch < 0 || - !((_parameters.rc_fs_thr < _parameters.min[thr_ch] && rc_input.values[thr_ch] < _parameters.rc_fs_thr) || - (_parameters.rc_fs_thr > _parameters.max[thr_ch] && rc_input.values[thr_ch] > _parameters.rc_fs_thr))) { - /* valid signal, throttle failsafe not configured or not triggered */ - signal_lost = false; + if (_parameters.rc_fs_thr > 0 && thr_ch >= 0) { + /* throttle failsafe configured */ + if ((_parameters.rc_fs_thr < _parameters.min[thr_ch] && rc_input.values[thr_ch] < _parameters.rc_fs_thr) || + (_parameters.rc_fs_thr > _parameters.max[thr_ch] && rc_input.values[thr_ch] > _parameters.rc_fs_thr)) { + /* throttle failsafe triggered, signal is lost by receiver */ + signal_lost = true; + } } } -- cgit v1.2.3 From 9f52c4459331bc0fd32764a35387abb2cab88b4a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 7 Apr 2014 14:34:14 +0400 Subject: sensors: use timestamp_last_signal for actuator_group_3 timestamping --- src/modules/sensors/sensors.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/sensors') diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 91bf92da6..ba233dfd0 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1431,7 +1431,7 @@ Sensors::rc_poll() struct actuator_controls_s actuator_group_3; memset(&actuator_group_3, 0 , sizeof(actuator_group_3)); - actuator_group_3.timestamp = rc_input.timestamp_publication; + actuator_group_3.timestamp = rc_input.timestamp_last_signal; actuator_group_3.control[0] = manual.roll; actuator_group_3.control[1] = manual.pitch; -- cgit v1.2.3