commander: modes display fixes, don't activate failsafe while disarmed

4 files changed, 66 insertions, 27 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 43683f833..f8dcec8cb 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -624,22 +624,34 @@ int commander_thread_main(int argc, char *argv[])
 	warnx("starting");
 
 	char *main_states_str[MAIN_STATE_MAX];
-	main_states_str[0] = "MANUAL";
-	main_states_str[1] = "ALTCTL";
-	main_states_str[2] = "POSCTL";
-	main_states_str[3] = "AUTO_MISSION";
-	main_states_str[4] = "AUTO_LOITER";
-	main_states_str[5] = "AUTO_RTL";
-	main_states_str[6] = "ACRO";
+	main_states_str[MAIN_STATE_MANUAL]			= "MANUAL";
+	main_states_str[MAIN_STATE_ALTCTL]			= "ALTCTL";
+	main_states_str[MAIN_STATE_POSCTL]			= "POSCTL";
+	main_states_str[MAIN_STATE_AUTO_MISSION]	= "AUTO_MISSION";
+	main_states_str[MAIN_STATE_AUTO_LOITER]		= "AUTO_LOITER";
+	main_states_str[MAIN_STATE_AUTO_RTL]		= "AUTO_RTL";
+	main_states_str[MAIN_STATE_ACRO]			= "ACRO";
 
 	char *arming_states_str[ARMING_STATE_MAX];
-	arming_states_str[0] = "INIT";
-	arming_states_str[1] = "STANDBY";
-	arming_states_str[2] = "ARMED";
-	arming_states_str[3] = "ARMED_ERROR";
-	arming_states_str[4] = "STANDBY_ERROR";
-	arming_states_str[5] = "REBOOT";
-	arming_states_str[6] = "IN_AIR_RESTORE";
+	arming_states_str[ARMING_STATE_INIT]			= "INIT";
+	arming_states_str[ARMING_STATE_STANDBY]			= "STANDBY";
+	arming_states_str[ARMING_STATE_ARMED]			= "ARMED";
+	arming_states_str[ARMING_STATE_ARMED_ERROR]		= "ARMED_ERROR";
+	arming_states_str[ARMING_STATE_STANDBY_ERROR]	= "STANDBY_ERROR";
+	arming_states_str[ARMING_STATE_REBOOT]			= "REBOOT";
+	arming_states_str[ARMING_STATE_IN_AIR_RESTORE]	= "IN_AIR_RESTORE";
+
+	char *nav_states_str[NAVIGATION_STATE_MAX];
+	nav_states_str[NAVIGATION_STATE_MANUAL]		= "MANUAL";
+	nav_states_str[NAVIGATION_STATE_ALTCTL]		= "ALTCTL";
+	nav_states_str[NAVIGATION_STATE_POSCTL]		= "POSCTL";
+	nav_states_str[NAVIGATION_STATE_AUTO_MISSION]	= "AUTO_MISSION";
+	nav_states_str[NAVIGATION_STATE_AUTO_LOITER]	= "AUTO_LOITER";
+	nav_states_str[NAVIGATION_STATE_AUTO_RTL]		= "AUTO_RTL";
+	nav_states_str[NAVIGATION_STATE_ACRO]			= "ACRO";
+	nav_states_str[NAVIGATION_STATE_LAND]			= "LAND";
+	nav_states_str[NAVIGATION_STATE_DESCEND]		= "DESCEND";
+	nav_states_str[NAVIGATION_STATE_TERMINATION]	= "TERMINATION";
 
 	/* pthread for slow low prio thread */
 	pthread_t commander_low_prio_thread;
@@ -844,7 +856,7 @@ int commander_thread_main(int argc, char *argv[])
 	/* check which state machines for changes, clear "changed" flag */
 	bool arming_state_changed = false;
 	bool main_state_changed = false;
