commander: centralize main_state strings and simplify main state change attempts

* commander: centralize main_state strings and simplify main state change attempts

src/modules/commander/Commander.cpp

@@ -435,6 +435,41 @@ static constexpr const char *arm_disarm_reason_str(arm_disarm_reason_t calling_r
 	return "";
 };
 
+static constexpr const char *main_state_str(uint8_t main_state)
+{
+	switch (main_state) {
+	case commander_state_s::MAIN_STATE_MANUAL: return "Manual";
+
+	case commander_state_s::MAIN_STATE_ALTCTL: return "Altitude";
+
+	case commander_state_s::MAIN_STATE_POSCTL: return "Position";
+
+	case commander_state_s::MAIN_STATE_AUTO_MISSION: return "Mission";
+
+	case commander_state_s::MAIN_STATE_AUTO_LOITER: return "Hold";
+
+	case commander_state_s::MAIN_STATE_AUTO_RTL: return "RTL";
+
+	case commander_state_s::MAIN_STATE_ACRO: return "Acro";
+
+	case commander_state_s::MAIN_STATE_OFFBOARD: return "Offboard";
+
+	case commander_state_s::MAIN_STATE_STAB: return "Stabilized";
+
+	case commander_state_s::MAIN_STATE_AUTO_TAKEOFF: return "Takeoff";
+
+	case commander_state_s::MAIN_STATE_AUTO_LAND: return "Land";
+
+	case commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET: return "Follow target";
+
+	case commander_state_s::MAIN_STATE_AUTO_PRECLAND: return "Precision land";
+
+	case commander_state_s::MAIN_STATE_ORBIT: return "Orbit";
+
+	default: return "Unknown";
+	}
+}
+
 transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_preflight_checks)
 {
 	// allow a grace period for re-arming: preflight checks don't need to pass during that time, for example for accidential in-air disarming
@@ -513,102 +548,66 @@ transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason)
 }
 
 transition_result_t
