Merge branch 'beta' into offboard2

1 files changed, 85 insertions, 55 deletions

diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 4dd63b5e1..23d3be55d 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -63,7 +63,7 @@ static const int ERROR = -1;
 
 static bool arming_state_changed = true;
 static bool main_state_changed = true;
-static bool flighttermination_state_changed = true;
+static bool failsafe_state_changed = true;
 
 transition_result_t
 arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety,
@@ -220,61 +220,66 @@ check_arming_state_changed()
 }
 
 transition_result_t
-main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state)
+main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state)
 {
 	transition_result_t ret = TRANSITION_DENIED;
 
-	/* only check transition if the new state is actually different from the current one */
-	if (new_main_state == current_state->main_state) {
-		ret = TRANSITION_NOT_CHANGED;
+	/* transition may be denied even if requested the same state because conditions may be changed */
+	switch (new_main_state) {
+	case MAIN_STATE_MANUAL:
+		ret = TRANSITION_CHANGED;
+		break;
 
-	} else {
+	case MAIN_STATE_SEATBELT:
 
-		switch (new_main_state) {
-		case MAIN_STATE_MANUAL:
+		/* need at minimum altitude estimate */
+		if (!status->is_rotary_wing ||
+		    (status->condition_local_altitude_valid ||
+		     status->condition_global_position_valid)) {
 			ret = TRANSITION_CHANGED;
-			break;
-
-		case MAIN_STATE_SEATBELT:
-
-			/* need at minimum altitude estimate */
-			if (!current_state->is_rotary_wing ||
-			    (current_state->condition_local_altitude_valid ||
-			     current_state->condition_global_position_valid)) {
-				ret = TRANSITION_CHANGED;
-			}
+		}
 
-			break;
+		break;
 
-		case MAIN_STATE_EASY:
+	case MAIN_STATE_EASY:
 
-			/* need at minimum local position estimate */
-			if (current_state->condition_local_position_valid ||
-			    current_state->condition_global_position_valid) {
-				ret = TRANSITION_CHANGED;
-			}
+		/* need at minimum local position estimate */
+		if (status->condition_local_position_valid ||
+		    status->condition_global_position_valid) {
+			ret = TRANSITION_CHANGED;
+		}
 
-			break;
+		break;
 
-		case MAIN_STATE_AUTO:
+	case MAIN_STATE_AUTO:
 
-			/* need global position estimate */
-			if (current_state->condition_global_position_valid) {
-				ret = TRANSITION_CHANGED;
-			}
+		/* need global position estimate */
+		if (status->condition_global_position_valid) {
+			ret = TRANSITION_CHANGED;
+		}
 
-			break;
+		break;
 
-		case MAIN_STATE_OFFBOARD:
+	case MAIN_STATE_OFFBOARD:
 
+		/* need global position estimate */
+		if (!status->offboard_control_signal_lost) {
 			ret = TRANSITION_CHANGED;
-
-			break;
 		}
 
-		if (ret == TRANSITION_CHANGED) {
-			current_state->main_state = new_main_state;
+		break;
+
+	default:
+		break;
+	}
+
+	if (ret == TRANSITION_CHANGED) {
+		if (status->main_state != new_main_state) {
+			status->main_state = new_main_state;
 			main_state_changed = true;
+
+		} else {
+			ret = TRANSITION_NOT_CHANGED;
 		}
 	}
 
@@ -294,10 +299,10 @@ check_main_state_changed()
 }
 
 bool
-check_flighttermination_state_changed()
+check_failsafe_state_changed()
 {
-	if (flighttermination_state_changed) {
-		flighttermination_state_changed = false;
+	if (failsafe_state_changed) {
+		failsafe_state_changed = false;
 		return true;
 
 	} else {
@@ -368,28 +373,49 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
 
 
 /**
-* Transition from one flightermination state to another
+* Transition from one failsafe state to another
 */
-transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state)
+transition_result_t failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state)
 {
 	transition_result_t ret = TRANSITION_DENIED;
 
-	/* only check transition if the new state is actually different from the current one */
-	if (new_flighttermination_state == status->flighttermination_state) {
-		ret = TRANSITION_NOT_CHANGED;
+	/* transition may be denied even if requested the same state because conditions may be changed */
+	if (status->failsafe_state == FAILSAFE_STATE_TERMINATION) {
+		/* transitions from TERMINATION to other states not allowed */
+		if (new_failsafe_state == FAILSAFE_STATE_TERMINATION) {
+			ret = TRANSITION_NOT_CHANGED;
+		}
 
 	} else {
-
-		switch (new_flighttermination_state) {
-		case FLIGHTTERMINATION_STATE_ON:
+		switch (new_failsafe_state) {
+		case FAILSAFE_STATE_NORMAL:
+			/* always allowed (except from TERMINATION state) */
 			ret = TRANSITION_CHANGED;
-			status->flighttermination_state = FLIGHTTERMINATION_STATE_ON;
-			warnx("state machine helper: change to FLIGHTTERMINATION_STATE_ON");
 			break;
 
-		case FLIGHTTERMINATION_STATE_OFF:
+		case FAILSAFE_STATE_RTL:
+			/* global position and home position required for RTL */
+			if (status->condition_global_position_valid && status->condition_home_position_valid) {
+				status->set_nav_state = NAV_STATE_RTL;
+				status->set_nav_state_timestamp = hrt_absolute_time();
+				ret = TRANSITION_CHANGED;
+			}
+
+			break;
+
+		case FAILSAFE_STATE_LAND:
+			/* at least relative altitude estimate required for landing */
+			if (status->condition_local_altitude_valid || status->condition_global_position_valid) {
+				status->set_nav_state = NAV_STATE_LAND;
+				status->set_nav_state_timestamp = hrt_absolute_time();
+				ret = TRANSITION_CHANGED;
+			}
+
+			break;
+
+		case FAILSAFE_STATE_TERMINATION:
+			/* always allowed */
 			ret = TRANSITION_CHANGED;
-			status->flighttermination_state = FLIGHTTERMINATION_STATE_OFF;
 			break;
 
 		default:
@@ -397,9 +423,13 @@ transition_result_t flighttermination_state_transition(struct vehicle_status_s *
 		}
 
 		if (ret == TRANSITION_CHANGED) {
-			flighttermination_state_changed = true;
-			// TODO
-			//control_mode->flag_control_flighttermination_enabled = status->flighttermination_state == FLIGHTTERMINATION_STATE_ON;
+			if (status->failsafe_state != new_failsafe_state) {
+				status->failsafe_state = new_failsafe_state;
+				failsafe_state_changed = true;
+
+			} else {
+				ret = TRANSITION_NOT_CHANGED;
+			}
 		}
 	}