-	bool failsafe_state_changed = false;
+	bool failsafe_old = false;
 
 	while (!thread_should_exit) {
 
@@ -1366,18 +1378,26 @@ int commander_thread_main(int argc, char *argv[])
 		was_armed = armed.armed;
 
 		/* now set navigation state according to failsafe and main state */
-		set_nav_state(&status);
+		bool nav_state_changed = set_nav_state(&status);
 
+		// TODO handle mode changes by commands
 		if (main_state_changed) {
 			status_changed = true;
+			warnx("main state: %s", main_states_str[status.main_state]);
 			mavlink_log_info(mavlink_fd, "[cmd] main state: %s", main_states_str[status.main_state]);
 			main_state_changed = false;
 		}
 
-		if (failsafe_state_changed) {
+		if (status.failsafe != failsafe_old) {
 			status_changed = true;
 			mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %i", status.failsafe);
-			failsafe_state_changed = false;
+			failsafe_old = status.failsafe;
+		}
+
+		if (nav_state_changed) {
+			status_changed = true;
+			warnx("nav state: %s", nav_states_str[status.nav_state]);
+			mavlink_log_info(mavlink_fd, "[cmd] nav state: %s", nav_states_str[status.nav_state]);
 		}
 
 		/* publish states (armed, control mode, vehicle status) at least with 5 Hz */
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index df718ff99..c0d730949 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -366,8 +366,11 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub,
 /**
  * Check failsafe and main status and set navigation status for navigator accordingly
  */
-void set_nav_state(struct vehicle_status_s *status)
+bool set_nav_state(struct vehicle_status_s *status)
 {
+	navigation_state_t nav_state_old = status->nav_state;
+
+	bool armed = (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR);
 	status->failsafe = false;
 
 	/* evaluate main state to decide in normal (non-failsafe) mode */
@@ -377,7 +380,7 @@ void set_nav_state(struct vehicle_status_s *status)
 	case MAIN_STATE_ALTCTL:
 	case MAIN_STATE_POSCTL:
 		/* require RC for all manual modes */
-		if (status->rc_signal_lost) {
+		if (status->rc_signal_lost && armed) {
 			status->failsafe = true;
 
 		} else {
@@ -407,31 +410,44 @@ void set_nav_state(struct vehicle_status_s *status)
 
 	case MAIN_STATE_AUTO_MISSION:
 		/* require data link and global position */
-		if (status->data_link_lost || !status->condition_global_position_valid) {
+		if ((status->data_link_lost || !status->condition_global_position_valid) && armed) {
 			status->failsafe = true;
 
 		} else {
-			status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
+			if (armed) {
+				status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
+
+			} else {
+				// TODO which mode should we set when disarmed?
+				status->nav_state = NAVIGATION_STATE_AUTO_LOITER;
+			}
 		}
 		break;
 
 	case MAIN_STATE_AUTO_LOITER:
 		/* require data link and local position */
-		if (status->data_link_lost || !status->condition_local_position_valid) {
+		if ((status->data_link_lost || !status->condition_local_position_valid) && armed) {
 			status->failsafe = true;
 
 		} else {
+			// TODO which mode should we set when disarmed?
 			status->nav_state = NAVIGATION_STATE_AUTO_LOITER;
 		}
 		break;
 
 	case MAIN_STATE_AUTO_RTL:
 		/* require global position and home */
-		if (!status->condition_global_position_valid || !status->condition_home_position_valid) {
+		if ((!status->condition_global_position_valid || !status->condition_home_position_valid) && armed) {
 			status->failsafe = true;
 
 		} else {
-			status->nav_state = NAVIGATION_STATE_AUTO_RTL;
+			if (armed) {
+				status->nav_state = NAVIGATION_STATE_AUTO_RTL;
+
+			} else {
+				// TODO which mode should we set when disarmed?
+				status->nav_state = NAVIGATION_STATE_AUTO_LOITER;
+			}
 		}
 		break;
 
@@ -453,5 +469,7 @@ void set_nav_state(struct vehicle_status_s *status)
 			status->nav_state = NAVIGATION_STATE_TERMINATION;
 		}
 	}
+
+	return status->nav_state != nav_state_old;
 }
 
diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h
index 062271764..cffbc9b8f 100644
--- a/src/modules/commander/state_machine_helper.h
+++ b/src/modules/commander/state_machine_helper.h
@@ -65,6 +65,6 @@ transition_result_t main_state_transition(struct vehicle_status_s *current_state
 
 transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd);
 
-void set_nav_state(struct vehicle_status_s *status);
+bool set_nav_state(struct vehicle_status_s *status);
 
 #endif /* STATE_MACHINE_HELPER_H_ */
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index 898c176fb..851938795 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -96,15 +96,16 @@ typedef enum {
  */
 typedef enum {
 	NAVIGATION_STATE_MANUAL = 0,		/**< Manual mode */
-	NAVIGATION_STATE_ACRO,			/**< Acro mode */
 	NAVIGATION_STATE_ALTCTL,		/**< Altitude control mode */
 	NAVIGATION_STATE_POSCTL,		/**< Position control mode */
 	NAVIGATION_STATE_AUTO_MISSION,		/**< Auto mission mode */
 	NAVIGATION_STATE_AUTO_LOITER,		/**< Auto loiter mode */
 	NAVIGATION_STATE_AUTO_RTL,		/**< Auto RTL mode */
+	NAVIGATION_STATE_ACRO,			/**< Acro mode */
 	NAVIGATION_STATE_LAND,			/**< Land mode */
 	NAVIGATION_STATE_DESCEND,			/**< Descend mode (no position control) */
 	NAVIGATION_STATE_TERMINATION,		/**< Termination mode */
+	NAVIGATION_STATE_MAX,
 } navigation_state_t;
 
 enum VEHICLE_MODE_FLAG {