diff options
Diffstat (limited to 'src/modules/sensors/sensors.cpp')
-rw-r--r-- | src/modules/sensors/sensors.cpp | 119 |
1 files changed, 53 insertions, 66 deletions
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 79b53a79c..2c1b1258c 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -267,9 +267,7 @@ private: int rc_map_aux4; int rc_map_aux5; - int rc_fs_ch; - int rc_fs_mode; - float rc_fs_thr; + int32_t rc_fs_thr; float battery_voltage_scaling; float battery_current_scaling; @@ -312,8 +310,6 @@ private: param_t rc_map_aux4; param_t rc_map_aux5; - param_t rc_fs_ch; - param_t rc_fs_mode; param_t rc_fs_thr; param_t battery_voltage_scaling; @@ -539,9 +535,7 @@ Sensors::Sensors() : _parameter_handles.rc_map_aux5 = param_find("RC_MAP_AUX5"); /* RC failsafe */ - _parameter_handles.rc_fs_ch = param_find("RC_FS_CH"); - _parameter_handles.rc_fs_mode = param_find("RC_FS_MODE"); - _parameter_handles.rc_fs_thr = param_find("RC_FS_THR"); + _parameter_handles.rc_fs_thr = param_find("RC_FAILS_THR"); /* gyro offsets */ _parameter_handles.gyro_offset[0] = param_find("SENS_GYRO_XOFF"); @@ -693,8 +687,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_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)); /* update RC function mappings */ @@ -1293,7 +1285,7 @@ Sensors::get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max return value; } } else { - return NAN; + return 0.0f; } } @@ -1325,30 +1317,21 @@ 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; - - _manual.signal_lost = true; + struct rc_input_values rc_input; orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); /* detect RC signal loss */ + bool signal_lost = true; + /* 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.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.signal_lost = true; - } - - } else if (_parameters.rc_fs_mode == 1) { - if (rc_input.values[_parameters.rc_fs_ch - 1] > _parameters.rc_fs_thr) { - _manual.signal_lost = true; - } - } + /* 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))) { + /* valid signal, throttle failsafe not configured or not triggered */ + signal_lost = false; } } @@ -1405,33 +1388,55 @@ Sensors::rc_poll() _rc.chan_count = rc_input.channel_count; _rc.rssi = rc_input.rssi; - _rc.signal_lost = _manual.signal_lost; + _rc.signal_lost = signal_lost; + _rc.timestamp = rc_input.timestamp_last_signal; + + /* publish rc_channels topic even if signal is invalid, for debug */ + if (_rc_pub > 0) { + orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc); + + } else { + _rc_pub = orb_advertise(ORB_ID(rc_channels), &_rc); + } + + if (!signal_lost) { + struct manual_control_setpoint_s manual; + memset(&manual, 0 , sizeof(manual)); - 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; + manual.timestamp = rc_input.timestamp_last_signal; /* limit controls */ - _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); + 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.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); + 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); + + /* publish manual_control_setpoint topic */ + if (_manual_control_pub > 0) { + orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &_manual); + + } else { + _manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &_manual); + } /* copy from mapped manual control to control group 3 */ 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.control[0] = _manual.roll; actuator_group_3.control[1] = _manual.pitch; @@ -1442,7 +1447,7 @@ Sensors::rc_poll() 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 */ + /* publish actuator_controls_3 topic */ if (_actuator_group_3_pub > 0) { orb_publish(ORB_ID(actuator_controls_3), _actuator_group_3_pub, &actuator_group_3); @@ -1450,25 +1455,7 @@ Sensors::rc_poll() _actuator_group_3_pub = orb_advertise(ORB_ID(actuator_controls_3), &actuator_group_3); } } - - /* publish rc_channels topic even if signal is invalid, for debug */ - if (_rc_pub > 0) { - orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc); - - } else { - /* advertise the rc topic */ - _rc_pub = orb_advertise(ORB_ID(rc_channels), &_rc); - } - - /* 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); - - } else { - _manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &_manual); - } } - } void |