Refactor mode button changes

msg/rc_channels.msg

@@ -1,38 +1,39 @@
 uint64 timestamp						# time since system start (microseconds)
 
-uint8 FUNCTION_THROTTLE   = 0
-uint8 FUNCTION_ROLL       = 1
-uint8 FUNCTION_PITCH      = 2
-uint8 FUNCTION_YAW        = 3
-uint8 FUNCTION_MODE       = 4
-uint8 FUNCTION_RETURN     = 5
-uint8 FUNCTION_POSCTL     = 6
-uint8 FUNCTION_LOITER     = 7
-uint8 FUNCTION_OFFBOARD   = 8
-uint8 FUNCTION_ACRO       = 9
-uint8 FUNCTION_FLAPS      = 10
-uint8 FUNCTION_AUX_1      = 11
-uint8 FUNCTION_AUX_2      = 12
-uint8 FUNCTION_AUX_3      = 13
-uint8 FUNCTION_AUX_4      = 14
-uint8 FUNCTION_AUX_5      = 15
-uint8 FUNCTION_PARAM_1    = 16
-uint8 FUNCTION_PARAM_2    = 17
-uint8 FUNCTION_PARAM_3_5  = 18
-uint8 FUNCTION_KILLSWITCH = 19
-uint8 FUNCTION_TRANSITION = 20
-uint8 FUNCTION_GEAR       = 21
-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
+uint8 FUNCTION_THROTTLE      = 0
+uint8 FUNCTION_ROLL          = 1
+uint8 FUNCTION_PITCH         = 2
+uint8 FUNCTION_YAW           = 3
+uint8 FUNCTION_MODE          = 4
+uint8 FUNCTION_RETURN        = 5
+uint8 FUNCTION_POSCTL        = 6
+uint8 FUNCTION_LOITER        = 7
+uint8 FUNCTION_OFFBOARD      = 8
+uint8 FUNCTION_ACRO          = 9
+uint8 FUNCTION_FLAPS         = 10
+uint8 FUNCTION_AUX_1         = 11
+uint8 FUNCTION_AUX_2         = 12
+uint8 FUNCTION_AUX_3         = 13
+uint8 FUNCTION_AUX_4         = 14
+uint8 FUNCTION_AUX_5         = 15
+uint8 FUNCTION_PARAM_1       = 16
+uint8 FUNCTION_PARAM_2       = 17
+uint8 FUNCTION_PARAM_3_5     = 18
+uint8 FUNCTION_KILLSWITCH    = 19
+uint8 FUNCTION_TRANSITION    = 20
+uint8 FUNCTION_GEAR          = 21
+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_SLOT_COUNT = 6
 
 uint64 timestamp_last_valid					# Timestamp of last valid RC signal
 float32[18] channels						# Scaled to -1..1 (throttle: 0..1)

src/modules/commander/ManualControl.cpp

