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

1 files changed, 70 insertions, 42 deletions

diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index c52e618ef..df718ff99 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -354,7 +354,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub,
 		}
 	}
 
-	if (ret = TRANSITION_CHANGED) {
+	if (ret == TRANSITION_CHANGED) {
 		current_status->hil_state = new_state;
 		current_status->timestamp = hrt_absolute_time();
 		// XXX also set lockdown here
@@ -363,67 +363,95 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub,
 	return ret;
 }
 
-
 /**
  * Check failsafe and main status and set navigation status for navigator accordingly
  */
 void set_nav_state(struct vehicle_status_s *status)
 {
-	switch (status->failsafe_state) {
-	case FAILSAFE_STATE_NORMAL:
-		/* evaluate main state to decide in normal (non-failsafe) mode */
-		switch (status->main_state) {
-		case MAIN_STATE_MANUAL:
-			status->set_nav_state = NAVIGATION_STATE_MANUAL;
-			break;
-
-		case MAIN_STATE_ALTCTL:
-			status->set_nav_state = NAVIGATION_STATE_ALTCTL;
-			break;
+	status->failsafe = false;
 
-		case MAIN_STATE_POSCTL:
-			status->set_nav_state = NAVIGATION_STATE_POSCTL;
-			break;
-
-		case MAIN_STATE_AUTO_MISSION:
-			status->set_nav_state = NAVIGATION_STATE_AUTO_MISSION;
-			break;
-
-		case MAIN_STATE_AUTO_LOITER:
-			status->set_nav_state = NAVIGATION_STATE_AUTO_LOITER;
-			break;
+	/* evaluate main state to decide in normal (non-failsafe) mode */
+	switch (status->main_state) {
+	case MAIN_STATE_ACRO:
+	case MAIN_STATE_MANUAL:
+	case MAIN_STATE_ALTCTL:
+	case MAIN_STATE_POSCTL:
+		/* require RC for all manual modes */
+		if (status->rc_signal_lost) {
+			status->failsafe = true;
 
-		case MAIN_STATE_AUTO_RTL:
-			status->set_nav_state = NAVIGATION_STATE_AUTO_RTL;
-			break;
+		} else {
+			switch (status->main_state) {
+			case MAIN_STATE_ACRO:
+				status->nav_state = NAVIGATION_STATE_ACRO;
+				break;
+
+			case MAIN_STATE_MANUAL:
+				status->nav_state = NAVIGATION_STATE_MANUAL;
+				break;
+
+			case MAIN_STATE_ALTCTL:
+				status->nav_state = NAVIGATION_STATE_ALTCTL;
+				break;
+
+			case MAIN_STATE_POSCTL:
+				status->nav_state = NAVIGATION_STATE_POSCTL;
+				break;
+
+			default:
+				status->nav_state = NAVIGATION_STATE_MANUAL;
+				break;
+			}
+		}
+		break;
 
-		case MAIN_STATE_ACRO:
-			status->set_nav_state = NAVIGATION_STATE_ACRO;
-			break;
+	case MAIN_STATE_AUTO_MISSION:
+		/* require data link and global position */
+		if (status->data_link_lost || !status->condition_global_position_valid) {
+			status->failsafe = true;
 
-		default:
-			break;
+		} else {
+			status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
 		}
 		break;
 
-	case FAILSAFE_STATE_RC_LOSS:
-		status->set_nav_state = NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS;
-		break;
+	case MAIN_STATE_AUTO_LOITER:
+		/* require data link and local position */
+		if (status->data_link_lost || !status->condition_local_position_valid) {
+			status->failsafe = true;
 
-	case FAILSAFE_STATE_DL_LOSS:
-		status->set_nav_state = NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS;
+		} else {
+			status->nav_state = NAVIGATION_STATE_AUTO_LOITER;
+		}
 		break;
 
-	case FAILSAFE_STATE_LAND:
-		status->set_nav_state = NAVIGATION_STATE_LAND;
-		break;
+	case MAIN_STATE_AUTO_RTL:
+		/* require global position and home */
+		if (!status->condition_global_position_valid || !status->condition_home_position_valid) {
+			status->failsafe = true;
 
-	case FAILSAFE_STATE_TERMINATION:
-		status->set_nav_state = NAVIGATION_STATE_TERMINATION;
+		} else {
+			status->nav_state = NAVIGATION_STATE_AUTO_RTL;
+		}
 		break;
 
 	default:
 		break;
 	}
+
+	if (status->failsafe) {
+		if (status->condition_global_position_valid && status->condition_home_position_valid) {
+			status->nav_state = NAVIGATION_STATE_AUTO_RTL;
+
+		} else if (status->condition_local_position_valid) {
+			status->nav_state = NAVIGATION_STATE_LAND;
+
+		} else if (status->condition_local_altitude_valid) {
+			status->nav_state = NAVIGATION_STATE_DESCEND;
+
+		} else {
+			status->nav_state = NAVIGATION_STATE_TERMINATION;
+		}
+	}
 }