mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-11-22 14:19:57 +08:00
feat(gnss_checks): always run strict checks to have the information for the selection algorithm, switch to simplified checks in-air when the strict checks fail
This commit is contained in:
1 parent
a0884c0a12
commit
493c86ce58
2 files changed
+18
-32
No files matched your search
@@ -59,33 +59,24 @@ bool GnssChecks::run(const gnssChecksSample &gnss, bool armed, bool in_air, bool
|
||||
_time_last_fail_us = gnss.time_us;
|
||||
}
|
||||
|
||||
// Run strict checks while disarmed on the ground
|
||||
if (!armed && !in_air) {
|
||||
_initial_checks_passed = false;
|
||||
_strict = true;
|
||||
_passed = false;
|
||||
|
||||
// always run the strict checks for the receiver selection algorithm
|
||||
_meets_requirements = runInitialFixChecks(gnss, in_air, vehicle_at_rest)
|
||||
&& isTimedOut(_time_last_fail_us, gnss.time_us, getRequiredPassDurationUs());
|
||||
|
||||
if (_meets_requirements) {
|
||||
_passed = true;
|
||||
}
|
||||
|
||||
_passed = false;
|
||||
_strict = !_initial_checks_passed;
|
||||
|
||||
if (_initial_checks_passed) {
|
||||
// relax the checks when flying for EKF fusion: allow the vehicle to continue in degraded conditions
|
||||
else if (armed && in_air) {
|
||||
_strict = false;
|
||||
clearDriftChecks();
|
||||
|
||||
if (runSimplifiedChecks(gnss)) {
|
||||
_passed = isTimedOut(_time_last_fail_us, gnss.time_us, getRequiredPassDurationUs());
|
||||
|
||||
} else {
|
||||
_time_last_fail_us = gnss.time_us;
|
||||
}
|
||||
|
||||
} else {
|
||||
if (runInitialFixChecks(gnss, in_air, vehicle_at_rest)) {
|
||||
if (isTimedOut(_time_last_fail_us, gnss.time_us, getRequiredPassDurationUs())) {
|
||||
_initial_checks_passed = true;
|
||||
_passed = true;
|
||||
}
|
||||
|
||||
} else {
|
||||
_time_last_fail_us = gnss.time_us;
|
||||
_passed = isTimedOut(_time_last_fail_us, gnss.time_us, getRequiredPassDurationUs(true));
|
||||
}
|
||||
}
|
||||
|
||||
@@ -94,14 +85,10 @@ bool GnssChecks::run(const gnssChecksSample &gnss, bool armed, bool in_air, bool
|
||||
|
||||
if (_passed) {
|
||||
_time_last_pass_us = gnss.time_us;
|
||||
}
|
||||
|
||||
// Same comparisons as the strict checks
|
||||
const uint16_t enabled_checks = getEnabledChecks();
|
||||
_meets_requirements = (!(enabled_checks & vehicle_gnss_s::CHECK_FIX) || !(gnss.fix_type < _params.req_fix))
|
||||
&& (!(enabled_checks & vehicle_gnss_s::CHECK_EPH) || !(gnss.hacc > _params.req_eph))
|
||||
&& (!(enabled_checks & vehicle_gnss_s::CHECK_EPV) || !(gnss.vacc > _params.req_epv))
|
||||
&& (!(enabled_checks & vehicle_gnss_s::CHECK_SACC) || !(gnss.sacc > _params.req_sacc));
|
||||
} else {
|
||||
_time_last_fail_us = gnss.time_us;
|
||||
}
|
||||
|
||||
return _passed;
|
||||
}
|
||||
|
||||
@@ -117,9 +117,9 @@ private:
|
||||
}
|
||||
|
||||
// How long the checks must pass after a failure before passed() is true
|
||||
uint64_t getRequiredPassDurationUs() const
|
||||
uint64_t getRequiredPassDurationUs(const bool simplified = false) const
|
||||
{
|
||||
return _initial_checks_passed ? math::max((uint64_t)1e6, (uint64_t)_params.min_health_time_us / 10)
|
||||
return simplified ? math::max((uint64_t)1e6, (uint64_t)_params.min_health_time_us / 10)
|
||||
: (uint64_t)_params.min_health_time_us;
|
||||
}
|
||||
|
||||
@@ -153,7 +153,6 @@ private:
|
||||
float _vel_d_filt{0.0f}; ///< GNSS filtered Down velocity (m/sec)
|
||||
uint64_t _time_last_fail_us{0};
|
||||
uint64_t _time_last_pass_us{0};
|
||||
bool _initial_checks_passed{false};
|
||||
bool _strict{true};
|
||||
bool _passed{false};
|
||||
bool _meets_requirements{false};
|
||||
|
||||
Reference in new issue
Block a user