@@ -179,10 +179,10 @@ bool ManualControl::wantsArm(const vehicle_control_mode_s &vehicle_control_mode,
 
 bool ManualControl::isModeInitializationRequired()
 {
-	const bool isMavlink = _manual_control_setpoint.data_source > manual_control_setpoint_s::SOURCE_RC;
-	const bool rc_uses_toggle_buttons = !isMavlink && _param_rc_map_flightmode_buttons.get() > 0;
+	const bool is_mavlink = _manual_control_setpoint.data_source > manual_control_setpoint_s::SOURCE_RC;
+	const bool rc_uses_toggle_buttons = _param_rc_map_flightmode_buttons.get() > 0;
 
-	return (isMavlink || rc_uses_toggle_buttons);
+	return (is_mavlink || rc_uses_toggle_buttons);
 }
 
 void ManualControl::updateParams()

src/modules/rc_update/params.c

@@ -1702,37 +1702,35 @@ PARAM_DEFINE_INT32(RC_MAP_STAB_SW, 0);
 PARAM_DEFINE_INT32(RC_MAP_MAN_SW, 0);
 
 /**
- * Enables changing flight modes with multiple toggle buttons.
+ * Button flight mode selection
  *
- * 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.
+ * This bitmask allows to specify multiple channels for changing flight modes using
+ * momentary buttons. Each channel is assigned to a mode slot ((lowest channel = slot 1).
+ * The resulting modes for each slot X is defined by the COM_FLTMODEX parameters.
  * The functionality can be used only if RC_MAP_FLTMODE is disabled.
  *
- * The maximum number of available slots is 6.
+ * The maximum number of available slots and hence bits set in the mask 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
- *
+ * @bit 0 Mask Channel 1 as a mode button
+ * @bit 1 Mask Channel 2 as a mode button
+ * @bit 2 Mask Channel 3 as a mode button
+ * @bit 3 Mask Channel 4 as a mode button
+ * @bit 4 Mask Channel 5 as a mode button
+ * @bit 5 Mask Channel 6 as a mode button
+ * @bit 6 Mask Channel 7 as a mode button
+ * @bit 7 Mask Channel 8 as a mode button
+ * @bit 8 Mask Channel 9 as a mode button
+ * @bit 9 Mask Channel 10 as a mode button
+ * @bit 10 Mask Channel 11 as a mode button
+ * @bit 11 Mask Channel 12 as a mode button
+ * @bit 12 Mask Channel 13 as a mode button
+ * @bit 13 Mask Channel 14 as a mode button
+ * @bit 14 Mask Channel 15 as a mode button
+ * @bit 15 Mask Channel 16 as a mode button
+ * @bit 16 Mask Channel 17 as a mode button
+ * @bit 17 Mask Channel 18 as a mode button
  */
 
 PARAM_DEFINE_INT32(RC_MAP_FLTM_BTN, 0);

src/modules/rc_update/rc_update.cpp

@@ -103,8 +103,9 @@ RCUpdate::RCUpdate() :
 	}
 
 	rc_parameter_map_poll(true /* forced */);
-
 	parameters_updated();
+
+	_button_pressed_hysteresis.set_hysteresis_time_from(false, 50_ms);
 }
 
 RCUpdate::~RCUpdate()
@@ -283,31 +284,31 @@ 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,
+	static_assert(rc_channels_s::FUNCTION_FLTBTN_SLOT_COUNT == 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;
+	for (uint8_t slot = 0; slot < manual_control_switches_s::MODE_SLOT_NUM; slot++) {
+		_rc.function[rc_channels_s::FUNCTION_FLTBTN_SLOT_1 + slot] = -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 the functionality is disabled we don't need to map channels
+	const int flightmode_buttons = _param_rc_map_flightmode_buttons.get();
 
-	if (buttons_rc_channels == 0) {
+	if (flightmode_buttons == 0) {
 		return;
 	}
 
-	uint8_t slot_counter = 0;
+	uint8_t slot = 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++;
+	for (uint8_t channel = 0; channel < RC_MAX_CHAN_COUNT; channel++) {
+		if (flightmode_buttons & (1 << channel)) {
+			PX4_DEBUG("Slot %d assigned to channel %d", slot + 1, channel);
+			_rc.function[rc_channels_s::FUNCTION_FLTBTN_SLOT_1 + slot] = channel;
+			slot++;
 		}
 
-		if (slot_counter == manual_control_switches_s::MODE_SLOT_NUM) {
+		if (slot >= manual_control_switches_s::MODE_SLOT_NUM) {
 			// we have filled all the available slots
 			break;
 		}
@@ -468,9 +469,7 @@ 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());
 
@@ -609,15 +608,15 @@ void RCUpdate::UpdateManualSwitches(const hrt_abstime &timestamp_sample)
 		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++) {
+		for (uint8_t slot = 0; slot < manual_control_switches_s::MODE_SLOT_NUM; slot++) {
 
 			// 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);
+			float value = get_rc_value(rc_channels_s::FUNCTION_FLTBTN_SLOT_1 + slot, -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;
+				const uint8_t current_button_press_slot = slot + 1;
 
 				// The same button stays pressed consistently
 				if (current_button_press_slot == _potential_button_press_slot) {
@@ -629,7 +628,6 @@ void RCUpdate::UpdateManualSwitches(const hrt_abstime &timestamp_sample)
 			}
 		}
 
-		_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()) {