From 68e47df055fc5fdbf5892c9ac9036608a62fbf8d Mon Sep 17 00:00:00 2001 From: gguidone Date: Fri, 13 Mar 2026 16:28:11 +0100 Subject: [PATCH] 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) --- msg/FailsafeFlags.msg | 3 +- .../checks/failureDetectorCheck.cpp | 4 +- src/modules/commander/failsafe/failsafe.cpp | 3 ++ .../failure_detector/FailureDetector.cpp | 41 +++++++++++++++++++ .../failure_detector/FailureDetector.hpp | 13 +++++- .../failure_detector_params.yaml | 23 +++++++++++ 6 files changed, 83 insertions(+), 4 deletions(-) diff --git a/msg/FailsafeFlags.msg b/msg/FailsafeFlags.msg index 2d974791f7..86f42f178e 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 0b5715662d..25370a2d9b 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 25d9728cc6..88a0f4e582 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/FailureDetector.cpp b/src/modules/commander/failure_detector/FailureDetector.cpp index baee7de486..e2160fd254 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,43 @@ 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; diff --git a/src/modules/commander/failure_detector/FailureDetector.hpp b/src/modules/commander/failure_detector/FailureDetector.hpp index 4318f43156..658b973cf4 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 @@ public: 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,11 @@ private: 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 _imbalanced_prop_lpf{}; uint32_t _selected_accel_device_id{0}; @@ -105,6 +112,8 @@ private: 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 @@ private: (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/failure_detector_params.yaml b/src/modules/commander/failure_detector/failure_detector_params.yaml index e9da4bb84d..597067d88b 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