commander: failsafe_state removed, replaced with bool failsafe, navigation state and failsafe determined directly from main state and conditions

1 files changed, 38 insertions, 52 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index e992706ac..43683f833 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -77,6 +77,7 @@
 #include <uORB/topics/differential_pressure.h>
 #include <uORB/topics/safety.h>
 #include <uORB/topics/mission_result.h>
+#include <uORB/topics/telemetry_status.h>
 
 #include <drivers/drv_led.h>
 #include <drivers/drv_hrt.h>
@@ -122,6 +123,7 @@ extern struct system_load_s system_load;
 #define POSITION_TIMEOUT		(600 * 1000)		/**< consider the local or global position estimate invalid after 600ms */
 #define FAILSAFE_DEFAULT_TIMEOUT	(3 * 1000 * 1000)	/**< hysteresis time - the failsafe will trigger after 3 seconds in this state */
 #define RC_TIMEOUT			500000
+#define DL_TIMEOUT			5 * 1000* 1000
 #define DIFFPRESS_TIMEOUT		2000000
 
 #define PRINT_INTERVAL	5000000
@@ -495,12 +497,12 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 			unsigned int mav_goto = cmd->param1;
 
 			if (mav_goto == 0) {	// MAV_GOTO_DO_HOLD
-				status->set_nav_state = NAVIGATION_STATE_AUTO_LOITER;
+				status->nav_state = NAVIGATION_STATE_AUTO_LOITER;
 				mavlink_log_critical(mavlink_fd, "#audio: pause mission cmd");
 				cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
 
 			} else if (mav_goto == 1) {	// MAV_GOTO_DO_CONTINUE
-				status->set_nav_state = NAVIGATION_STATE_AUTO_MISSION;
+				status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
 				mavlink_log_critical(mavlink_fd, "#audio: continue mission cmd");
 				cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
 
@@ -639,13 +641,6 @@ int commander_thread_main(int argc, char *argv[])
 	arming_states_str[5] = "REBOOT";
 	arming_states_str[6] = "IN_AIR_RESTORE";
 
-	char *failsafe_states_str[FAILSAFE_STATE_MAX];
-	failsafe_states_str[0] = "NORMAL";
-	failsafe_states_str[1] = "RTL_RC";
-	failsafe_states_str[2] = "RTL_DL";
-	failsafe_states_str[3] = "LAND";
-	failsafe_states_str[4] = "TERMINATION";
-
 	/* pthread for slow low prio thread */
 	pthread_t commander_low_prio_thread;
 
@@ -666,10 +661,10 @@ int commander_thread_main(int argc, char *argv[])
 	// We want to accept RC inputs as default
 	status.rc_input_blocked = false;
 	status.main_state = MAIN_STATE_MANUAL;
-	status.set_nav_state = NAVIGATION_STATE_MANUAL;
+	status.nav_state = NAVIGATION_STATE_MANUAL;
 	status.arming_state = ARMING_STATE_INIT;
 	status.hil_state = HIL_STATE_OFF;
-	status.failsafe_state = FAILSAFE_STATE_NORMAL;
+	status.failsafe = false;
 
 	/* neither manual nor offboard control commands have been received */
 	status.offboard_control_signal_found_once = false;
@@ -678,6 +673,7 @@ int commander_thread_main(int argc, char *argv[])
 	/* mark all signals lost as long as they haven't been found */
 	status.rc_signal_lost = true;
 	status.offboard_control_signal_lost = true;
+	status.data_link_lost = true;
 
 	/* set battery warning flag */
 	status.battery_warning = VEHICLE_BATTERY_WARNING_NONE;
@@ -771,6 +767,11 @@ int commander_thread_main(int argc, char *argv[])
 	struct offboard_control_setpoint_s sp_offboard;
 	memset(&sp_offboard, 0, sizeof(sp_offboard));
 
+	/* Subscribe to telemetry status */
+	int telemetry_sub = orb_subscribe(ORB_ID(telemetry_status));
+	struct telemetry_status_s telemetry;
+	memset(&telemetry, 0, sizeof(telemetry));
+
 	/* Subscribe to global position */
 	int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
 	struct vehicle_global_position_s global_position;
@@ -908,6 +909,12 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard);
 		}
 
+		orb_check(telemetry_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(telemetry_status), telemetry_sub, &telemetry);
+		}
+
 		orb_check(sensor_sub, &updated);
 
 		if (updated) {
@@ -1186,7 +1193,7 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result);
 		}
 
-		/* start RC input check */
+		/* RC input check */
 		if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) {
 			/* handle the case where RC signal was regained */
 			if (!status.rc_signal_found_once) {
@@ -1198,9 +1205,6 @@ int commander_thread_main(int argc, char *argv[])
 				if (status.rc_signal_lost) {
 					mavlink_log_critical(mavlink_fd, "#audio: RC signal regained");
 					status_changed = true;
-
-					status.failsafe_state = FAILSAFE_STATE_NORMAL;
-					failsafe_state_changed = true;
 				}
 			}
 
@@ -1271,12 +1275,6 @@ int commander_thread_main(int argc, char *argv[])
 				mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied");
 			}
 
