From 7c85d756f909d924622d723c1b463a3a7e0b35be Mon Sep 17 00:00:00 2001 From: Pernilla Date: Thu, 23 Oct 2025 12:09:53 +0200 Subject: [PATCH] Failsafe: add timer for non gps fused position estimates --- msg/FailsafeFlags.msg | 2 + .../checks/estimatorCheck.cpp | 61 +++++++++++++++++++ .../checks/estimatorCheck.hpp | 10 ++- .../commander/ModeUtil/mode_requirements.cpp | 2 + src/modules/commander/commander_params.c | 34 +++++++++++ src/modules/commander/failsafe/failsafe.cpp | 5 ++ src/modules/commander/failsafe/failsafe.h | 3 +- 7 files changed, 115 insertions(+), 2 deletions(-) diff --git a/msg/FailsafeFlags.msg b/msg/FailsafeFlags.msg index 243a4b5c9e..568e6d51cd 100644 --- a/msg/FailsafeFlags.msg +++ b/msg/FailsafeFlags.msg @@ -8,6 +8,7 @@ uint64 timestamp # time since system start (microseconds) # Per-mode requirements uint32 mode_req_angular_velocity uint32 mode_req_attitude +uint32 mode_req_dead_reckoning_invalid uint32 mode_req_local_alt uint32 mode_req_local_position uint32 mode_req_local_position_relaxed @@ -52,6 +53,7 @@ bool wind_limit_exceeded # Wind limit exceeded bool flight_time_limit_exceeded # Maximum flight time exceeded bool position_accuracy_low # Position estimate has dropped below threshold, but is currently still declared valid bool navigator_failure # Navigator failed to execute a mode +bool dead_reckoning_invalid # Postion estimate based on dead reckoning is too old to be trusted # Failure detector bool fd_critical_failure # Critical failure (attitude/altitude limit exceeded, or external ATS) diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp index add7456b59..90e3f28e38 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp @@ -131,6 +131,7 @@ void EstimatorChecks::checkAndReport(const Context &context, Report &reporter) lowPositionAccuracy(context, reporter, lpos); + deadReckoningTimeout(context, reporter, lpos); } void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &reporter, @@ -645,6 +646,66 @@ void EstimatorChecks::lowPositionAccuracy(const Context &context, Report &report reporter.failsafeFlags().position_accuracy_low = position_valid_but_low_accuracy; } +void EstimatorChecks::deadReckoningTimeout(const Context &context, Report &reporter, + const vehicle_local_position_s &lpos) +{ + + const hrt_abstime now = hrt_absolute_time(); + + vehicle_land_detected_s vehicle_land_detected; + _vehicle_land_detected_sub.copy(&vehicle_land_detected); + _in_air_valid = (now < vehicle_land_detected.timestamp + 2_s) && !vehicle_land_detected.landed; + + bool dead_reckoning_valid_but_timouted = false; + bool dead_reckoning_changed = _was_dead_reckoning != lpos.dead_reckoning; + + if (dead_reckoning_changed && _in_air_valid) { + if (lpos.dead_reckoning) { + _last_initiated_dead_reckoning_time_us = now; + + } else { + reporter.failsafeFlags().mode_req_dead_reckoning_invalid = true; + reporter.failsafeFlags().dead_reckoning_invalid = false; + _last_initiated_dead_reckoning_time_us = 0; + } + } + + + if (reporter.failsafeFlags().mode_req_dead_reckoning_invalid + && !reporter.failsafeFlags().dead_reckoning_invalid) { + + dead_reckoning_valid_but_timouted = (_param_com_dead_reckoning_tout_t.get() > FLT_EPSILON + && lpos.dead_reckoning + && now > _last_initiated_dead_reckoning_time_us + _param_com_dead_reckoning_tout_t.get() * 1_s); + + reporter.failsafeFlags().dead_reckoning_invalid = dead_reckoning_valid_but_timouted; + reporter.failsafeFlags().mode_req_dead_reckoning_invalid = !dead_reckoning_valid_but_timouted; + } + + if (dead_reckoning_valid_but_timouted && _param_com_dead_reckoning_tout_act.get()) { + + // only report if armed + if (context.isArmed()) { + /* EVENT + * @description Position estimates based on dead reckoning has surpassed the timeout for being trusted. Warn user. + * + * + * This check can be configured via COM_DR_TOUT_T and COM_DR_TOUT_ACT parameters. + * + */ + reporter.armingCheckFailure(NavModes::All, health_component_t::local_position_estimate, + events::ID("check_estimator_dead_reckoning_invalid"), + events::Log::Error, "Dead reckoning is too old to be trusted"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_warning(reporter.mavlink_log_pub(), "Dead reckoning is too old to be trusted\t"); + } + } + } + + _was_dead_reckoning = lpos.dead_reckoning; +} + void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_flt_fail_innov_heading, bool pre_flt_fail_innov_vel_horiz, bool pre_flt_fail_innov_pos_horiz, const vehicle_local_position_s &lpos, const sensor_gps_s &vehicle_gps_position, failsafe_flags_s &failsafe_flags, diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp index 13b351c986..8a7ea3ebd0 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp @@ -43,6 +43,7 @@ #include #include #include +#include #include #include @@ -72,6 +73,7 @@ private: const vehicle_local_position_s &lpos); void checkGps(const Context &context, Report &reporter, const sensor_gps_s &vehicle_gps_position) const; void lowPositionAccuracy(const Context &context, Report &reporter, const vehicle_local_position_s &lpos) const; + void deadReckoningTimeout(const Context &context, Report &reporter, const vehicle_local_position_s &lpos); void setModeRequirementFlags(const Context &context, bool pre_flt_fail_innov_heading, bool pre_flt_fail_innov_vel_horiz, bool pre_flt_fail_innov_pos_horiz, @@ -94,15 +96,19 @@ private: uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)}; + uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; hrt_abstime _last_gpos_fail_time_us{0}; ///< Last time that the global position validity recovery check failed (usec) hrt_abstime _last_gpos_relaxed_fail_time_us{0}; ///< Last time that the global position relaxed validity recovery check failed (usec) hrt_abstime _last_lpos_fail_time_us{0}; ///< Last time that the local position validity recovery check failed (usec) hrt_abstime _last_lpos_relaxed_fail_time_us{0}; ///< Last time that the relaxed local position validity recovery check failed (usec) hrt_abstime _last_lvel_fail_time_us{0}; ///< Last time that the local velocity validity recovery check failed (usec) + hrt_abstime _last_initiated_dead_reckoning_time_us{0}; ///< Last time that the dead reckoning initiated (usec) bool _gps_was_fused{false}; bool _gnss_spoofed{false}; + bool _in_air_valid{false}; + bool _was_dead_reckoning{false}; bool _nav_failure_imminent_warned{false}; @@ -114,6 +120,8 @@ private: (ParamFloat) _param_com_pos_fs_eph, (ParamFloat) _param_com_vel_fs_evh, (ParamFloat) _param_com_low_eph, - (ParamInt) _param_com_pos_low_act + (ParamInt) _param_com_pos_low_act, + (ParamInt) _param_com_dead_reckoning_tout_act, + (ParamFloat) _param_com_dead_reckoning_tout_t ) }; diff --git a/src/modules/commander/ModeUtil/mode_requirements.cpp b/src/modules/commander/ModeUtil/mode_requirements.cpp index 850a0cf6e9..96a2568f37 100644 --- a/src/modules/commander/ModeUtil/mode_requirements.cpp +++ b/src/modules/commander/ModeUtil/mode_requirements.cpp @@ -49,6 +49,7 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags) flags.mode_req_attitude = 0; flags.mode_req_local_position = 0; flags.mode_req_local_position_relaxed = 0; + flags.mode_req_dead_reckoning_invalid = 0; flags.mode_req_global_position = 0; flags.mode_req_global_position_relaxed = 0; flags.mode_req_local_alt = 0; @@ -97,6 +98,7 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags) if (vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_global_position_relaxed); setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_local_position_relaxed); + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_dead_reckoning_invalid); } else { setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_global_position); diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 2c53e36c66..d933daa127 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -967,6 +967,40 @@ PARAM_DEFINE_FLOAT(COM_POS_LOW_EPH, -1.0f); */ PARAM_DEFINE_INT32(COM_POS_LOW_ACT, 3); +/** + * Dead reckoning timeout failsafe threshold + * + * This triggers the action specified in COM_DR_TOUT_ACT if the dead reckoning has been active longer than this threshold. + * + * Set to -1 to disable. + * + * @min -1 + * @max 1000 + * @group Commander + * @unit s + */ +PARAM_DEFINE_FLOAT(COM_DR_TOUT_T, -1.0f); + +/** + * Dead reckoning timeout action + * + * Action the system takes when the dead reckoning has active for a specified time. + * See COM_DR_TOUT_ACT to set the failsafe threshold. + * The failsafe action is only executed if the vehicle is in auto mission, + * otherwise it is only a warning. + * + * @group Commander + * + * @value 0 None + * @value 1 Warning + * @value 2 Hold + * @value 3 Return + * @value 4 Terminate + * @value 5 Land + * @increment 1 + */ +PARAM_DEFINE_INT32(COM_DR_TOUT_ACT, 1); + /** * Flag to allow arming * diff --git a/src/modules/commander/failsafe/failsafe.cpp b/src/modules/commander/failsafe/failsafe.cpp index e366e12947..2e1a94e94f 100644 --- a/src/modules/commander/failsafe/failsafe.cpp +++ b/src/modules/commander/failsafe/failsafe.cpp @@ -549,6 +549,11 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state, CHECK_FAILSAFE(status_flags, position_accuracy_low, fromPosLowActParam(_param_com_pos_low_act.get())); } + // trigger dead reckoning Timeout Failsafe (only in auto mission) + if (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) { + CHECK_FAILSAFE(status_flags, dead_reckoning_invalid, fromPosLowActParam(_param_com_dead_reckoning_tout_act.get())); + } + if (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF || state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { CHECK_FAILSAFE(status_flags, navigator_failure, diff --git a/src/modules/commander/failsafe/failsafe.h b/src/modules/commander/failsafe/failsafe.h index 9a9384f621..83f31c8590 100644 --- a/src/modules/commander/failsafe/failsafe.h +++ b/src/modules/commander/failsafe/failsafe.h @@ -213,7 +213,8 @@ private: (ParamInt) _param_com_qc_act, (ParamInt) _param_com_wind_max_act, (ParamInt) _param_com_fltt_low_act, - (ParamInt) _param_com_pos_low_act + (ParamInt) _param_com_pos_low_act, + (ParamInt) _param_com_dead_reckoning_tout_act ); };