diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp index 90e3f28e38..8d575f74e5 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp @@ -656,30 +656,20 @@ void EstimatorChecks::deadReckoningTimeout(const Context &context, Report &repor _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 (!lpos.dead_reckoning || !_in_air_valid) { + _last_not_dead_reckoning_time_us = now; - 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; - } } + bool dead_reckoning_valid_but_timouted = false; - if (reporter.failsafeFlags().mode_req_dead_reckoning_invalid - && !reporter.failsafeFlags().dead_reckoning_invalid) { + if ((reporter.failsafeFlags().mode_req_global_position && !reporter.failsafeFlags().global_position_invalid) || + (reporter.failsafeFlags().mode_req_global_position_relaxed + && !reporter.failsafeFlags().global_position_invalid_relaxed) || + (reporter.failsafeFlags().mode_req_local_position && !reporter.failsafeFlags().local_position_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; + dead_reckoning_valid_but_timouted = (_last_not_dead_reckoning_time_us != 0 + && now > _last_not_dead_reckoning_time_us + _param_com_dead_reckoning_tout_t.get() * 1_s); } if (dead_reckoning_valid_but_timouted && _param_com_dead_reckoning_tout_act.get()) { @@ -703,7 +693,7 @@ void EstimatorChecks::deadReckoningTimeout(const Context &context, Report &repor } } - _was_dead_reckoning = lpos.dead_reckoning; + reporter.failsafeFlags().dead_reckoning_invalid = dead_reckoning_valid_but_timouted; } void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_flt_fail_innov_heading, diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp index 8a7ea3ebd0..bc23df6e99 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp @@ -103,12 +103,11 @@ private: 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) + hrt_abstime _last_not_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}; diff --git a/src/modules/commander/ModeUtil/mode_requirements.cpp b/src/modules/commander/ModeUtil/mode_requirements.cpp index 96a2568f37..3f1c0bd500 100644 --- a/src/modules/commander/ModeUtil/mode_requirements.cpp +++ b/src/modules/commander/ModeUtil/mode_requirements.cpp @@ -98,7 +98,6 @@ 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/failsafe/failsafe.cpp b/src/modules/commander/failsafe/failsafe.cpp index 2e1a94e94f..dd8f1bc8aa 100644 --- a/src/modules/commander/failsafe/failsafe.cpp +++ b/src/modules/commander/failsafe/failsafe.cpp @@ -550,7 +550,8 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state, } // trigger dead reckoning Timeout Failsafe (only in auto mission) - if (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) { + if (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION || + state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER) { CHECK_FAILSAFE(status_flags, dead_reckoning_invalid, fromPosLowActParam(_param_com_dead_reckoning_tout_act.get())); }