-
-			// if (status.failsafe_state != FAILSAFE_STATE_NORMAL) {
-			//	/* recover from failsafe */
-			//	(void)failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
-			// }
-
 			/* evaluate the main state machine according to mode switches */
 			transition_result_t main_res = set_main_state_rc(&status, &sp_man);
 
@@ -1295,34 +1293,24 @@ int commander_thread_main(int argc, char *argv[])
 				mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST");
 				status.rc_signal_lost = true;
 				status_changed = true;
-
-				if (!(status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION && !mission_result.mission_finished)) {
-
-					/* if we have a global position, we can switch to RTL, if not, we can try to land */
-					if (status.condition_global_position_valid) {
-						status.failsafe_state = FAILSAFE_STATE_RC_LOSS;
-					} else {
-						status.failsafe_state = FAILSAFE_STATE_LAND;
-					}
-					failsafe_state_changed = true;
-				} else {
-					mavlink_log_info(mavlink_fd, "#audio: no RTL during Mission");
-				}
 			}
 		}
 
-		/* hack to detect if we finished a mission after we lost RC, so that we can trigger RTL now */
-		if (status.rc_signal_lost && status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION &&
-		    mission_result.mission_finished && status.failsafe_state != FAILSAFE_STATE_RC_LOSS) {
-			/* if we have a global position, we can switch to RTL, if not, we can try to land */
-			if (status.condition_global_position_valid) {
-				status.failsafe_state = FAILSAFE_STATE_RC_LOSS;
-				mavlink_log_info(mavlink_fd, "#audio: RTL after Mission is finished");
-			} else {
-				/* this probably doesn't make sense since we are in mission and have global position */
-				status.failsafe_state = FAILSAFE_STATE_LAND;
+		/* data link check */
+		if (hrt_abstime() < telemetry.heartbeat_time + DL_TIMEOUT) {
+			/* handle the case where RC signal was regained */
+			if (status.data_link_lost) {
+				mavlink_log_critical(mavlink_fd, "#audio: data link regained");
+				status.data_link_lost = false;
+				status_changed = true;
+			}
+
+		} else {
+			if (!status.data_link_lost) {
+				mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: DATA LINK LOST");
+				status.data_link_lost = false;
+				status_changed = true;
 			}
-			failsafe_state_changed = true;
 		}
 
 		/* handle commands last, as the system needs to be updated to handle them */
@@ -1377,7 +1365,7 @@ int commander_thread_main(int argc, char *argv[])
 
 		was_armed = armed.armed;
 
-		/* now set nav state according to failsafe and main state */
+		/* now set navigation state according to failsafe and main state */
 		set_nav_state(&status);
 
 		if (main_state_changed) {
@@ -1388,7 +1376,7 @@ int commander_thread_main(int argc, char *argv[])
 
 		if (failsafe_state_changed) {
 			status_changed = true;
-			mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %s", failsafe_states_str[status.failsafe_state]);
+			mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %i", status.failsafe);
 			failsafe_state_changed = false;
 		}
 
@@ -1415,7 +1403,7 @@ int commander_thread_main(int argc, char *argv[])
 			/* play tune on battery critical */
 			set_tune(TONE_BATTERY_WARNING_FAST_TUNE);
 
-		} else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW || status.failsafe_state != FAILSAFE_STATE_NORMAL) {
+		} else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW || status.failsafe) {
 			/* play tune on battery warning or failsafe */
 			set_tune(TONE_BATTERY_WARNING_SLOW_TUNE);
 
@@ -1519,7 +1507,7 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a
 
 		if (set_normal_color) {
 			/* set color */
-			if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status->failsafe_state != FAILSAFE_STATE_NORMAL) {
+			if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status->failsafe) {
 				rgbled_set_color(RGBLED_COLOR_AMBER);
 				/* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */
 
@@ -1667,7 +1655,7 @@ set_control_mode()
 	control_mode.flag_external_manual_override_ok = !status.is_rotary_wing;
 	control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON;
 
-	switch (status.set_nav_state) {
+	switch (status.nav_state) {
 	case NAVIGATION_STATE_MANUAL:
 		control_mode.flag_control_manual_enabled = true;
 		control_mode.flag_control_auto_enabled = false;
@@ -1719,8 +1707,6 @@ set_control_mode()
 	case NAVIGATION_STATE_AUTO_MISSION:
 	case NAVIGATION_STATE_AUTO_LOITER:
 	case NAVIGATION_STATE_AUTO_RTL:
-	case NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS:
-	case NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS:
 		control_mode.flag_control_manual_enabled = false;
 		control_mode.flag_control_auto_enabled = true;
 		control_mode.flag_control_rates_enabled = true;