From c23ef0af4ba009e3e4c8bc51e4a2a7f1d319817b Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Thu, 19 Mar 2020 14:20:09 +0100
Subject: [PATCH] Flight termination: lockdown if failure is detected on
 takeoff

During the first few seconds after takeoff, the failure detector is allowed to
trigger motor lockdown.
This is done for safety reasons to detect tipping-over or unstable
tuning gains
---
 src/modules/commander/Commander.cpp | 44 +++++++++++++++++++++--------
 src/modules/commander/Commander.hpp |  1 +
 2 files changed, 33 insertions(+), 12 deletions(-)

diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp
index d858395104..050b20337f 100644
--- a/src/modules/commander/Commander.cpp
+++ b/src/modules/commander/Commander.cpp
@@ -758,13 +758,19 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
 
 	case vehicle_command_s::VEHICLE_CMD_DO_FLIGHTTERMINATION: {
 			if (cmd.param1 > 1.5f) {
-				armed_local->lockdown = true;
-				warnx("forcing lockdown (motors off)");
+				if (!_lockdown_triggered) {
+					armed_local->lockdown = true;
+					_lockdown_triggered = true;
+					warnx("forcing lockdown (motors off)");
+				}
 
 			} else if (cmd.param1 > 0.5f) {
 				//XXX update state machine?
-				armed_local->force_failsafe = true;
-				warnx("forcing failsafe (termination)");
+				if (!_flight_termination_triggered) {
+					armed_local->force_failsafe = true;
+					_flight_termination_triggered = true;
+					warnx("forcing failsafe (termination)");
+				}
 
 				if ((int)cmd.param2 <= 0) {
 					/* reset all commanded failure modes */
@@ -774,6 +780,10 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
 			} else {
 				armed_local->force_failsafe = false;
 				armed_local->lockdown = false;
+
+				_lockdown_triggered = false;
+				_flight_termination_triggered = false;
+
 				warnx("disabling failsafe and lockdown");
 			}
 
@@ -2102,19 +2112,29 @@ Commander::run()
 		}
 
 		if (armed.armed &&
-		    failure_detector_updated &&
-		    !_flight_termination_triggered &&
-		    !status_flags.circuit_breaker_flight_termination_disabled) {
+		    failure_detector_updated) {
 
 			if (_failure_detector.isFailure()) {
+				if ((hrt_elapsed_time(&_time_at_takeoff) < 3_s) &&
+				    !_lockdown_triggered) {
+					// This handles the case where something fails during the early takeoff phase
 
-				armed.force_failsafe = true;
-				_status_changed = true;
+					armed.lockdown = true;
+					_lockdown_triggered = true;
+					_status_changed = true;
 
-				_flight_termination_triggered = true;
+					mavlink_log_emergency(&mavlink_log_pub, "Critical failure detected: lockdown");
 
-				mavlink_log_critical(&mavlink_log_pub, "Critical failure detected: terminate flight");
-				set_tune_override(TONE_PARACHUTE_RELEASE_TUNE);
+				} else if (!status_flags.circuit_breaker_flight_termination_disabled &&
+					   !_flight_termination_triggered) {
+
+					armed.force_failsafe = true;
+					_flight_termination_triggered = true;
+					_status_changed = true;
+
+					mavlink_log_emergency(&mavlink_log_pub, "Critical failure detected: terminate flight");
+					set_tune_override(TONE_PARACHUTE_RELEASE_TUNE);
+				}
 			}
 		}
 
diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp
index 3804063281..a10f64b13a 100644
--- a/src/modules/commander/Commander.hpp
+++ b/src/modules/commander/Commander.hpp
@@ -309,6 +309,7 @@ private:
 
 	FailureDetector	_failure_detector;
 	bool		_flight_termination_triggered{false};
+	bool		_lockdown_triggered{false};
 
 
 	hrt_abstime	_datalink_last_heartbeat_gcs{0};