feat(commander): uncommanded altitude loss detection with parachute failsafe

Detects when a rotary-wing vehicle drops more than FD_ALT_LOSS metres
below a NED-z reference while altitude control is active,
and immediately triggers flight termination (parachute deployment).

Detection (FailureDetector):
- FD_ALT_LOSS: drop threshold in metres (0 = disabled, default)
- FD_ALT_LOSS_T: hysteresis time
- Guards: rotary-wing only, altitude control active, z_valid, setpoint
  fresh (<1 s). Manual, Acro and FW/VTOL-FW modes are excluded.
- Ratcheting reference: initialises to lpos.z on first sample below
  setpoint, preventing false triggers on new waypoints

Failsafe action (commander):
- New fd_alt_loss flag in FailsafeFlags.msg
- COM_ALT_LOSS_ACT: -1=Disabled (default), 0=Terminate
- Terminate fires immediately, cannot be overridden, and never clears
  until disarm (parachute deployment is irreversible)

Author: gguidone

msg/FailsafeFlags.msg

@@ -45,10 +45,11 @@ bool battery_low_remaining_time       # Low battery based on remaining flight ti
 bool battery_unhealthy                # Battery unhealthy
 
 # Failure detector
-bool fd_critical_failure              # Critical failure (attitude/altitude limit exceeded, or external ATS)
+bool fd_critical_failure              # Critical failure (attitude limit exceeded, or external ATS)
 bool fd_esc_arming_failure            # ESC failed to arm
 bool fd_imbalanced_prop               # Imbalanced propeller detected
 bool fd_motor_failure                 # Motor failure
+bool fd_alt_loss                      # Uncommanded altitude loss (rotary-wing, altitude-controlled flight)
 
 # Other
 bool geofence_breached        	      # Geofence breached (one or multiple)

src/modules/commander/HealthAndArmingChecks/checks/failureDetectorCheck.cpp

@@ -100,8 +100,8 @@ void FailureDetectorChecks::checkAndReport(const Context &context, Report &repor
 		}
 	}
 
-	reporter.failsafeFlags().fd_critical_failure = fd_status.fd_roll || fd_status.fd_pitch || fd_status.fd_alt ||
-			fd_status.fd_ext;
+	reporter.failsafeFlags().fd_critical_failure = fd_status.fd_roll || fd_status.fd_pitch || fd_status.fd_ext;
+	reporter.failsafeFlags().fd_alt_loss = fd_status.fd_alt;
 
 	reporter.failsafeFlags().fd_imbalanced_prop = fd_status.fd_imbalanced_prop;
 

src/modules/commander/failsafe/failsafe.cpp

@@ -624,12 +624,15 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
 		+ static_cast<hrt_abstime>((_param_com_lkdown_tko.get() + _param_com_spoolup_time.get()) * 1_s))
 	   ) {
 		CHECK_FAILSAFE(status_flags, fd_critical_failure, ActionOptions(Action::Disarm).cannotBeDeferred());
+		CHECK_FAILSAFE(status_flags, fd_alt_loss, ActionOptions(Action::Disarm).cannotBeDeferred());
 
 	} else if (!circuit_breaker_enabled_by_val(_param_cbrk_flightterm.get(), CBRK_FLIGHTTERM_KEY)) {
 		CHECK_FAILSAFE(status_flags, fd_critical_failure, ActionOptions(Action::Terminate).cannotBeDeferred());
+		CHECK_FAILSAFE(status_flags, fd_alt_loss, ActionOptions(Action::Terminate).cannotBeDeferred());
 
 	} else {
 		CHECK_FAILSAFE(status_flags, fd_critical_failure, Action::Warn);
+		CHECK_FAILSAFE(status_flags, fd_alt_loss, Action::Warn);
 	}
 
 	CHECK_FAILSAFE(status_flags, fd_imbalanced_prop, fromImbalancedPropActParam(_param_com_imb_prop_act.get()));

src/modules/commander/failure_detector/FailureDetector.cpp

