manual_contol_setpoint: fix mode slot numbering (PX4#12578)

* This fix is necessary because usually uORB structs get initialized with
all zeros and then get filled with the fields that are actually in use.
 * The number 0 for the mode slot was already commanding to switch to the
mode in slot one even though for example the joystick input via mavlink
does not use this mechanism at all.

msg/manual_control_setpoint.msg

@@ -4,14 +4,14 @@ uint8 SWITCH_POS_NONE = 0		# switch is not mapped
 uint8 SWITCH_POS_ON = 1		# switch activated (value = 1)
 uint8 SWITCH_POS_MIDDLE = 2		# middle position (value = 0)
 uint8 SWITCH_POS_OFF = 3		# switch not activated (value = -1)
-int8 MODE_SLOT_NONE = -1		# no mode slot assigned
-int8 MODE_SLOT_1 = 0			# mode slot 1 selected
-int8 MODE_SLOT_2 = 1			# mode slot 2 selected
-int8 MODE_SLOT_3 = 2			# mode slot 3 selected
-int8 MODE_SLOT_4 = 3			# mode slot 4 selected
-int8 MODE_SLOT_5 = 4			# mode slot 5 selected
-int8 MODE_SLOT_6 = 5			# mode slot 6 selected
-int8 MODE_SLOT_MAX = 6			# number of slots plus one
+uint8 MODE_SLOT_NONE = 0		# no mode slot assigned
+uint8 MODE_SLOT_1 = 1			# mode slot 1 selected
+uint8 MODE_SLOT_2 = 2			# mode slot 2 selected
+uint8 MODE_SLOT_3 = 3			# mode slot 3 selected
+uint8 MODE_SLOT_4 = 4			# mode slot 4 selected
+uint8 MODE_SLOT_5 = 5			# mode slot 5 selected
+uint8 MODE_SLOT_6 = 6			# mode slot 6 selected
+uint8 MODE_SLOT_NUM = 6			# number of slots
 uint8 SOURCE_RC = 1			# radio control
 uint8 SOURCE_MAVLINK_0 = 2		# mavlink instance 0
 uint8 SOURCE_MAVLINK_1 = 3		# mavlink instance 1
@@ -61,7 +61,7 @@ uint8 kill_switch		 # throttle kill: _NORMAL_, KILL
 uint8 arm_switch		 # arm/disarm switch: _DISARMED_, ARMED
 uint8 transition_switch	 # VTOL transition switch: _HOVER, FORWARD_FLIGHT
 uint8 gear_switch	 # landing gear switch: _DOWN_, UP
-int8 mode_slot			 # the slot a specific model selector is in
+uint8 mode_slot			 # the slot a specific model selector is in
 uint8 data_source		 # where this input is coming from
 uint8 stab_switch 		 # stabilize switch (only relevant for fixed wings, optional): _MANUAL, STABILIZED
 uint8 man_switch		 # manual switch (only relevant for fixed wings, optional): _STABILIZED_, MANUAL

src/modules/commander/Commander.cpp

@@ -139,7 +139,7 @@ static float min_stick_change = 0.25f;
 static struct vehicle_status_s status = {};
 static struct actuator_armed_s armed = {};
 static struct safety_s safety = {};
-static int32_t _flight_mode_slots[manual_control_setpoint_s::MODE_SLOT_MAX];
+static int32_t _flight_mode_slots[manual_control_setpoint_s::MODE_SLOT_NUM];
 static struct commander_state_s internal_state = {};
 
 static uint8_t main_state_before_rtl = commander_state_s::MAIN_STATE_MAX;
@@ -2696,12 +2696,12 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed
 	/* we know something has changed - check if we are in mode slot operation */
 	if (sp_man.mode_slot != manual_control_setpoint_s::MODE_SLOT_NONE) {
 
-		if (sp_man.mode_slot >= (int)(sizeof(_flight_mode_slots) / sizeof(_flight_mode_slots[0]))) {
+		if (sp_man.mode_slot > manual_control_setpoint_s::MODE_SLOT_NUM) {
 			warnx("m slot overflow");
 			return TRANSITION_DENIED;
 		}
 
-		int new_mode = _flight_mode_slots[sp_man.mode_slot];
+		int new_mode = _flight_mode_slots[sp_man.mode_slot - 1];
 
 		if (new_mode < 0) {
 			/* slot is unused */

src/modules/sensors/rc_update.cpp

@@ -350,9 +350,8 @@ RCUpdate::rc_poll(const ParameterHandles &parameter_handles)
 			manual.z = math::constrain(_filter_throttle.apply(manual.z), 0.f, 1.f);
 
 			if (_parameters.rc_map_flightmode > 0) {
-
-				/* the number of valid slots equals the index of the max marker minus one */
-				const int num_slots = manual_control_setpoint_s::MODE_SLOT_MAX;
+				/* number of valid slots */
+				const int num_slots = manual_control_setpoint_s::MODE_SLOT_NUM;
 
 				/* the half width of the range of a slot is the total range
 				 * divided by the number of slots, again divided by two
@@ -369,10 +368,10 @@ RCUpdate::rc_poll(const ParameterHandles &parameter_handles)
 				 * will take us to the correct final index.
 				 */
 				manual.mode_slot = (((((_rc.channels[_parameters.rc_map_flightmode - 1] - slot_min) * num_slots) + slot_width_half) /
-						     (slot_max - slot_min)) + (1.0f / num_slots));
+						     (slot_max - slot_min)) + (1.0f / num_slots)) + 1;
 
-				if (manual.mode_slot >= num_slots) {
-					manual.mode_slot = num_slots - 1;
+				if (manual.mode_slot > num_slots) {
+					manual.mode_slot = num_slots;
 				}
 			}