cammander: state machine can now deny current state (e.g. when position lock lost during EASY mode), added FAILSAFE_STATE_LAND

5 files changed, 144 insertions, 48 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 722230eff..bca0569d5 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1143,7 +1143,7 @@ int commander_thread_main(int argc, char *argv[])
 				/* fill current_status according to mode switches */
 				check_mode_switches(&sp_man, &status);
 
-				/* evaluate the main state machine */
+				/* evaluate the main state machine according to mode switches */
 				res = check_main_state_machine(&status);
 
 				if (res == TRANSITION_CHANGED) {
@@ -1160,12 +1160,41 @@ int commander_thread_main(int argc, char *argv[])
 					status.rc_signal_lost = true;
 					status_changed = true;
 				}
-				if (status.main_state != MAIN_STATE_AUTO && armed.armed) {
+
+				if (status.main_state == MAIN_STATE_AUTO) {
+					/* check if AUTO mode still allowed */
+					transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO);
+					if (res == TRANSITION_DENIED) {
+						/* AUTO mode denied, don't try RTL, switch to failsafe state LAND */
+						res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
+						if (res == TRANSITION_CHANGED) {
+							mavlink_log_critical(mavlink_fd, "#audio: failsafe: LAND");
+						} else if (res == TRANSITION_DENIED) {
+							/* LAND mode denied, switch to failsafe state TERMINATION */
+							transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
+							if (res == TRANSITION_CHANGED) {
+								mavlink_log_critical(mavlink_fd, "#audio: failsafe: TERMINATION");
+							}
+						}
+					}
+
+				} else if (armed.armed) {
+					/* failsafe for manual modes */
 					transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL);
 					if (res == TRANSITION_CHANGED) {
 						mavlink_log_critical(mavlink_fd, "#audio: failsafe: RTL");
+					} else if (res == TRANSITION_DENIED) {
+						/* RTL not allowed (no global position estimate), try LAND */
+						res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
+						if (res == TRANSITION_CHANGED) {
+							mavlink_log_critical(mavlink_fd, "#audio: failsafe: LAND");
+						} else if (res == TRANSITION_DENIED) {
+							res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
+							if (res == TRANSITION_CHANGED) {
+								mavlink_log_critical(mavlink_fd, "#audio: failsafe: TERMINATION");
+							}
+						}
 					}