-Commander::try_mode_change(main_state_t desired_mode, const FlightModeChange enable_fallback)
+Commander::try_mode_change(main_state_t desired_mode)
 {
 	transition_result_t res = main_state_transition(_status, desired_mode, _status_flags, &_internal_state);
 
 	if (res == TRANSITION_DENIED) {
 
+		print_reject_mode(desired_mode);
+
 		if (desired_mode == commander_state_s::MAIN_STATE_OFFBOARD) {
 			/* offboard does not have a fallback */
-			print_reject_mode("Offboard");
-
 			return res;
 		}
 
 		if (desired_mode == commander_state_s::MAIN_STATE_AUTO_MISSION) {
-			print_reject_mode("Auto Mission");
+			desired_mode = commander_state_s::MAIN_STATE_AUTO_LOITER;
-
+			res = main_state_transition(_status, desired_mode, _status_flags, &_internal_state);
-			if (enable_fallback == FlightModeChange::FallbackEnabled) {
-				desired_mode = commander_state_s::MAIN_STATE_AUTO_LOITER;
-				res = main_state_transition(_status, desired_mode, _status_flags, &_internal_state);
-			}
 		}
 
 		if (desired_mode == commander_state_s::MAIN_STATE_AUTO_RTL && (res == TRANSITION_DENIED)) {
-			print_reject_mode("Auto RTL");
+			desired_mode = commander_state_s::MAIN_STATE_AUTO_LOITER;
-
+			res = main_state_transition(_status, desired_mode, _status_flags, &_internal_state);
-			if (enable_fallback == FlightModeChange::FallbackEnabled) {
-				desired_mode = commander_state_s::MAIN_STATE_AUTO_LOITER;
-				res = main_state_transition(_status, desired_mode, _status_flags, &_internal_state);
-			}
 		}
 
 		if (desired_mode == commander_state_s::MAIN_STATE_AUTO_LAND && (res == TRANSITION_DENIED)) {
-			print_reject_mode("Auto Land");
+			desired_mode = commander_state_s::MAIN_STATE_AUTO_LOITER;
-
+			res = main_state_transition(_status, desired_mode, _status_flags, &_internal_state);
-			if (enable_fallback == FlightModeChange::FallbackEnabled) {
-				desired_mode = commander_state_s::MAIN_STATE_AUTO_LOITER;
-				res = main_state_transition(_status, desired_mode, _status_flags, &_internal_state);
-			}
 		}
 
 		if (desired_mode == commander_state_s::MAIN_STATE_AUTO_TAKEOFF && (res == TRANSITION_DENIED)) {
-			print_reject_mode("Auto Takeoff");
+			desired_mode = commander_state_s::MAIN_STATE_AUTO_LOITER;
-
+			res = main_state_transition(_status, desired_mode, _status_flags, &_internal_state);
-			if (enable_fallback == FlightModeChange::FallbackEnabled) {
-				desired_mode = commander_state_s::MAIN_STATE_AUTO_LOITER;
-				res = main_state_transition(_status, desired_mode, _status_flags, &_internal_state);
-			}
 		}
 
 		if (desired_mode == commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET && (res == TRANSITION_DENIED)) {
-			print_reject_mode("Auto Follow");
+			desired_mode = commander_state_s::MAIN_STATE_AUTO_LOITER;
-
+			res = main_state_transition(_status, desired_mode, _status_flags, &_internal_state);
-			if (enable_fallback == FlightModeChange::FallbackEnabled) {
-				desired_mode = commander_state_s::MAIN_STATE_AUTO_LOITER;
-				res = main_state_transition(_status, desired_mode, _status_flags, &_internal_state);
-			}
 		}
 
 		if (desired_mode == commander_state_s::MAIN_STATE_AUTO_LOITER && (res == TRANSITION_DENIED)) {
-			print_reject_mode("Auto Hold");
+			/* fall back to position control */
-
+			desired_mode = commander_state_s::MAIN_STATE_POSCTL;
-			if (enable_fallback == FlightModeChange::FallbackEnabled) {
+			res = main_state_transition(_status, desired_mode, _status_flags, &_internal_state);
-				/* fall back to position control */
-				desired_mode = commander_state_s::MAIN_STATE_POSCTL;
-				res = main_state_transition(_status, desired_mode, _status_flags, &_internal_state);
-			}
 		}
 
 		if (desired_mode == commander_state_s::MAIN_STATE_POSCTL && (res == TRANSITION_DENIED)) {
-			print_reject_mode("Position");
+			/* fall back to altitude control */
-
+			desired_mode = commander_state_s::MAIN_STATE_ALTCTL;
-			if (enable_fallback == FlightModeChange::FallbackEnabled) {
+			res = main_state_transition(_status, desired_mode, _status_flags, &_internal_state);
-				/* fall back to altitude control */
-				desired_mode = commander_state_s::MAIN_STATE_ALTCTL;
-				res = main_state_transition(_status, desired_mode, _status_flags, &_internal_state);
-			}
 		}
 
 		if (desired_mode == commander_state_s::MAIN_STATE_ALTCTL && (res == TRANSITION_DENIED)) {
-			print_reject_mode("Altitude");
+			/* fall back to stabilized */
-
+			desired_mode = commander_state_s::MAIN_STATE_STAB;
-			if (enable_fallback == FlightModeChange::FallbackEnabled) {
+			res = main_state_transition(_status, desired_mode, _status_flags, &_internal_state);
-				/* fall back to stabilized */
-				desired_mode = commander_state_s::MAIN_STATE_STAB;
-				res = main_state_transition(_status, desired_mode, _status_flags, &_internal_state);
-			}
 		}
 
 		if (desired_mode == commander_state_s::MAIN_STATE_STAB && (res == TRANSITION_DENIED)) {
-			print_reject_mode("Stabilized");
+			/* fall back to manual */
-
+			desired_mode = commander_state_s::MAIN_STATE_MANUAL;
-			if (enable_fallback == FlightModeChange::FallbackEnabled) {
+			res = main_state_transition(_status, desired_mode, _status_flags, &_internal_state);
-				/* fall back to manual */
-				desired_mode = commander_state_s::MAIN_STATE_MANUAL;
-				res = main_state_transition(_status, desired_mode, _status_flags, &_internal_state);
-			}
 		}
 	}
 
@@ -2243,11 +2242,9 @@ Commander::run()
 				}
 			}
 
