From 8b12b7c6419b43eaf86d3235732afa64d1e3ece4 Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 31 Jul 2024 15:15:56 +0200 Subject: [PATCH] commander: extend local position 'relaxed' validity Relaxed position is valid as long as a velocity aiding source is active (e.g.: optical flow or airspeed+sideslip) --- .../checks/estimatorCheck.cpp | 26 +++---------------- .../checks/estimatorCheck.hpp | 2 -- 2 files changed, 4 insertions(+), 24 deletions(-) diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp index 2ede2baf63..888835ba7e 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp @@ -533,19 +533,6 @@ void EstimatorChecks::checkEstimatorStatusFlags(const Context &context, Report & estimator_status_flags_s estimator_status_flags; if (_estimator_status_flags_sub.copy(&estimator_status_flags)) { - - bool dead_reckoning = estimator_status_flags.cs_wind_dead_reckoning - || estimator_status_flags.cs_inertial_dead_reckoning; - - if (!dead_reckoning) { - // position requirements (update if not dead reckoning) - bool gps = estimator_status_flags.cs_gps; - bool optical_flow = estimator_status_flags.cs_opt_flow; - bool vision_position = estimator_status_flags.cs_ev_pos; - - _position_reliant_on_optical_flow = !gps && optical_flow && !vision_position; - } - // Check for a magnetometer fault and notify the user if (estimator_status_flags.cs_mag_fault) { /* EVENT @@ -722,15 +709,6 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f // Check if quality checking of position accuracy and consistency is to be performed const float lpos_eph_threshold = (_param_com_pos_fs_eph.get() < 0) ? INFINITY : _param_com_pos_fs_eph.get(); - float lpos_eph_threshold_relaxed = lpos_eph_threshold; - - // Set the allowable position uncertainty based on combination of flight and estimator state - // When we are in a operator demanded position control mode and are solely reliant on optical flow, - // do not check position error because it will gradually increase throughout flight and the operator will compensate for the drift - if (_position_reliant_on_optical_flow) { - lpos_eph_threshold_relaxed = INFINITY; - } - bool xy_valid = lpos.xy_valid && !_nav_test_failed; bool v_xy_valid = lpos.v_xy_valid && !_nav_test_failed; @@ -793,6 +771,10 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f !checkPosVelValidity(now, xy_valid, lpos.eph, lpos_eph_threshold, lpos.timestamp, _last_lpos_fail_time_us, !failsafe_flags.local_position_invalid); + + // In some modes we assume that the operator will compensate for the drift so we do not need to check the position error + const float lpos_eph_threshold_relaxed = INFINITY; + failsafe_flags.local_position_invalid_relaxed = !checkPosVelValidity(now, xy_valid, lpos.eph, lpos_eph_threshold_relaxed, lpos.timestamp, _last_lpos_relaxed_fail_time_us, !failsafe_flags.local_position_invalid_relaxed); diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp index 6578776ee9..4f61df8867 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp @@ -100,8 +100,6 @@ private: bool _nav_test_passed{false}; ///< true if the post takeoff navigation test has passed bool _nav_test_failed{false}; ///< true if the post takeoff navigation test has failed - bool _position_reliant_on_optical_flow{false}; - bool _gps_was_fused{false}; bool _gnss_spoofed{false};