commander: add ground distance check to prevent flight termination

src/modules/commander/Commander.cpp

@@ -2883,7 +2883,8 @@ Commander::run()
 						events::send(events::ID("commander_fd_lockdown"), {events::Log::Emergency, events::LogInternal::Warning},
 							     "Critical failure detected: lockdown");
 
-					} else if (!_status_flags.circuit_breaker_flight_termination_disabled &&
+					} else if (_failure_detector.getTerminationAllowed() &&
+						   !_status_flags.circuit_breaker_flight_termination_disabled &&
 						   !_flight_termination_triggered && !_lockdown_triggered) {
 
 						_armed.force_failsafe = true;
@@ -2900,6 +2901,13 @@ Commander::run()
 						events::send(events::ID("commander_fd_terminate"), {events::Log::Emergency, events::LogInternal::Warning},
 							     "Critical failure detected: terminate flight");
 						send_parachute_command();
+
+					} else if (!_failure_detector_msg_flag) {
+						/* Failure is detected during the flight but the flight will not be terminated due to preconditions.
+						 * Send warning to the pilot! */
+						mavlink_log_critical(&_mavlink_log_pub, "Failure detected, please land now!\t");
+						events::send(events::ID("commander_fd_failure_detected"), events::Log::Critical, "Failure detected, please land now!");
+						_failure_detector_msg_flag = true;
 					}
 				}
 

src/modules/commander/Commander.hpp

@@ -327,6 +327,7 @@ class Commander : public ModuleBase<Commander>, public ModuleParams
 	bool		_flight_termination_triggered{false};
 	bool		_lockdown_triggered{false};
 	bool            _imbalanced_propeller_check_triggered{false};
+	bool		_failure_detector_msg_flag{false};
 
 
 	hrt_abstime	_datalink_last_heartbeat_gcs{0};

src/modules/commander/failure_detector/FailureDetector.cpp

@@ -80,9 +80,25 @@ bool FailureDetector::update(const vehicle_status_s &vehicle_status, const vehic
 		_status.flags.mpc_vz = false;
 	}
 
+	updateTerminationAllowedStatus();
+
 	return _status.value != status_prev.value;
 }
 
+void FailureDetector::updateTerminationAllowedStatus()
+{
+	vehicle_local_position_s local_position;
+	_vehicle_local_position_sub.copy(&local_position);
+
+	// If the measurement is valid and under the distance threshold, flight termination is not allowed, otherwise it is.
+	if (local_position.dist_bottom_valid && (local_position.dist_bottom < _param_fd_min_dist_trm_p.get())) {
+		_termination_allowed = false;
+
+	} else {
+		_termination_allowed = true;
+	}
+}
+
 void FailureDetector::updateAttitudeStatus()
 {
 	vehicle_attitude_s attitude;

src/modules/commander/failure_detector/FailureDetector.hpp

@@ -85,11 +85,13 @@ class FailureDetector : public ModuleParams
 	FailureDetector(ModuleParams *parent);
 
 	bool update(const vehicle_status_s &vehicle_status, const vehicle_control_mode_s &vehicle_control_mode);
+	bool getTerminationAllowed() {return _termination_allowed; };
 	const failure_detector_status_u &getStatus() const { return _status; }
 	const decltype(failure_detector_status_u::flags) &getStatusFlags() const { return _status.flags; }
 	float getImbalancedPropMetric() const { return _imbalanced_prop_lpf.getState(); }
 
 private:
+	void updateTerminationAllowedStatus();
 	void updateAttitudeStatus();
 	void updateExternalAtsStatus();
 	void updateEscsStatus(const vehicle_status_s &vehicle_status);
@@ -104,6 +106,7 @@ class FailureDetector : public ModuleParams
 	systemlib::Hysteresis _esc_failure_hysteresis{false};
 	systemlib::Hysteresis _mpc_vz_failure_hysteresis{false};
 
+	bool _termination_allowed{false}; ///< Flag indicating if preconditions for flight termination are satisfied
 	static constexpr float _imbalanced_prop_lpf_time_constant{5.f};
 	AlphaFilter<float> _imbalanced_prop_lpf{};
 	uint32_t _selected_accel_device_id{0};
@@ -118,6 +121,7 @@ class FailureDetector : public ModuleParams
 	uORB::Subscription _vehicle_local_position_setpoint_sub{ORB_ID(vehicle_local_position_setpoint)};
 
 	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::FD_MIN_DIST_TRM>) _param_fd_min_dist_trm_p,
 		(ParamInt<px4::params::FD_FAIL_P>) _param_fd_fail_p,
 		(ParamInt<px4::params::FD_FAIL_R>) _param_fd_fail_r,
 		(ParamFloat<px4::params::FD_FAIL_R_TTRI>) _param_fd_fail_r_ttri,

src/modules/commander/failure_detector/failure_detector_params.c

@@ -42,6 +42,22 @@
 #include <px4_platform_common/px4_config.h>
 #include <parameters/param.h>
 
+/**
+ * Minimum Ground Distance For Termination
+ *
+ * The value has implications for determining whether the flight termination action will occur.
+ * If a failure is detected and the vehicle's distance from the ground exceeds this parameter's value,
+ * termination will be executed. Otherwise, if the distance is less than the parameter value,
+ * the failure detector values will only be logged.
+ *
+ * @decimal 2
+ * @min 0.00
+ * @max 10000.00
+ * @unit m
+ * @group Failure Detector
+ */
+PARAM_DEFINE_FLOAT(FD_MIN_DIST_TRM, 0.00);
+
 /**
  * FailureDetector Max Roll
  *