diff --git a/msg/FailsafeFlags.msg b/msg/FailsafeFlags.msg index 2d974791f752..86f42f178e49 100644 --- a/msg/FailsafeFlags.msg +++ b/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) diff --git a/src/modules/commander/HealthAndArmingChecks/checks/failureDetectorCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/failureDetectorCheck.cpp index 0b5715662dbe..25370a2d9b08 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/failureDetectorCheck.cpp +++ b/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; diff --git a/src/modules/commander/failsafe/failsafe.cpp b/src/modules/commander/failsafe/failsafe.cpp index 25d9728cc6db..88a0f4e58214 100644 --- a/src/modules/commander/failsafe/failsafe.cpp +++ b/src/modules/commander/failsafe/failsafe.cpp @@ -624,12 +624,15 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state, + static_cast((_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())); diff --git a/src/modules/commander/failure_detector/CMakeLists.txt b/src/modules/commander/failure_detector/CMakeLists.txt index 91503f37b457..9e391573b935 100644 --- a/src/modules/commander/failure_detector/CMakeLists.txt +++ b/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 +) diff --git a/src/modules/commander/failure_detector/FailureDetector.cpp b/src/modules/commander/failure_detector/FailureDetector.cpp index baee7de486bb..eb5a50967ea9 100644 --- a/src/modules/commander/failure_detector/FailureDetector.cpp +++ b/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; diff --git a/src/modules/commander/failure_detector/FailureDetector.hpp b/src/modules/commander/failure_detector/FailureDetector.hpp index 4318f43156e1..0d14036da6ff 100644 --- a/src/modules/commander/failure_detector/FailureDetector.hpp +++ b/src/modules/commander/failure_detector/FailureDetector.hpp @@ -60,6 +60,8 @@ #include #include #include +#include +#include #include union failure_detector_status_u { @@ -89,6 +91,8 @@ 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(); @@ -96,8 +100,12 @@ class FailureDetector : public ModuleParams 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 _imbalanced_prop_lpf{}; uint32_t _selected_accel_device_id{0}; @@ -105,6 +113,8 @@ class FailureDetector : public ModuleParams 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) _param_fd_fail_p_ttri, (ParamBool) _param_fd_ext_ats_en, (ParamInt) _param_fd_ext_ats_trig, - (ParamInt) _param_fd_imb_prop_thr + (ParamInt) _param_fd_imb_prop_thr, + (ParamFloat) _param_fd_alt_loss, + (ParamFloat) _param_fd_alt_loss_ttri ) }; diff --git a/src/modules/commander/failure_detector/FailureDetectorAltitudeLossTest.cpp b/src/modules/commander/failure_detector/FailureDetectorAltitudeLossTest.cpp new file mode 100644 index 000000000000..aebd2025e456 --- /dev/null +++ b/src/modules/commander/failure_detector/FailureDetectorAltitudeLossTest.cpp @@ -0,0 +1,162 @@ +/**************************************************************************** + * + * 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 + +#include "FailureDetector.hpp" + +#include +#include +#include + +// 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 _lpos_pub{ORB_ID(vehicle_local_position)}; + uORB::Publication _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 +} + +TEST_F(FailureDetectorAltitudeLossTest, NoTriggerOnSetpointJump) +{ + // Setpoint jumps 10m higher; vehicle position unchanged so ratchet stays at -97, drop = 0m. + EXPECT_FALSE(update(-97.f, -100.f)); // ratchet = -97, drop = 0m + EXPECT_FALSE(update(-97.f, -110.f)); // setpoint jumps, vehicle 13m below sp, but drop = 0m + EXPECT_FALSE(update(-97.f, -110.f)); // vehicle holds position, no trigger +} diff --git a/src/modules/commander/failure_detector/failure_detector_params.yaml b/src/modules/commander/failure_detector/failure_detector_params.yaml index e9da4bb84dc3..597067d88bf1 100644 --- a/src/modules/commander/failure_detector/failure_detector_params.yaml +++ b/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