«flighttermination state» replaced by more general «failsafe state»

7 files changed, 198 insertions, 156 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index d51bb63ff..7715f73e0 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -512,7 +512,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 	case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command
 
 			if (armed->armed && cmd->param3 > 0.5) { //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO
-				transition_result_t flighttermination_res = flighttermination_state_transition(status, FLIGHTTERMINATION_STATE_ON);
+				transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION);
 				result = VEHICLE_CMD_RESULT_ACCEPTED;
 				ret = true;
 
@@ -1112,6 +1112,14 @@ int commander_thread_main(int argc, char *argv[])
 					mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
 				}
 
+				if (status.failsafe_state != FAILSAFE_STATE_NORMAL) {
+					/* recover from failsafe */
+					transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
+					if (res == TRANSITION_CHANGED) {
+						mavlink_log_critical(mavlink_fd, "#audio: recover from failsafe");
+					}
+				}
+
 				/* fill current_status according to mode switches */
 				check_mode_switches(&sp_man, &status);
 
@@ -1135,32 +1143,23 @@ int commander_thread_main(int argc, char *argv[])
 					status_changed = true;
 				}
 				if (status.main_state != MAIN_STATE_AUTO && armed.armed) {
-					transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO);
+					transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL);
 					if (res == TRANSITION_CHANGED) {
-						mavlink_log_critical(mavlink_fd, "#audio: failsafe, switching to RTL mode");
-						status.set_nav_state = NAV_STATE_RTL;
-						status.set_nav_state_timestamp = hrt_absolute_time();
-					} else if (status.main_state != MAIN_STATE_SEATBELT) {
-						res = main_state_transition(&status, MAIN_STATE_SEATBELT);
-						if (res == TRANSITION_CHANGED) {
-							mavlink_log_critical(mavlink_fd, "#audio: failsafe, switching to SEATBELT mode");
-						}
+						mavlink_log_critical(mavlink_fd, "#audio: failsafe: RTL");
 					}
+					// TODO add other failsafe modes if position estimate not available
 				}
 			}
 		}
 
-		/*  Flight termination in manual mode if assisted switch is on easy position //xxx hack! */
-		if (armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) {
-			transition_result_t flighttermination_res = flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_ON);
-			if (flighttermination_res == TRANSITION_CHANGED) {
+		// TODO remove this hack
+		/* flight termination in manual mode if assisted switch is on easy position */
+		if (!status.is_rotary_wing && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) {
+			if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) {
 				tune_positive();
 			}
-		} else {
-			flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_OFF);
 		}
 
-
 		/* handle commands last, as the system needs to be updated to handle them */
 		orb_check(cmd_sub, &updated);
 
@@ -1176,12 +1175,12 @@ int commander_thread_main(int argc, char *argv[])
 		/* check which state machines for changes, clear "changed" flag */
 		bool arming_state_changed = check_arming_state_changed();
 		bool main_state_changed = check_main_state_changed();
-		bool flighttermination_state_changed = check_flighttermination_state_changed();
+		bool failsafe_state_changed = check_failsafe_state_changed();
 
 		hrt_abstime t1 = hrt_absolute_time();
 
