Commander: separate failure detector cases

One case could lead to the action for the other!

src/modules/commander/Commander.cpp

@@ -2238,40 +2238,38 @@ Commander::run()
 			status.failure_detector_status = _failure_detector.getStatus();
 			_status_changed = true;
 
-			if (armed.armed && _failure_detector.isFailure()) {
-				const hrt_abstime time_at_arm = armed.armed_time_ms * 1000;
+			if (armed.armed) {
+				if (status.failure_detector_status & vehicle_status_s::FAILURE_ARM_ESC) {
+					const hrt_abstime time_at_arm = armed.armed_time_ms * 1000;
 
-				if (hrt_elapsed_time(&time_at_arm) < 500_ms) {
 					// 500ms is the PWM spoolup time. Within this timeframe controllers are not affecting actuator_outputs
-
-					if (status.failure_detector_status & vehicle_status_s::FAILURE_ARM_ESC) {
+					if (hrt_elapsed_time(&time_at_arm) < 500_ms) {
 						arm_disarm(false, true, &mavlink_log_pub, arm_disarm_reason_t::FAILURE_DETECTOR);
 						_status_changed = true;
 						mavlink_log_critical(&mavlink_log_pub, "ESCs did not respond to arm request");
 					}
-
 				}
 
-				if (hrt_elapsed_time(&_time_at_takeoff) < (1_s * _param_com_lkdown_tko.get())) {
-					// This handles the case where something fails during the early takeoff phase
-					if (!_lockdown_triggered) {
+				if (status.failure_detector_status & (vehicle_status_s::FAILURE_ROLL | vehicle_status_s::FAILURE_PITCH |
+								      vehicle_status_s::FAILURE_ALT | vehicle_status_s::FAILURE_EXT)) {
+					const bool is_second_after_takeoff = hrt_elapsed_time(&_time_at_takeoff) < (1_s * _param_com_lkdown_tko.get());
 
+					if (is_second_after_takeoff && !_lockdown_triggered) {
+						// This handles the case where something fails during the early takeoff phase
 						armed.lockdown = true;
 						_lockdown_triggered = true;
 						_status_changed = true;
-
 						mavlink_log_emergency(&mavlink_log_pub, "Critical failure detected: lockdown");
-					}
 
-				} else if (!status_flags.circuit_breaker_flight_termination_disabled &&
-					   !_flight_termination_triggered && !_lockdown_triggered) {
+					} else if (!status_flags.circuit_breaker_flight_termination_disabled &&
+						   !_flight_termination_triggered && !_lockdown_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);
+						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);
+					}
 				}
 			}
 		}

src/modules/commander/failure_detector/FailureDetector.hpp

@@ -74,9 +74,7 @@ public:
 	FailureDetector(ModuleParams *parent);
 
 	bool update(const vehicle_status_s &vehicle_status);
-
 	uint8_t getStatus() const { return _status; }
-	bool isFailure() const { return _status != FAILURE_NONE; }
 
 private:
 	void resetAttitudeStatus();