feat(commander): uncommanded altitude loss detection with parachute f…

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/CMakeLists.txt

@@ -36,3 +36,7 @@ px4_add_library(failure_detector
 	FailureInjector.cpp
 )
 set_property(GLOBAL APPEND PROPERTY PX4_MODULE_CONFIG_FILES ${CMAKE_CURRENT_SOURCE_DIR}/failure_detector_params.yaml)
+
+px4_add_functional_gtest(SRC FailureDetectorAltitudeLossTest.cpp
+	LINKLIBS failure_detector hysteresis
+)

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,55 @@ 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);
+
+	// Adjust reference on EKF z reset to avoid false triggers
+	if (lpos.z_reset_counter != _alt_loss_z_reset_counter) {
+		if (PX4_ISFINITE(_alt_loss_ref_z)) {
+			_alt_loss_ref_z += lpos.delta_z;
+		}
+
+		_alt_loss_z_reset_counter = lpos.z_reset_counter;
+	}
+
+	const hrt_abstime now = hrt_absolute_time();
+
+	if (lpos.z > lpos_sp.z) {
+		// Ratcheting NED-z reference: tracks the highest altitude reached while below setpoint.
+		if (!PX4_ISFINITE(_alt_loss_ref_z)) {
+			_alt_loss_ref_z = lpos.z;
+		}
+
+		_alt_loss_ref_z = math::constrain(_alt_loss_ref_z, lpos_sp.z, lpos.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,30 @@ 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
+	uint8_t _alt_loss_z_reset_counter{0}; // tracks EKF z resets to avoid false altitude loss triggers
+
 	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 +130,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/FailureDetectorAltitudeLossTest.cpp

@@ -0,0 +1,154 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2026 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <gtest/gtest.h>
+
+#include "FailureDetector.hpp"
+
+#include <uORB/Publication.hpp>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_local_position_setpoint.h>
+
+// to run: make tests TESTFILTER=FailureDetectorAltitudeLossTest
+
+using namespace time_literals;
+
+class FailureDetectorAltitudeLossTest : public ::testing::Test
+{
+public:
+	void SetUp() override
+	{
+		param_control_autosave(false);
+
+		// FD_ALT_LOSS = 5m, FD_ALT_LOSS_T = 0s
+		param_t param = param_handle(px4::params::FD_ALT_LOSS);
+		float threshold = 5.f;
+		param_set(param, &threshold);
+
+		param = param_handle(px4::params::FD_ALT_LOSS_T);
+		float ttri = 0.f;
+		param_set(param, &ttri);
+	}
+
+	// Publish position and setpoint, then run the detector. Returns the alt flag state.
+	bool update(float lpos_z, float lpos_sp_z, float delta_z = 0.f, uint8_t z_reset_counter = 0)
+	{
+		vehicle_local_position_s lpos{};
+		lpos.timestamp = hrt_absolute_time();
+		lpos.z = lpos_z;
+		lpos.z_valid = true;
+		lpos.delta_z = delta_z;
+		lpos.z_reset_counter = z_reset_counter;
+		_lpos_pub.publish(lpos);
+
+		vehicle_local_position_setpoint_s lpos_sp{};
+		lpos_sp.timestamp = hrt_absolute_time();
+		lpos_sp.z = lpos_sp_z;
+		_lpos_sp_pub.publish(lpos_sp);
+
+		vehicle_status_s status{};
+		status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
+
+		vehicle_control_mode_s control_mode{};
+		control_mode.flag_control_attitude_enabled = true;
+		control_mode.flag_control_altitude_enabled = true;
+
+		_fd.update(status, control_mode);
+
+		return _fd.getStatus().flags.alt;
+	}
+
+private:
+	FailureDetector _fd{nullptr};
+
+	uORB::Publication<vehicle_local_position_s> _lpos_pub{ORB_ID(vehicle_local_position)};
+	uORB::Publication<vehicle_local_position_setpoint_s> _lpos_sp_pub{ORB_ID(vehicle_local_position_setpoint)};
+};
+
+TEST_F(FailureDetectorAltitudeLossTest, NoTriggerWhenDisabled)
+{
+	param_t param = param_handle(px4::params::FD_ALT_LOSS);
+	float threshold = 0.f;
+	param_set(param, &threshold);
+
+	EXPECT_FALSE(update(-90.f, -100.f));
+}
+
+TEST_F(FailureDetectorAltitudeLossTest, NoTriggerAboveSetpoint)
+{
+	EXPECT_FALSE(update(-105.f, -100.f));
+}
+
+TEST_F(FailureDetectorAltitudeLossTest, NoTriggerWithinThreshold)
+{
+	// 3m below setpoint, threshold is 5m
+	EXPECT_FALSE(update(-97.f, -100.f));
+}
+
+TEST_F(FailureDetectorAltitudeLossTest, TriggerAfterSustainedDrop)
+{
+	// Ratchet initialises at -97; vehicle sinks to -91 (6m drop exceeds 5m threshold)
+	EXPECT_FALSE(update(-97.f, -100.f)); // 0m drop
+	EXPECT_FALSE(update(-95.f, -100.f)); // 2m drop
+	EXPECT_FALSE(update(-93.f, -100.f)); // 4m drop
+	EXPECT_TRUE(update(-91.f, -100.f));  // 6m drop, triggers
+}
+
+TEST_F(FailureDetectorAltitudeLossTest, RatchetHoldsOnPartialRecovery)
+{
+	// Ratchet tracks the best recovery point and is not reset by a partial climb
+	EXPECT_FALSE(update(-96.f, -100.f)); // ratchet = -96, drop = 0m
+	EXPECT_FALSE(update(-94.f, -100.f)); // ratchet = -96, drop = 2m
+	EXPECT_FALSE(update(-96.f, -100.f)); // partial recovery, ratchet stays at -96, drop = 0m
+	EXPECT_FALSE(update(-95.f, -100.f)); // ratchet = -96, drop = 1m
+	EXPECT_TRUE(update(-90.f, -100.f));  // ratchet = -96, drop = 6m, triggers
+}
+
+TEST_F(FailureDetectorAltitudeLossTest, ResetWhenBackAboveSetpoint)
+{
+	// After triggering, climbing above setpoint clears the flag and resets the ratchet
+	EXPECT_FALSE(update(-97.f, -100.f));
+	EXPECT_TRUE(update(-91.f, -100.f));
+
+	EXPECT_FALSE(update(-101.f, -100.f)); // above setpoint, resets
+
+	EXPECT_FALSE(update(-97.f, -100.f)); // ratchet reinitialises at -97, drop = 0m
+	EXPECT_FALSE(update(-95.f, -100.f)); // drop = 2m, no trigger
+}
+
+TEST_F(FailureDetectorAltitudeLossTest, NoTriggerOnEkfZReset)
+{
+	// EKF resets z by 6m downward; reference shifts with it so the drop stays 0m.
+	EXPECT_FALSE(update(-97.f, -100.f));          // ratchet = -97, drop = 0m
+	EXPECT_FALSE(update(-91.f, -100.f, 6.f, 1)); // EKF reset: ref shifts to -91, drop = 0m
+}

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