rc_update: introduce support for toggle buttons via RC channels

Signed-off-by: Claudio Micheli <claudio@auterion.com>

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

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
  *

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 &timestamp_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());

src/modules/rc_update/rc_update.h

@@ -59,6 +59,7 @@
 #include <uORB/topics/rc_channels.h>
 #include <uORB/topics/rc_parameter_map.h>
 #include <uORB/topics/parameter_update.h>
+#include <hysteresis/hysteresis.h>
 
 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<px4::params::RC_MAP_GEAR_SW>) _param_rc_map_gear_sw,
 		(ParamInt<px4::params::RC_MAP_STAB_SW>) _param_rc_map_stab_sw,
 		(ParamInt<px4::params::RC_MAP_MAN_SW>) _param_rc_map_man_sw,
+		(ParamInt<px4::params::RC_MAP_FLTM_BTN>) _param_rc_map_flightmode_buttons,
 
 		(ParamInt<px4::params::RC_MAP_AUX1>) _param_rc_map_aux1,
 		(ParamInt<px4::params::RC_MAP_AUX2>) _param_rc_map_aux2,