@@ -56,6 +56,7 @@ bool FailureDetector::update(const vehicle_status_s &vehicle_status, const vehic
 
 	if (vehicle_control_mode.flag_control_attitude_enabled) {
 		updateAttitudeStatus(vehicle_status);
+		updateAltitudeStatus(vehicle_status, vehicle_control_mode);
 
 		if (_param_fd_ext_ats_en.get()) {
 			updateExternalAtsStatus();
@@ -66,6 +67,9 @@ bool FailureDetector::update(const vehicle_status_s &vehicle_status, const vehic
 		_failure_detector_status.flags.pitch = false;
 		_failure_detector_status.flags.alt = false;
 		_failure_detector_status.flags.ext = false;
+		// Reset altitude loss state so it reinitialises cleanly when altitude control re-engages.
+		_alt_loss_ref_z = NAN;
+		_alt_loss_hysteresis.set_state_and_update(false, hrt_absolute_time());
 	}
 
 	if (_param_fd_imb_prop_thr.get() > 0) {
@@ -141,6 +145,44 @@ void FailureDetector::updateAttitudeStatus(const vehicle_status_s &vehicle_statu
 	}
 }
 
+void FailureDetector::updateAltitudeStatus(const vehicle_status_s &vehicle_status,
+		const vehicle_control_mode_s &vehicle_control_mode)
+{
+	const float threshold = _param_fd_alt_loss.get();
+
+	if (threshold < FLT_EPSILON
+	    || !vehicle_control_mode.flag_control_altitude_enabled
+	    || vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+		_failure_detector_status.flags.alt = false;
+		_alt_loss_ref_z = NAN;
+		return;
+	}
+
+	vehicle_local_position_s lpos{};
+	vehicle_local_position_setpoint_s lpos_sp{};
+	_vehicle_local_position_sub.copy(&lpos);
+	_vehicle_local_position_setpoint_sub.copy(&lpos_sp);
+
+	const hrt_abstime now = hrt_absolute_time();
+
+	if (lpos.z > lpos_sp.z) {
+
+		// Ratcheting NED-z reference (mirrors VtolType::isUncommandedDescent in vtol_type.cpp,
+		// translated to NED: ref_z = max(min(ref_z, lpos.z), lpos_sp.z); trigger when (lpos.z - ref_z) > threshold).
+		_alt_loss_ref_z = math::max(math::min(_alt_loss_ref_z, lpos.z), lpos_sp.z);
+
+		const bool is_below_threshold = (lpos.z - _alt_loss_ref_z) > threshold;
+		_alt_loss_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)(1_s * _param_fd_alt_loss_ttri.get()));
+		_alt_loss_hysteresis.set_state_and_update(is_below_threshold, now);
+
+	} else {
+		_alt_loss_ref_z = NAN;
+		_alt_loss_hysteresis.set_state_and_update(false, now);
+	}
+
+	_failure_detector_status.flags.alt = _alt_loss_hysteresis.get_state();
+}
+
 void FailureDetector::updateExternalAtsStatus()
 {
 	pwm_input_s pwm_input;

src/modules/commander/failure_detector/FailureDetector.hpp

@@ -60,6 +60,8 @@
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_imu_status.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/vehicle_status.h>
 
 union failure_detector_status_u {
@@ -89,22 +91,29 @@ class FailureDetector : public ModuleParams
 
 private:
 	void updateAttitudeStatus(const vehicle_status_s &vehicle_status);
+	void updateAltitudeStatus(const vehicle_status_s &vehicle_status,
+				  const vehicle_control_mode_s &vehicle_control_mode);
 	void updateExternalAtsStatus();
 	void updateImbalancedPropStatus();
 
 	failure_detector_status_u _failure_detector_status{};
 
 	systemlib::Hysteresis _roll_failure_hysteresis{false};
 	systemlib::Hysteresis _pitch_failure_hysteresis{false};
+	systemlib::Hysteresis _alt_loss_hysteresis{false};
 	systemlib::Hysteresis _ext_ats_failure_hysteresis{false};
 
+	float _alt_loss_ref_z{NAN}; // ratcheting NED-z reference for altitude loss detection
+
 	static constexpr float _imbalanced_prop_lpf_time_constant{5.f};
 	AlphaFilter<float> _imbalanced_prop_lpf{};
 	uint32_t _selected_accel_device_id{0};
 	hrt_abstime _imu_status_timestamp_prev{0};
 
 
 	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
+	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
+	uORB::Subscription _vehicle_local_position_setpoint_sub{ORB_ID(vehicle_local_position_setpoint)};
 	uORB::Subscription _pwm_input_sub{ORB_ID(pwm_input)};
 	uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)};
 	uORB::Subscription _vehicle_imu_status_sub{ORB_ID(vehicle_imu_status)};
@@ -120,6 +129,8 @@ class FailureDetector : public ModuleParams
 		(ParamFloat<px4::params::FD_FAIL_P_TTRI>) _param_fd_fail_p_ttri,
 		(ParamBool<px4::params::FD_EXT_ATS_EN>) _param_fd_ext_ats_en,
 		(ParamInt<px4::params::FD_EXT_ATS_TRIG>) _param_fd_ext_ats_trig,
-		(ParamInt<px4::params::FD_IMB_PROP_THR>) _param_fd_imb_prop_thr
+		(ParamInt<px4::params::FD_IMB_PROP_THR>) _param_fd_imb_prop_thr,
+		(ParamFloat<px4::params::FD_ALT_LOSS>) _param_fd_alt_loss,
+		(ParamFloat<px4::params::FD_ALT_LOSS_T>) _param_fd_alt_loss_ttri
 	)
 };

src/modules/commander/failure_detector/failure_detector_params.yaml

@@ -91,3 +91,26 @@ parameters:
       min: 0
       max: 1000
       increment: 1
+    FD_ALT_LOSS:
+      description:
+        short: Altitude loss threshold for termination and parachute deployment
+        long: |-
+          Maximum altitude loss below the setpoint allowed before the vehicle terminates and deploys the parachute. Set to 0 to disable.
+      type: float
+      default: 0.0
+      min: 0.0
+      max: 200.0
+      unit: m
+      decimal: 1
+      increment: 0.5
+    FD_ALT_LOSS_T:
+      description:
+        short: Altitude loss failure trigger time
+        long: |-
+          Seconds that the altitude loss threshold must be exceeded before the failure is declared.
+      type: float
+      default: 1.0
+      min: 0.02
+      max: 5.0
+      unit: s
+      decimal: 2