-					// TODO add other failsafe modes if position estimate not available
 				}
 			}
 		}
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 90ca2a6d2..77edea546 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -223,51 +223,50 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m
 {
 	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;
-
-	} else {
-
-		switch (new_main_state) {
-		case MAIN_STATE_MANUAL:
+	/* 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;
+
+	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;
-
-		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;
-			}
-
-			break;
+		/* need at minimum local position estimate */
+		if (current_state->condition_local_position_valid ||
+			current_state->condition_global_position_valid) {
+			ret = TRANSITION_CHANGED;
+		}
 
-		case MAIN_STATE_AUTO:
+		break;
 
-			/* need global position estimate */
-			if (current_state->condition_global_position_valid) {
-				ret = TRANSITION_CHANGED;
-			}
+	case MAIN_STATE_AUTO:
 
-			break;
+		/* need global position estimate */
+		if (current_state->condition_global_position_valid) {
+			ret = TRANSITION_CHANGED;
 		}
 
-		if (ret == TRANSITION_CHANGED) {
+		break;
+	}
+
+	if (ret == TRANSITION_CHANGED) {
+		if (current_state->main_state != new_main_state) {
 			current_state->main_state = new_main_state;
 			main_state_changed = true;
+
+		} else {
+			ret = TRANSITION_NOT_CHANGED;
 		}
 	}
 
@@ -367,17 +366,22 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f
 {
 	transition_result_t ret = TRANSITION_DENIED;
 
-	/* only check transition if the new state is actually different from the current one */
-	if (new_failsafe_state == status->failsafe_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 if (status->failsafe_state != FAILSAFE_STATE_TERMINATION) {
+	} else {
 		switch (new_failsafe_state) {
 		case FAILSAFE_STATE_NORMAL:
 			ret = TRANSITION_CHANGED;
 			break;
 		case FAILSAFE_STATE_RTL:
-			ret = TRANSITION_CHANGED;
+			if (status->condition_global_position_valid) {
+				ret = TRANSITION_CHANGED;
+			}
 			break;
 		case FAILSAFE_STATE_TERMINATION:
 			ret = TRANSITION_CHANGED;
@@ -388,8 +392,13 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f
 		}
 
 		if (ret == TRANSITION_CHANGED) {
-			status->failsafe_state = new_failsafe_state;
-			failsafe_state_changed = true;
+			if (status->failsafe_state != new_failsafe_state) {
+				status->failsafe_state = new_failsafe_state;
+				failsafe_state_changed = true;
+
+			} else {
+				ret = TRANSITION_NOT_CHANGED;
+			}
 		}
 	}
 
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index d72ed7058..388fefd02 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -216,6 +216,7 @@ private:
 		EVENT_LOITER_REQUESTED,
 		EVENT_MISSION_REQUESTED,
 		EVENT_RTL_REQUESTED,
+		EVENT_LAND_REQUESTED,
 		EVENT_MISSION_CHANGED,
 		EVENT_HOME_POSITION_CHANGED,
 		MAX_EVENT
@@ -292,7 +293,7 @@ private:
 	void		start_loiter();
 	void		start_mission();
 	void		start_rtl();
-	void		finish_rtl();
+	void		start_land();
 
 	/**
 	 * Guards offboard mission
@@ -733,6 +734,12 @@ Navigator::task_main()
 						dispatch(EVENT_RTL_REQUESTED);
 					}
 
+				} else if (_vstatus.failsafe_state == FAILSAFE_STATE_LAND) {
+					/* LAND on failsafe */
+					if (myState != NAV_STATE_READY) {
+						dispatch(EVENT_LAND_REQUESTED);
+					}
+
 				} else {
 					/* shouldn't act */
 					dispatch(EVENT_NONE_REQUESTED);
@@ -892,6 +899,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
 		/* EVENT_LOITER_REQUESTED */		{ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
 		/* EVENT_MISSION_REQUESTED */		{ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
 		/* EVENT_RTL_REQUESTED */		{ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
+		/* EVENT_LAND_REQUESTED */		{ACTION(&Navigator::start_land), NAV_STATE_LAND},
 		/* EVENT_MISSION_CHANGED */		{NO_ACTION, NAV_STATE_NONE},
 		/* EVENT_HOME_POSITION_CHANGED */	{NO_ACTION, NAV_STATE_NONE},
 	},
@@ -902,6 +910,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
 		/* EVENT_LOITER_REQUESTED */		{NO_ACTION, NAV_STATE_READY},
 		/* EVENT_MISSION_REQUESTED */		{ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
 		/* EVENT_RTL_REQUESTED */		{ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
+		/* EVENT_LAND_REQUESTED */		{NO_ACTION, NAV_STATE_READY},
 		/* EVENT_MISSION_CHANGED */		{NO_ACTION, NAV_STATE_READY},
 		/* EVENT_HOME_POSITION_CHANGED */	{NO_ACTION, NAV_STATE_READY},
 	},
@@ -912,6 +921,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
 		/* EVENT_LOITER_REQUESTED */		{NO_ACTION, NAV_STATE_LOITER},
 		/* EVENT_MISSION_REQUESTED */		{ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
 		/* EVENT_RTL_REQUESTED */		{ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
+		/* EVENT_LAND_REQUESTED */		{ACTION(&Navigator::start_land), NAV_STATE_LAND},
 		/* EVENT_MISSION_CHANGED */		{NO_ACTION, NAV_STATE_LOITER},
 		/* EVENT_HOME_POSITION_CHANGED */	{NO_ACTION, NAV_STATE_LOITER},
 	},
@@ -922,6 +932,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
 		/* EVENT_LOITER_REQUESTED */		{ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
 		/* EVENT_MISSION_REQUESTED */		{NO_ACTION, NAV_STATE_MISSION},
 		/* EVENT_RTL_REQUESTED */		{ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
+		/* EVENT_LAND_REQUESTED */		{ACTION(&Navigator::start_land), NAV_STATE_LAND},
 		/* EVENT_MISSION_CHANGED */		{ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
 		/* EVENT_HOME_POSITION_CHANGED */	{NO_ACTION, NAV_STATE_MISSION},
 	},
@@ -932,9 +943,21 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
 		/* EVENT_LOITER_REQUESTED */		{ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
 		/* EVENT_MISSION_REQUESTED */		{ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
 		/* EVENT_RTL_REQUESTED */		{NO_ACTION, NAV_STATE_RTL},
+		/* EVENT_LAND_REQUESTED */		{ACTION(&Navigator::start_land), NAV_STATE_LAND},
 		/* EVENT_MISSION_CHANGED */		{NO_ACTION, NAV_STATE_RTL},
 		/* EVENT_HOME_POSITION_CHANGED */	{ACTION(&Navigator::start_rtl), NAV_STATE_RTL},	// TODO need to reset rtl_state
 	},
+	{
+							/* STATE_LAND */
+		/* EVENT_NONE_REQUESTED */		{ACTION(&Navigator::start_none), NAV_STATE_NONE},
+		/* EVENT_READY_REQUESTED */		{ACTION(&Navigator::start_ready), NAV_STATE_READY},
+		/* EVENT_LOITER_REQUESTED */		{ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
+		/* EVENT_MISSION_REQUESTED */		{ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
+		/* EVENT_RTL_REQUESTED */		{ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
+		/* EVENT_LAND_REQUESTED */		{ACTION(&Navigator::start_land), NAV_STATE_LAND},
+		/* EVENT_MISSION_CHANGED */		{NO_ACTION, NAV_STATE_RTL},
+		/* EVENT_HOME_POSITION_CHANGED */	{ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
+	},
 };
 
 void
@@ -1143,6 +1166,27 @@ Navigator::start_rtl()
 }
 
 void
+Navigator::start_land()
+{
+	_do_takeoff = false;
+	_reset_loiter_pos = true;
+
+	_pos_sp_triplet.previous.valid = false;
+	_pos_sp_triplet.next.valid = false;
+
+	_pos_sp_triplet.current.valid = true;
+	_pos_sp_triplet.current.type = SETPOINT_TYPE_LAND;
+	_pos_sp_triplet.current.lat = _global_pos.lat;
+	_pos_sp_triplet.current.lon = _global_pos.lon;
+	_pos_sp_triplet.current.alt = _global_pos.alt;
+	_pos_sp_triplet.current.loiter_direction = 1;
+	_pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius;
+	_pos_sp_triplet.current.yaw = NAN;
+
+	mavlink_log_info(_mavlink_fd, "[navigator] LAND started");
+}
+
+void
 Navigator::set_rtl_item()
 {
 	switch (_rtl_state) {
@@ -1508,9 +1552,21 @@ Navigator::publish_control_mode()
 		navigator_enabled = true;
 		break;
 
+	case FAILSAFE_STATE_LAND:
+		/* land with or without position control */
+		_control_mode.flag_control_manual_enabled = false;
+		_control_mode.flag_control_rates_enabled = true;
+		_control_mode.flag_control_attitude_enabled = true;
+		_control_mode.flag_control_position_enabled = _vstatus.condition_global_position_valid;
+		_control_mode.flag_control_velocity_enabled = _vstatus.condition_global_position_valid;
+		_control_mode.flag_control_altitude_enabled = true;
+		_control_mode.flag_control_climb_rate_enabled = true;
+		break;
+
 	case FAILSAFE_STATE_TERMINATION:
 		navigator_enabled = true;
 		/* disable all controllers on termination */
+		_control_mode.flag_control_manual_enabled = false;
 		_control_mode.flag_control_rates_enabled = false;
 		_control_mode.flag_control_attitude_enabled = false;
 		_control_mode.flag_control_position_enabled = false;
diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h
index f9f8414df..5aecac898 100644
--- a/src/modules/uORB/topics/vehicle_control_mode.h
+++ b/src/modules/uORB/topics/vehicle_control_mode.h
@@ -67,6 +67,7 @@ typedef enum {
 	NAV_STATE_LOITER,
 	NAV_STATE_MISSION,
 	NAV_STATE_RTL,
+	NAV_STATE_LAND,
 	NAV_STATE_MAX
 } nav_state_t;
 
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index 73102090f..a5988d3ba 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -84,9 +84,10 @@ typedef enum {
 } hil_state_t;
 
 typedef enum {
-	FAILSAFE_STATE_NORMAL = 0,
-	FAILSAFE_STATE_RTL,
-	FAILSAFE_STATE_TERMINATION,
+	FAILSAFE_STATE_NORMAL = 0,		/**< Normal operation */
+	FAILSAFE_STATE_RTL,				/**< Return To Launch */
+	FAILSAFE_STATE_LAND,			/**< Land without position control */
+	FAILSAFE_STATE_TERMINATION,		/**< Disable motors and use parachute, can't be recovered */
 	FAILSAFE_STATE_MAX
 } failsafe_state_t;