-		if (arming_state_changed || main_state_changed) {
-			mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d", status.arming_state, status.main_state);
+		if (arming_state_changed || main_state_changed || failsafe_state_changed) {
+			mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, fs %d", status.arming_state, status.main_state, status.failsafe_state);
 			status_changed = true;
 		}
 
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 731e0e3ff..90ca2a6d2 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,
@@ -287,10 +287,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 {
@@ -361,41 +361,39 @@ 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;
-
-		} else {
+	/* 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;
 
-			switch (new_flighttermination_state) {
-			case FLIGHTTERMINATION_STATE_ON:
-				ret = TRANSITION_CHANGED;
-				status->flighttermination_state = FLIGHTTERMINATION_STATE_ON;
-				warnx("state machine helper: change to FLIGHTTERMINATION_STATE_ON");
-				break;
-			case FLIGHTTERMINATION_STATE_OFF:
-				ret = TRANSITION_CHANGED;
-				status->flighttermination_state = FLIGHTTERMINATION_STATE_OFF;
-				break;
+	} else if (status->failsafe_state != FAILSAFE_STATE_TERMINATION) {
+		switch (new_failsafe_state) {
+		case FAILSAFE_STATE_NORMAL:
+			ret = TRANSITION_CHANGED;
+			break;
+		case FAILSAFE_STATE_RTL:
+			ret = TRANSITION_CHANGED;
+			break;
+		case FAILSAFE_STATE_TERMINATION:
+			ret = TRANSITION_CHANGED;
+			break;
 
-			default:
-				break;
-			}
+		default:
+			break;
+		}
 
-			if (ret == TRANSITION_CHANGED) {
-				flighttermination_state_changed = true;
-				// TODO
-				//control_mode->flag_control_flighttermination_enabled = status->flighttermination_state == FLIGHTTERMINATION_STATE_ON;
-			}
+		if (ret == TRANSITION_CHANGED) {
+			status->failsafe_state = new_failsafe_state;
+			failsafe_state_changed = true;
 		}
+	}
 
-		return ret;
+	return ret;
 }
 
 
diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h
index e569fb4f3..f04879ff9 100644
--- a/src/modules/commander/state_machine_helper.h
+++ b/src/modules/commander/state_machine_helper.h
@@ -67,11 +67,11 @@ transition_result_t main_state_transition(struct vehicle_status_s *current_state
 
 bool check_main_state_changed();
 
-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);
 
 bool check_navigation_state_changed();
 
-bool check_flighttermination_state_changed();
+bool check_failsafe_state_changed();
 
 void set_navigation_state_changed();
 
diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp
index dc2196de6..17b1028f9 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -619,7 +619,7 @@ FixedwingAttitudeControl::task_main()
 			}
 
 			/* Simple handling of failsafe: deploy parachute if failsafe is on */
-			if (_vcontrol_mode.flag_control_flighttermination_enabled) {
+			if (_vcontrol_mode.flag_control_termination_enabled) {
 				_actuators_airframe.control[1] = 1.0f;
 //				warnx("_actuators_airframe.control[1] = 1.0f;");
 			} else {
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index cfcc886b6..89a62e166 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -643,87 +643,101 @@ Navigator::task_main()
 			vehicle_status_update();
 			pub_control_mode = true;
 
-			/* Evaluate state machine from commander and set the navigator mode accordingly */
-			if (_vstatus.main_state == MAIN_STATE_AUTO &&
-					(_vstatus.arming_state == ARMING_STATE_ARMED || _vstatus.arming_state == ARMING_STATE_ARMED_ERROR)) {
-				bool stick_mode = false;
-				if (!_vstatus.rc_signal_lost) {
-					/* RC signal available, use control switches to set mode */
-					/* RETURN switch, overrides MISSION switch */
-					if (_vstatus.return_switch == RETURN_SWITCH_RETURN) {
-						if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) {
-							dispatch(EVENT_RTL_REQUESTED);
-						}
-						stick_mode = true;
-					} else {
-						/* MISSION switch */
-						if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) {
-							dispatch(EVENT_LOITER_REQUESTED);
-							stick_mode = true;
-						} else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) {
-							/* switch to mission only if available */
-							if (_mission.current_mission_available()) {
-								dispatch(EVENT_MISSION_REQUESTED);
+			/* evaluate state machine from commander and set the navigator mode accordingly */
+			if (_vstatus.arming_state == ARMING_STATE_ARMED || _vstatus.arming_state == ARMING_STATE_ARMED_ERROR) {
+				if (_vstatus.failsafe_state == FAILSAFE_STATE_NORMAL) {
+					if (_vstatus.main_state == MAIN_STATE_AUTO) {
+						bool stick_mode = false;
+						if (!_vstatus.rc_signal_lost) {
+							/* RC signal available, use control switches to set mode */
+							/* RETURN switch, overrides MISSION switch */
+							if (_vstatus.return_switch == RETURN_SWITCH_RETURN) {
+								if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) {
+									dispatch(EVENT_RTL_REQUESTED);
+								}
+								stick_mode = true;
 							} else {
-								dispatch(EVENT_LOITER_REQUESTED);
+								/* MISSION switch */
+								if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) {
+									dispatch(EVENT_LOITER_REQUESTED);
+									stick_mode = true;
+								} else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) {
+									/* switch to mission only if available */
+									if (_mission.current_mission_available()) {
+										dispatch(EVENT_MISSION_REQUESTED);
+									} else {
+										dispatch(EVENT_LOITER_REQUESTED);
+									}
+									stick_mode = true;
+								}
+								if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) {
+									/* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */
+									dispatch(EVENT_LOITER_REQUESTED);
+									stick_mode = true;
+								}
 							}
-							stick_mode = true;
 						}
-						if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) {
-							/* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */
-							dispatch(EVENT_LOITER_REQUESTED);
-							stick_mode = true;
-						}
-					}
-				}
-
-				if (!stick_mode) {
-					if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) {
-						/* commander requested new navigation mode, try to set it */
-						_set_nav_state_timestamp = _vstatus.set_nav_state_timestamp;
-
-						switch (_vstatus.set_nav_state) {
-						case NAV_STATE_NONE:
-							/* nothing to do */
-							break;
 
-						case NAV_STATE_LOITER:
-							dispatch(EVENT_LOITER_REQUESTED);
-							break;
+						if (!stick_mode) {
+							if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) {
+								/* commander requested new navigation mode, try to set it */
+								_set_nav_state_timestamp = _vstatus.set_nav_state_timestamp;
+
+								switch (_vstatus.set_nav_state) {
+								case NAV_STATE_NONE:
+									/* nothing to do */
+									break;
+
+								case NAV_STATE_LOITER:
+									dispatch(EVENT_LOITER_REQUESTED);
+									break;
+
+								case NAV_STATE_MISSION:
+									if (_mission.current_mission_available()) {
+										dispatch(EVENT_MISSION_REQUESTED);
+									} else {
+										dispatch(EVENT_LOITER_REQUESTED);
+									}
+									break;
+
+								case NAV_STATE_RTL:
+									if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) {
+										dispatch(EVENT_RTL_REQUESTED);
+									}
+									break;
+
+								default:
+									warnx("ERROR: Requested navigation state not supported");
+									break;
+								}
 
-						case NAV_STATE_MISSION:
-							if (_mission.current_mission_available()) {
-								dispatch(EVENT_MISSION_REQUESTED);
 							} else {
-								dispatch(EVENT_LOITER_REQUESTED);
+								/* on first switch to AUTO try mission by default, if none is available fallback to loiter */
+								if (myState == NAV_STATE_NONE) {
+									if (_mission.current_mission_available()) {
+										dispatch(EVENT_MISSION_REQUESTED);
+									} else {
+										dispatch(EVENT_LOITER_REQUESTED);
+									}
+								}
 							}
-							break;
-
-						case NAV_STATE_RTL:
-							if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) {
-								dispatch(EVENT_RTL_REQUESTED);
-							}
-							break;
-
-						default:
-							warnx("ERROR: Requested navigation state not supported");
-							break;
 						}
-
 					} else {
-						/* on first switch to AUTO try mission by default, if none is available fallback to loiter */
-						if (myState == NAV_STATE_NONE) {
-							if (_mission.current_mission_available()) {
-								dispatch(EVENT_MISSION_REQUESTED);
-							} else {
-								dispatch(EVENT_LOITER_REQUESTED);
-							}
-						}
+						/* not in AUTO mode */
+						dispatch(EVENT_NONE_REQUESTED);
 					}
+
+				} else if (_vstatus.failsafe_state == FAILSAFE_STATE_RTL) {
+					/* RTL on failsafe */
+					dispatch(EVENT_RTL_REQUESTED);
+
+				} else {
+					/* shouldn't act */
+					dispatch(EVENT_NONE_REQUESTED);
 				}
 
 			} else {
-				/* not in AUTO */
+				/* not armed */
 				dispatch(EVENT_NONE_REQUESTED);
 			}
 		}
