From 1c21a631015fa77b60646aded57e295722d4dc83 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 4 Mar 2016 11:47:15 +0100 Subject: [PATCH] Sensors: Support single channel flight mode selection --- src/modules/sensors/sensor_params.c | 31 ++++++++++++++++ src/modules/sensors/sensors.cpp | 57 +++++++++++++++++++---------- 2 files changed, 68 insertions(+), 20 deletions(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 3b892d5108..20c45abd7f 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -1941,6 +1941,37 @@ PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 0); */ PARAM_DEFINE_INT32(RC_MAP_YAW, 0); +/** + * Single channel flight mode selection + * + * If this parameter is non-zero, flight modes are only selected + * by this channel and are assigned to six slots. + * + * @min 0 + * @max 18 + * @group Radio Switches + * @value 0 Unassigned + * @value 1 RC Channel 1 + * @value 2 RC Channel 2 + * @value 3 RC Channel 3 + * @value 4 RC Channel 4 + * @value 5 RC Channel 5 + * @value 6 RC Channel 6 + * @value 7 RC Channel 7 + * @value 8 RC Channel 8 + * @value 9 RC Channel 9 + * @value 10 RC Channel 10 + * @value 11 RC Channel 11 + * @value 12 RC Channel 12 + * @value 13 RC Channel 13 + * @value 14 RC Channel 14 + * @value 15 RC Channel 15 + * @value 16 RC Channel 16 + * @value 17 RC Channel 17 + * @value 18 RC Channel 18 + */ +PARAM_DEFINE_INT32(RC_MAP_FLTMODE, 0); + /** * Mode switch channel mapping. * diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index b79e828b2b..7f9612c11c 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -298,6 +298,8 @@ private: int rc_map_param[rc_parameter_map_s::RC_PARAM_MAP_NCHAN]; + int rc_map_flightmode; + int32_t rc_fails_thr; float rc_assist_th; float rc_auto_th; @@ -364,6 +366,8 @@ private: _parameters struct are not existing because these parameters are never read. */ + param_t rc_map_flightmode; + param_t rc_fails_thr; param_t rc_assist_th; param_t rc_auto_th; @@ -600,6 +604,8 @@ Sensors::Sensors() : _parameter_handles.rc_map_param[i] = param_find(name); } + _parameter_handles.rc_map_flightmode = param_find("RC_MAP_FLTMODE"); + /* RC thresholds */ _parameter_handles.rc_fails_thr = param_find("RC_FAILS_THR"); _parameter_handles.rc_assist_th = param_find("RC_ASSIST_TH"); @@ -790,6 +796,8 @@ Sensors::parameters_update() param_get(_parameter_handles.rc_map_param[i], &(_parameters.rc_map_param[i])); } + param_get(_parameter_handles.rc_map_flightmode, &(_parameters.rc_map_flightmode)); + param_get(_parameter_handles.rc_fails_thr, &(_parameters.rc_fails_thr)); param_get(_parameter_handles.rc_assist_th, &(_parameters.rc_assist_th)); _parameters.rc_assist_inv = (_parameters.rc_assist_th < 0); @@ -1870,8 +1878,7 @@ Sensors::rc_poll() } if (!signal_lost) { - struct manual_control_setpoint_s manual; - memset(&manual, 0 , sizeof(manual)); + struct manual_control_setpoint_s manual = {}; /* fill values in manual_control_setpoint topic only if signal is valid */ manual.timestamp = rc_input.timestamp_last_signal; @@ -1888,24 +1895,34 @@ Sensors::rc_poll() manual.aux4 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_4, -1.0, 1.0); manual.aux5 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_5, -1.0, 1.0); - /* mode switches */ - manual.mode_switch = get_rc_sw3pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_MODE, _parameters.rc_auto_th, - _parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv); - manual.rattitude_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_RATTITUDE, - _parameters.rc_rattitude_th, - _parameters.rc_rattitude_inv); - manual.posctl_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL, _parameters.rc_posctl_th, - _parameters.rc_posctl_inv); - manual.return_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_RETURN, _parameters.rc_return_th, - _parameters.rc_return_inv); - manual.loiter_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_LOITER, _parameters.rc_loiter_th, - _parameters.rc_loiter_inv); - manual.acro_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_ACRO, _parameters.rc_acro_th, - _parameters.rc_acro_inv); - manual.offboard_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD, - _parameters.rc_offboard_th, _parameters.rc_offboard_inv); - manual.kill_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_KILLSWITCH, - _parameters.rc_killswitch_th, _parameters.rc_killswitch_inv); + if (_parameters.rc_map_flightmode > 0) { + + const unsigned slot_min = 1000; + const unsigned slot_max = 2000; + const unsigned num_slots = 6; + + manual.mode_slot = ((_rc.channels[_parameters.rc_map_flightmode - 1] - slot_min) * num_slots) / (slot_max - slot_min); + + } else { + /* mode switches */ + manual.mode_switch = get_rc_sw3pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_MODE, _parameters.rc_auto_th, + _parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv); + manual.rattitude_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_RATTITUDE, + _parameters.rc_rattitude_th, + _parameters.rc_rattitude_inv); + manual.posctl_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL, _parameters.rc_posctl_th, + _parameters.rc_posctl_inv); + manual.return_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_RETURN, _parameters.rc_return_th, + _parameters.rc_return_inv); + manual.loiter_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_LOITER, _parameters.rc_loiter_th, + _parameters.rc_loiter_inv); + manual.acro_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_ACRO, _parameters.rc_acro_th, + _parameters.rc_acro_inv); + manual.offboard_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD, + _parameters.rc_offboard_th, _parameters.rc_offboard_inv); + manual.kill_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_KILLSWITCH, + _parameters.rc_killswitch_th, _parameters.rc_killswitch_inv); + } /* publish manual_control_setpoint topic */ if (_manual_control_pub != nullptr) {