fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 02:33:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: estimator aid source status (GPS pos, GPS vel)

Browse Source
This commit is contained in:
Daniel Agar
2022-05-23 16:21:49 -04:00
parent 1ae467e9cd
commit 639222dd65
16 changed files with 813 additions and 628 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/gps_control.cpp
+10 -1
View File
@@ -48,6 +48,14 @@ void Ekf::controlGpsFusion()
// Check for new GPS data that has fallen behind the fusion time horizon
if (_gps_data_ready) {
// reset flags
resetEstimatorAidStatusFlags(_aid_src_gnss_vel);
resetEstimatorAidStatusFlags(_aid_src_gnss_pos);
updateGpsVel(_gps_sample_delayed);
updateGpsPos(_gps_sample_delayed);
const bool gps_checks_passing = isTimedOut(_last_gps_fail_us, (uint64_t)5e6);
const bool gps_checks_failing = isTimedOut(_last_gps_pass_us, (uint64_t)5e6);
@@ -67,7 +75,8 @@ void Ekf::controlGpsFusion()
if (continuing_conditions_passing
|| !isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) {
fuseGpsVelPos();
fuseGpsVel();
fuseGpsPos();
if (shouldResetGpsFusion()) {
const bool was_gps_signal_lost = isTimedOut(_time_prev_gps_us, 1000000);