diff --git a/msg/rc_channels.msg b/msg/rc_channels.msg index 024b5d5deb..7892cb49e9 100644 --- a/msg/rc_channels.msg +++ b/msg/rc_channels.msg @@ -26,11 +26,18 @@ uint8 FUNCTION_ARMSWITCH = 22 uint8 FUNCTION_STAB = 23 uint8 FUNCTION_AUX_6 = 24 uint8 FUNCTION_MAN = 25 +uint8 FUNCTION_FLTBTN_SLOT_1=26 +uint8 FUNCTION_FLTBTN_SLOT_2=27 +uint8 FUNCTION_FLTBTN_SLOT_3=28 +uint8 FUNCTION_FLTBTN_SLOT_4=39 +uint8 FUNCTION_FLTBTN_SLOT_5=30 +uint8 FUNCTION_FLTBTN_SLOT_6=31 +uint8 FUNCTION_FLTBTN_NUM_SLOTS=6 uint64 timestamp_last_valid # Timestamp of last valid RC signal float32[18] channels # Scaled to -1..1 (throttle: 0..1) uint8 channel_count # Number of valid channels -int8[26] function # Functions mapping +int8[32] function # Functions mapping uint8 rssi # Receive signal strength index bool signal_lost # Control signal lost, should be checked together with topic timeout uint32 frame_drop_count # Number of dropped frames diff --git a/src/modules/rc_update/params.c b/src/modules/rc_update/params.c index 3a5f41246d..34209dce1c 100644 --- a/src/modules/rc_update/params.c +++ b/src/modules/rc_update/params.c @@ -1701,6 +1701,42 @@ PARAM_DEFINE_INT32(RC_MAP_STAB_SW, 0); */ PARAM_DEFINE_INT32(RC_MAP_MAN_SW, 0); +/** + * Enables changing flight modes with multiple toggle buttons. + * + * This bitmask allows to specify multiple channels for changing flight modes. + * Each channel is assigned to a flight mode slot ((lowest channel = slot 1), + * the behavior of each flight mode is defined by the COM_FLTMODE1,COM_FLTMODE2,... + * parameters. + * The functionality can be used only if RC_MAP_FLTMODE is disabled. + * + * The maximum number of available slots is 6. + * @min 0 + * @max 258048 + * @group Radio Switches + * @bit 0 Enable Channel 1 as toggle button + * @bit 1 Enable Channel 2 as toggle button + * @bit 2 Enable Channel 3 as toggle button + * @bit 3 Enable Channel 4 as toggle button + * @bit 4 Enable Channel 5 as toggle button + * @bit 5 Enable Channel 6 as toggle button + * @bit 6 Enable Channel 7 as toggle button + * @bit 7 Enable Channel 8 as toggle button + * @bit 8 Enable Channel 9 as toggle button + * @bit 9 Enable Channel 10 as toggle button + * @bit 10 Enable Channel 11 as toggle button + * @bit 11 Enable Channel 12 as toggle button + * @bit 12 Enable Channel 13 as toggle button + * @bit 13 Enable Channel 14 as toggle button + * @bit 14 Enable Channel 15 as toggle button + * @bit 15 Enable Channel 16 as toggle button + * @bit 16 Enable Channel 17 as toggle button + * @bit 17 Enable Channel 18 as toggle button + * + */ + +PARAM_DEFINE_INT32(RC_MAP_FLTM_BTN, 0); + /** * AUX1 Passthrough RC channel * diff --git a/src/modules/rc_update/rc_update.cpp b/src/modules/rc_update/rc_update.cpp index 7e80417a4f..9118efdb10 100644 --- a/src/modules/rc_update/rc_update.cpp +++ b/src/modules/rc_update/rc_update.cpp @@ -202,6 +202,8 @@ void RCUpdate::update_rc_functions() for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) { _rc.function[rc_channels_s::FUNCTION_PARAM_1 + i] = _parameters.rc_map_param[i] - 1; } + + map_flight_modes_buttons(); } void RCUpdate::rc_parameter_map_poll(bool forced) @@ -276,6 +278,42 @@ void RCUpdate::set_params_from_rc() } } +void +RCUpdate::map_flight_modes_buttons() +{ + static_assert(rc_channels_s::FUNCTION_FLTBTN_SLOT_1 + manual_control_switches_s::MODE_SLOT_NUM <= sizeof( + _rc.function) / sizeof(_rc.function[0]), "Unexpected number of RC functions"); + static_assert(rc_channels_s::FUNCTION_FLTBTN_NUM_SLOTS == manual_control_switches_s::MODE_SLOT_NUM, + "Unexpected number of Flight Modes slots"); + + // Reset all the slots to -1 + for (uint8_t index = 0; index < manual_control_switches_s::MODE_SLOT_NUM; index++) { + _rc.function[rc_channels_s::FUNCTION_FLTBTN_SLOT_1 + index] = -1; + } + + // If the functionality is disabled we can early return, no need to map channels + int buttons_rc_channels = _param_rc_map_flightmode_buttons.get(); + + if (buttons_rc_channels == 0) { + return; + } + + uint8_t slot_counter = 0; + + for (uint8_t index = 0; index < RC_MAX_CHAN_COUNT; index++) { + if (buttons_rc_channels & (1 << index)) { + PX4_DEBUG("Slot %d assigned to channel %d", slot_counter + 1, index); + _rc.function[rc_channels_s::FUNCTION_FLTBTN_SLOT_1 + slot_counter] = index; + slot_counter++; + } + + if (slot_counter == manual_control_switches_s::MODE_SLOT_NUM) { + // we have filled all the available slots + break; + } + } +} + void RCUpdate::Run() { if (should_exit()) { @@ -427,9 +465,18 @@ void RCUpdate::Run() } } + /* + * some RC systems glitch after a reboot, we should ignore the first 100ms of regained signal + * as the glitch might be interpreted as a commanded stick action or a flight mode switch + * + */ + + _rc_signal_lost_hysteresis.set_hysteresis_time_from(true, 100_ms); + _rc_signal_lost_hysteresis.set_state_and_update(signal_lost, hrt_absolute_time()); + _rc.channel_count = input_rc.channel_count; _rc.rssi = input_rc.rssi; - _rc.signal_lost = signal_lost; + _rc.signal_lost = _rc_signal_lost_hysteresis.get_state(); _rc.timestamp = input_rc.timestamp_last_signal; _rc.frame_drop_count = input_rc.rc_lost_frame_count; @@ -437,7 +484,7 @@ void RCUpdate::Run() _rc_channels_pub.publish(_rc); // only publish manual control if the signal is present and regularly updating - if (input_source_stable && channel_count_stable && !signal_lost) { + if (input_source_stable && channel_count_stable && !_rc_signal_lost_hysteresis.get_state()) { if ((input_rc.timestamp_last_signal > _last_timestamp_signal) && (input_rc.timestamp_last_signal - _last_timestamp_signal < 1_s)) { @@ -557,6 +604,37 @@ void RCUpdate::UpdateManualSwitches(const hrt_abstime ×tamp_sample) switches.acro_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_ACRO, _param_rc_acro_th.get()); switches.stab_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_STAB, _param_rc_stab_th.get()); switches.posctl_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_POSCTL, _param_rc_posctl_th.get()); + + } else if (_param_rc_map_flightmode_buttons.get() > 0) { + switches.mode_slot = manual_control_switches_s::MODE_SLOT_NONE; + bool is_consistent_button_press = false; + + for (uint8_t index = 0; index < manual_control_switches_s::MODE_SLOT_NUM; index++) { + + // If the slot is not in use (-1), get_rc_value() will return 0 + float value = get_rc_value(rc_channels_s::FUNCTION_FLTBTN_SLOT_1 + index, -1.0, 1.0); + + // The range goes from -1 to 1, checking that value is greater than 0.5f + // corresponds to check that the signal is above 75% of the overall range. + if (value > 0.5f) { + const uint8_t current_button_press_slot = index + 1; + + // The same button stays pressed consistently + if (current_button_press_slot == _potential_button_press_slot) { + is_consistent_button_press = true; + } + + _potential_button_press_slot = current_button_press_slot; + break; + } + } + + _button_pressed_hysteresis.set_hysteresis_time_from(false, 50_ms); + _button_pressed_hysteresis.set_state_and_update(is_consistent_button_press, hrt_absolute_time()); + + if (_button_pressed_hysteresis.get_state()) { + switches.mode_slot = _potential_button_press_slot; + } } switches.return_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_RETURN, _param_rc_return_th.get()); diff --git a/src/modules/rc_update/rc_update.h b/src/modules/rc_update/rc_update.h index 93324cbdb8..ef6de3a813 100644 --- a/src/modules/rc_update/rc_update.h +++ b/src/modules/rc_update/rc_update.h @@ -59,6 +59,7 @@ #include #include #include +#include using namespace time_literals; @@ -130,6 +131,8 @@ private: */ void set_params_from_rc(); + void map_flight_modes_buttons(); + static constexpr uint8_t RC_MAX_CHAN_COUNT{input_rc_s::RC_INPUT_MAX_CHANNELS}; /**< maximum number of r/c channels we handle */ struct Parameters { @@ -186,6 +189,10 @@ private: uint8_t _channel_count_previous{0}; uint8_t _input_source_previous{input_rc_s::RC_INPUT_SOURCE_UNKNOWN}; + uint8_t _potential_button_press_slot{0}; + systemlib::Hysteresis _button_pressed_hysteresis{false}; + systemlib::Hysteresis _rc_signal_lost_hysteresis{true}; + uint8_t _channel_count_max{0}; perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; @@ -216,6 +223,7 @@ private: (ParamInt) _param_rc_map_gear_sw, (ParamInt) _param_rc_map_stab_sw, (ParamInt) _param_rc_map_man_sw, + (ParamInt) _param_rc_map_flightmode_buttons, (ParamInt) _param_rc_map_aux1, (ParamInt) _param_rc_map_aux2,