Commander: Fix single channel mode switching

src/modules/commander/commander.cpp

@@ -2988,71 +2988,69 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
 			res = main_state_transition(status_local, new_mode, main_state_prev, &status_flags, &internal_state);
 
 			/* enable the use of break */
-			do {
+			/* fallback strategies, give the user the closest mode to what he wanted */
-				/* fallback strategies, give the user the closest mode to what he wanted */
+			while (res == TRANSITION_DENIED) {
-				if (res == TRANSITION_DENIED) {
 
-					if (new_mode == commander_state_s::MAIN_STATE_AUTO_MISSION) {
+				if (new_mode == commander_state_s::MAIN_STATE_AUTO_MISSION) {
-						res = main_state_transition(status_local, new_mode, main_state_prev, &status_flags, &internal_state);
 
-						if (res != TRANSITION_DENIED) {
+					/* fall back to loiter */
-							break;
+					new_mode = commander_state_s::MAIN_STATE_AUTO_LOITER;
-						} else {
+					print_reject_mode(status_local, "AUTO MISSION");
-							/* fall back to loiter */
+					res = main_state_transition(status_local, new_mode, main_state_prev, &status_flags, &internal_state);
-							new_mode = commander_state_s::MAIN_STATE_AUTO_LOITER;
-							print_reject_mode(status_local, "AUTO MISSION");
-						}
-					}
 
-					if (new_mode == commander_state_s::MAIN_STATE_AUTO_LOITER) {
+					if (res != TRANSITION_DENIED) {
-						res = main_state_transition(status_local, new_mode, main_state_prev, &status_flags, &internal_state);
+						break;
-
-						if (res != TRANSITION_DENIED) {
-							break;
-						} else {
-							/* fall back to position control */
-							new_mode = commander_state_s::MAIN_STATE_POSCTL;
-							print_reject_mode(status_local, "AUTO PAUSE");
-						}
-					}
-
-					if (new_mode == commander_state_s::MAIN_STATE_POSCTL) {
-						res = main_state_transition(status_local, new_mode, main_state_prev, &status_flags, &internal_state);
-
-						if (res != TRANSITION_DENIED) {
-							break;
-						} else {
-							/* fall back to altitude control */
-							new_mode = commander_state_s::MAIN_STATE_ALTCTL;
-							print_reject_mode(status_local, "POSITION CONTROL");
-						}
-					}
-
-					if (new_mode == commander_state_s::MAIN_STATE_ALTCTL) {
-						res = main_state_transition(status_local, new_mode, main_state_prev, &status_flags, &internal_state);
-
-						if (res != TRANSITION_DENIED) {
-							break;
-						} else {
-							/* fall back to stabilized */
-							new_mode = commander_state_s::MAIN_STATE_STAB;
-							print_reject_mode(status_local, "ALTITUDE CONTROL");
-						}
-					}
-
-					if (new_mode == commander_state_s::MAIN_STATE_STAB) {
-						res = main_state_transition(status_local, new_mode, main_state_prev, &status_flags, &internal_state);
-
-						if (res != TRANSITION_DENIED) {
-							break;
-						} else {
-							/* there is no decent fallback any more, stay in mode and emit a warning */
-							print_reject_mode(status_local, "STABILIZED");
-						}
 					}
 				}
-			} while (0);
 
+				if (new_mode == commander_state_s::MAIN_STATE_AUTO_LOITER) {
+
+					/* fall back to position control */
+					new_mode = commander_state_s::MAIN_STATE_POSCTL;
+					print_reject_mode(status_local, "AUTO PAUSE");
+					res = main_state_transition(status_local, new_mode, main_state_prev, &status_flags, &internal_state);
+
+					if (res != TRANSITION_DENIED) {
+						break;
+					}
+				}
+
+				if (new_mode == commander_state_s::MAIN_STATE_POSCTL) {
+
+					/* fall back to altitude control */
+					new_mode = commander_state_s::MAIN_STATE_ALTCTL;
+					print_reject_mode(status_local, "POSITION CONTROL");
+					res = main_state_transition(status_local, new_mode, main_state_prev, &status_flags, &internal_state);
+
+					if (res != TRANSITION_DENIED) {
+						break;
+					}
+				}
+
+				if (new_mode == commander_state_s::MAIN_STATE_ALTCTL) {
+
+					/* fall back to stabilized */
+					new_mode = commander_state_s::MAIN_STATE_STAB;
+					print_reject_mode(status_local, "ALTITUDE CONTROL");
+					res = main_state_transition(status_local, new_mode, main_state_prev, &status_flags, &internal_state);
+
+					if (res != TRANSITION_DENIED) {
+						break;
+					}
+				}
+
+				if (new_mode == commander_state_s::MAIN_STATE_STAB) {
+
+					/* fall back to manual */
+					new_mode = commander_state_s::MAIN_STATE_MANUAL;
+					print_reject_mode(status_local, "STABILIZED");
+					res = main_state_transition(status_local, new_mode, main_state_prev, &status_flags, &internal_state);
+
+					if (res != TRANSITION_DENIED) {
+						break;
+					}
+				}
+			}
 		}
 
 		return res;

src/modules/commander/commander_params.c

@@ -279,6 +279,7 @@ PARAM_DEFINE_INT32(COM_DISARM_LAND, 0);
  * @value 7 Offboard
  * @value 8 Stabilized
  * @value 9 Rattitude
+ * @value 12 Follow Me
  */
 PARAM_DEFINE_INT32(COM_FLTMODE1, -1);
 
@@ -301,6 +302,7 @@ PARAM_DEFINE_INT32(COM_FLTMODE1, -1);
  * @value 7 Offboard
  * @value 8 Stabilized
  * @value 9 Rattitude
+ * @value 12 Follow Me
  */
 PARAM_DEFINE_INT32(COM_FLTMODE2, -1);
 
@@ -323,6 +325,7 @@ PARAM_DEFINE_INT32(COM_FLTMODE2, -1);
  * @value 7 Offboard
  * @value 8 Stabilized
  * @value 9 Rattitude
+ * @value 12 Follow Me
  */
 PARAM_DEFINE_INT32(COM_FLTMODE3, -1);
 
@@ -345,6 +348,7 @@ PARAM_DEFINE_INT32(COM_FLTMODE3, -1);
  * @value 7 Offboard
  * @value 8 Stabilized
  * @value 9 Rattitude
+ * @value 12 Follow Me
  */
 PARAM_DEFINE_INT32(COM_FLTMODE4, -1);
 
@@ -367,6 +371,7 @@ PARAM_DEFINE_INT32(COM_FLTMODE4, -1);
  * @value 7 Offboard
  * @value 8 Stabilized
  * @value 9 Rattitude
+ * @value 12 Follow Me
  */
 PARAM_DEFINE_INT32(COM_FLTMODE5, -1);
 
@@ -389,5 +394,6 @@ PARAM_DEFINE_INT32(COM_FLTMODE5, -1);
  * @value 7 Offboard
  * @value 8 Stabilized
  * @value 9 Rattitude
+ * @value 12 Follow Me
  */
 PARAM_DEFINE_INT32(COM_FLTMODE6, -1);