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)
This commit is contained in:
bresch
2024-08-09 12:51:09 +02:00
committed by Mathieu Bresciani
parent f04aa2494b
commit 8b12b7c641
2 changed files with 4 additions and 24 deletions
@@ -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);
@@ -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};