-			if (_manual_control.isMavlink()) {
+			if (!_armed.armed && _manual_control.isMavlink() && (_internal_state.main_state_changes == 0)) {
 				// if there's never been a mode change force position control as initial state
-				if (!_armed.armed && (_internal_state.main_state_changes == 0)) {
+				_internal_state.main_state = commander_state_s::MAIN_STATE_POSCTL;
-					_internal_state.main_state = commander_state_s::MAIN_STATE_POSCTL;
-				}
 			}
 
 			if (_manual_control_switches_sub.update(&_manual_control_switches) || safety_updated) {
@@ -2942,9 +2939,13 @@ Commander::set_main_state_rc()
 
 	/* offboard switch overrides main switch */
 	if (_manual_control_switches.offboard_switch == manual_control_switches_s::SWITCH_POS_ON) {
-		res = try_mode_change(commander_state_s::MAIN_STATE_OFFBOARD, FlightModeChange::FallbackDisabled);
+		res = main_state_transition(_status, commander_state_s::MAIN_STATE_OFFBOARD, _status_flags, &_internal_state);
 
-		if (res != TRANSITION_DENIED) {
+		if (res == TRANSITION_DENIED) {
+			print_reject_mode(commander_state_s::MAIN_STATE_OFFBOARD);
+			/* mode rejected, continue to evaluate the main system mode */
+
+		} else {
 			/* changed successfully or already in this state */
 			return res;
 		}
@@ -2952,8 +2953,7 @@ Commander::set_main_state_rc()
 
 	/* RTL switch overrides main switch */
 	if (_manual_control_switches.return_switch == manual_control_switches_s::SWITCH_POS_ON) {
-
+		res = try_mode_change(commander_state_s::MAIN_STATE_AUTO_RTL);
-		res = try_mode_change(commander_state_s::MAIN_STATE_AUTO_RTL, FlightModeChange::FallbackEnabled);
 
 		if (res != TRANSITION_DENIED) {
 			/* changed successfully or already in this state */
@@ -2965,9 +2965,14 @@ Commander::set_main_state_rc()
 
 	/* Loiter switch overrides main switch */
 	if (_manual_control_switches.loiter_switch == manual_control_switches_s::SWITCH_POS_ON) {
-		res = try_mode_change(commander_state_s::MAIN_STATE_AUTO_LOITER, FlightModeChange::FallbackDisabled);
+		res = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LOITER, _status_flags, &_internal_state);
 
-		if (res != TRANSITION_DENIED) {
+		if (res == TRANSITION_DENIED) {
+			print_reject_mode(commander_state_s::MAIN_STATE_AUTO_LOITER);
+			/* mode rejected, continue to evaluate the main system mode */
+
+		} else {
+			/* changed successfully or already in this state */
 			return res;
 		}
 	}
@@ -2987,7 +2992,7 @@ Commander::set_main_state_rc()
 			res = TRANSITION_NOT_CHANGED;
 
 		} else {
-			res = try_mode_change(new_mode, FlightModeChange::FallbackEnabled);
+			res = try_mode_change(new_mode);
 		}
 
 		return res;
@@ -3053,31 +3058,16 @@ Commander::set_main_state_rc()
 
 		case manual_control_switches_s::SWITCH_POS_MIDDLE:		// ASSIST
 			if (_manual_control_switches.posctl_switch == manual_control_switches_s::SWITCH_POS_ON) {
-				res = try_mode_change(commander_state_s::MAIN_STATE_POSCTL, FlightModeChange::FallbackDisabled);
+				res = try_mode_change(commander_state_s::MAIN_STATE_POSCTL);
 
-				if (res != TRANSITION_DENIED) {
+			} else {
-					break;	// changed successfully or already in this state
+				res = try_mode_change(commander_state_s::MAIN_STATE_ALTCTL);
-				}
 			}
 
-			// fallback to ALTCTL
-			res = main_state_transition(_status, commander_state_s::MAIN_STATE_ALTCTL, _status_flags, &_internal_state);
-
-			if (res != TRANSITION_DENIED) {
-				break;	// changed successfully or already in this mode
-			}
-
-			if (_manual_control_switches.posctl_switch != manual_control_switches_s::SWITCH_POS_ON) {
-				print_reject_mode("Altitude");
-			}
-
-			// fallback to MANUAL
-			res = main_state_transition(_status, commander_state_s::MAIN_STATE_MANUAL, _status_flags, &_internal_state);
-			// TRANSITION_DENIED is not possible here
 			break;
 
-		case manual_control_switches_s::SWITCH_POS_ON:			// AUTO
+		case manual_control_switches_s::SWITCH_POS_ON:		// AUTO
-			res = try_mode_change(commander_state_s::MAIN_STATE_AUTO_MISSION, FlightModeChange::FallbackEnabled);
+			res = try_mode_change(commander_state_s::MAIN_STATE_AUTO_MISSION);
 			break;
 
 		default:
@@ -3308,17 +3298,17 @@ Commander::stabilization_required()
 }
 
 void
-Commander::print_reject_mode(const char *msg)
+Commander::print_reject_mode(uint8_t main_state)
 {
-	const hrt_abstime t = hrt_absolute_time();
+	if (hrt_elapsed_time(&_last_print_mode_reject_time) > 1_s) {
 
-	if (t - _last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) {
+		mavlink_log_critical(&_mavlink_log_pub, "Switching to %s is currently not available.", main_state_str(main_state));
-		_last_print_mode_reject_time = t;
-		mavlink_log_critical(&_mavlink_log_pub, "Switching to %s is currently not available.", msg);
 
 		/* only buzz if armed, because else we're driving people nuts indoors
 		they really need to look at the leds as well. */
 		tune_negative(_armed.armed);
+
+		_last_print_mode_reject_time = hrt_absolute_time();
 	}
 }
 

src/modules/commander/Commander.hpp

@@ -122,17 +122,11 @@ public:
 	void get_circuit_breaker_params();
 
 private:
-
-	enum class FlightModeChange {
-		FallbackDisabled = 0,
-		FallbackEnabled
-	};
-
 	void answer_command(const vehicle_command_s &cmd, uint8_t result);
 
 	transition_result_t arm(arm_disarm_reason_t calling_reason, bool run_preflight_checks = true);
 	transition_result_t disarm(arm_disarm_reason_t calling_reason);
-	transition_result_t try_mode_change(main_state_t desired_mode, const FlightModeChange enable_fallback);
+	transition_result_t try_mode_change(main_state_t desired_mode);
 
 	void battery_status_check();
 
@@ -161,7 +155,7 @@ private:
 
 	void offboard_control_update();
 
-	void print_reject_mode(const char *msg);
+	void print_reject_mode(uint8_t main_state);
 
 	void reset_posvel_validity();
 
@@ -292,7 +286,6 @@ private:
 	static constexpr uint64_t COMMANDER_MONITORING_INTERVAL{10_ms};
 
 	static constexpr uint64_t HOTPLUG_SENS_TIMEOUT{8_s};	/**< wait for hotplug sensors to come online for upto 8 seconds */
-	static constexpr uint64_t PRINT_MODE_REJECT_INTERVAL{500_ms};
 	static constexpr uint64_t INAIR_RESTART_HOLDOFF_INTERVAL{500_ms};
 
 	const int64_t POSVEL_PROBATION_MIN = 1_s;	/**< minimum probation duration (usec) */