@@ -1442,40 +1456,74 @@ Navigator::publish_control_mode()
 	_control_mode.flag_system_hil_enabled = _vstatus.hil_state == HIL_STATE_ON;
 
 	_control_mode.flag_control_offboard_enabled = false;
-	_control_mode.flag_control_flighttermination_enabled = false;
+	_control_mode.flag_control_termination_enabled = false;
+
+	/* set this flag when navigator has control */
+	bool navigator_enabled = false;
+
+	switch (_vstatus.failsafe_state) {
+	case FAILSAFE_STATE_NORMAL:
+		switch (_vstatus.main_state) {
+		case MAIN_STATE_MANUAL:
+			_control_mode.flag_control_manual_enabled = true;
+			_control_mode.flag_control_rates_enabled = _vstatus.is_rotary_wing;
+			_control_mode.flag_control_attitude_enabled = _vstatus.is_rotary_wing;
+			_control_mode.flag_control_altitude_enabled = false;
+			_control_mode.flag_control_climb_rate_enabled = false;
+			_control_mode.flag_control_position_enabled = false;
+			_control_mode.flag_control_velocity_enabled = false;
+			break;
 
-	switch (_vstatus.main_state) {
-	case MAIN_STATE_MANUAL:
-		_control_mode.flag_control_manual_enabled = true;
-		_control_mode.flag_control_rates_enabled = _vstatus.is_rotary_wing;
-		_control_mode.flag_control_attitude_enabled = _vstatus.is_rotary_wing;
-		_control_mode.flag_control_altitude_enabled = false;
-		_control_mode.flag_control_climb_rate_enabled = false;
-		_control_mode.flag_control_position_enabled = false;
-		_control_mode.flag_control_velocity_enabled = false;
+		case MAIN_STATE_SEATBELT:
+			_control_mode.flag_control_manual_enabled = true;
+			_control_mode.flag_control_rates_enabled = true;
+			_control_mode.flag_control_attitude_enabled = true;
+			_control_mode.flag_control_altitude_enabled = true;
+			_control_mode.flag_control_climb_rate_enabled = true;
+			_control_mode.flag_control_position_enabled = false;
+			_control_mode.flag_control_velocity_enabled = false;
+			break;
+
+		case MAIN_STATE_EASY:
+			_control_mode.flag_control_manual_enabled = true;
+			_control_mode.flag_control_rates_enabled = true;
+			_control_mode.flag_control_attitude_enabled = true;
+			_control_mode.flag_control_altitude_enabled = true;
+			_control_mode.flag_control_climb_rate_enabled = true;
+			_control_mode.flag_control_position_enabled = true;
+			_control_mode.flag_control_velocity_enabled = true;
+			break;
+
+		case MAIN_STATE_AUTO:
+			navigator_enabled = true;
+
+		default:
+			break;
+		}
 		break;
 
-	case MAIN_STATE_SEATBELT:
-		_control_mode.flag_control_manual_enabled = true;
-		_control_mode.flag_control_rates_enabled = true;
-		_control_mode.flag_control_attitude_enabled = true;
-		_control_mode.flag_control_altitude_enabled = true;
-		_control_mode.flag_control_climb_rate_enabled = true;
+	case FAILSAFE_STATE_RTL:
+		navigator_enabled = true;
+		break;
+
+	case FAILSAFE_STATE_TERMINATION:
+		navigator_enabled = true;
+		/* disable all controllers on termination */
+		_control_mode.flag_control_rates_enabled = false;
+		_control_mode.flag_control_attitude_enabled = false;
 		_control_mode.flag_control_position_enabled = false;
 		_control_mode.flag_control_velocity_enabled = false;
+		_control_mode.flag_control_altitude_enabled = false;
+		_control_mode.flag_control_climb_rate_enabled = false;
+		_control_mode.flag_control_termination_enabled = true;
 		break;
 
-	case MAIN_STATE_EASY:
-		_control_mode.flag_control_manual_enabled = true;
-		_control_mode.flag_control_rates_enabled = true;
-		_control_mode.flag_control_attitude_enabled = true;
-		_control_mode.flag_control_altitude_enabled = true;
-		_control_mode.flag_control_climb_rate_enabled = true;
-		_control_mode.flag_control_position_enabled = true;
-		_control_mode.flag_control_velocity_enabled = true;
+	default:
 		break;
+	}
 
-	case MAIN_STATE_AUTO:
+	/* navigator has control, set control mode flags according to nav state*/
+	if (navigator_enabled) {
 		_control_mode.flag_control_manual_enabled = false;
 		if (myState == NAV_STATE_READY) {
 			/* disable all controllers, armed but idle */
@@ -1493,10 +1541,6 @@ Navigator::publish_control_mode()
 			_control_mode.flag_control_altitude_enabled = true;
 			_control_mode.flag_control_climb_rate_enabled = true;
 		}
-		break;
-
-	default:
-		break;
 	}
 
 	_control_mode.timestamp = hrt_absolute_time();
diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h
index 26dcbd985..f9f8414df 100644
--- a/src/modules/uORB/topics/vehicle_control_mode.h
+++ b/src/modules/uORB/topics/vehicle_control_mode.h
@@ -92,7 +92,7 @@ struct vehicle_control_mode_s
 	bool flag_control_position_enabled;		/**< true if position is controlled */
 	bool flag_control_altitude_enabled;		/**< true if altitude is controlled */
 	bool flag_control_climb_rate_enabled;		/**< true if climb rate is controlled */
-	bool flag_control_flighttermination_enabled;   /**< true if flighttermination is enabled */
+	bool flag_control_termination_enabled;   /**< true if flighttermination is enabled */
 };
 
 /**
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index 1a9dec5f5..a3a862d85 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -82,9 +82,11 @@ typedef enum {
 } hil_state_t;
 
 typedef enum {
-	FLIGHTTERMINATION_STATE_OFF = 0,
-	FLIGHTTERMINATION_STATE_ON
-} flighttermination_state_t;
+	FAILSAFE_STATE_NORMAL = 0,
+	FAILSAFE_STATE_RTL,
+	FAILSAFE_STATE_TERMINATION,
+	FAILSAFE_STATE_MAX
+} failsafe_state_t;
 
 typedef enum {
 	MODE_SWITCH_MANUAL = 0,
@@ -173,6 +175,7 @@ struct vehicle_status_s
 	uint64_t set_nav_state_timestamp;	/**< timestamp of latest change of set_nav_state */
 	arming_state_t arming_state;			/**< current arming state */
 	hil_state_t hil_state;					/**< current hil state */
+	failsafe_state_t failsafe_state;		/**< current failsafe state */
 
 	int32_t system_type;				/**< system type, inspired by MAVLink's VEHICLE_TYPE enum */
 	int32_t	system_id;				/**< system id, inspired by MAVLink's system ID field */
@@ -223,8 +226,6 @@ struct vehicle_status_s
 	uint16_t errors_count2;
 	uint16_t errors_count3;
 	uint16_t errors_count4;
-
-	flighttermination_state_t flighttermination_state;
 };
 
 /**