Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-18 06:38:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: Reset to GPS yaw if fusion times out when on ground.

Browse Source
This commit is contained in:
Paul Riseborough 2021-05-03 21:52:18 +10:00 committed by Andrew Tridgell
parent 0d43cb4622
commit b9abef37c3
3 changed files with 18 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -569,7 +569,7 @@ bool NavEKF3_core::using_external_yaw(void) const
#endif #endif
if (yaw_source == AP_NavEKF_Source::SourceYaw::GPS || yaw_source == AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK || if (yaw_source == AP_NavEKF_Source::SourceYaw::GPS || yaw_source == AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK ||
yaw_source == AP_NavEKF_Source::SourceYaw::GSF || !use_compass()) { yaw_source == AP_NavEKF_Source::SourceYaw::GSF || !use_compass()) {
return imuSampleTime_ms - last_gps_yaw_fusion_ms < 5000 || imuSampleTime_ms - lastSynthYawTime_ms < 5000; return imuSampleTime_ms - last_gps_yaw_ms < 5000 || imuSampleTime_ms - lastSynthYawTime_ms < 5000;
} }
return false; return false;
} }

21
libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
View File

@ -264,13 +264,18 @@ void NavEKF3_core::SelectMagFusion()
if (tiltAlignComplete && (!yawAlignComplete || yaw_source_reset)) { if (tiltAlignComplete && (!yawAlignComplete || yaw_source_reset)) {
alignYawAngle(yawAngDataDelayed); alignYawAngle(yawAngDataDelayed);
yaw_source_reset = false; yaw_source_reset = false;
have_fused_gps_yaw = true;
lastSynthYawTime_ms = imuSampleTime_ms;
last_gps_yaw_fuse_ms = imuSampleTime_ms;
} else if (tiltAlignComplete && yawAlignComplete) { } else if (tiltAlignComplete && yawAlignComplete) {
fuseEulerYaw(yawFusionMethod::GPS); have_fused_gps_yaw = fuseEulerYaw(yawFusionMethod::GPS);
if (have_fused_gps_yaw) {
last_gps_yaw_fuse_ms = imuSampleTime_ms;
}
} }
have_fused_gps_yaw = true; last_gps_yaw_ms = imuSampleTime_ms;
last_gps_yaw_fusion_ms = imuSampleTime_ms;
} else if (tiltAlignComplete && !yawAlignComplete) { } else if (tiltAlignComplete && !yawAlignComplete) {
// External yaw sources can take singificant time to start providing yaw data so // External yaw sources can take significant time to start providing yaw data so
// wuile waiting, fuse a 'fake' yaw observation at 7Hz to keeop the filter stable // wuile waiting, fuse a 'fake' yaw observation at 7Hz to keeop the filter stable
if(imuSampleTime_ms - lastSynthYawTime_ms > 140) { if(imuSampleTime_ms - lastSynthYawTime_ms > 140) {
yawAngDataDelayed.yawAngErr = MAX(frontend->_yawNoise, 0.05f); yawAngDataDelayed.yawAngErr = MAX(frontend->_yawNoise, 0.05f);
@ -284,7 +289,11 @@ void NavEKF3_core::SelectMagFusion()
} }
lastSynthYawTime_ms = imuSampleTime_ms; lastSynthYawTime_ms = imuSampleTime_ms;
} }
} else if (tiltAlignComplete && yawAlignComplete && onGround && imuSampleTime_ms - last_gps_yaw_fuse_ms > 10000) {
// handle scenario where were were using GPS yaw previously, but the yaw fusion has timed out.
yaw_source_reset = true;
} }
if (yaw_source == AP_NavEKF_Source::SourceYaw::GPS) { if (yaw_source == AP_NavEKF_Source::SourceYaw::GPS) {
// no fallback // no fallback
return; return;
@ -305,7 +314,7 @@ void NavEKF3_core::SelectMagFusion()
// we don't have GPS yaw data and are configured for // we don't have GPS yaw data and are configured for
// fallback. If we've only just lost GPS yaw // fallback. If we've only just lost GPS yaw
if (imuSampleTime_ms - last_gps_yaw_fusion_ms < 10000) { if (imuSampleTime_ms - last_gps_yaw_ms < 10000) {
// don't fallback to magnetometer fusion for 10s // don't fallback to magnetometer fusion for 10s
return; return;
} }
@ -315,7 +324,7 @@ void NavEKF3_core::SelectMagFusion()
return; return;
} }
if (!inFlight) { if (!inFlight) {
// don't fall back if not flying // don't fall back if not flying but reset to GPS yaw if it becomes available
return; return;
} }
if (!gps_yaw_mag_fallback_active) { if (!gps_yaw_mag_fallback_active) {

3
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -1435,7 +1435,8 @@ private:
*/ */
bool learnMagBiasFromGPS(void); bool learnMagBiasFromGPS(void);
uint32_t last_gps_yaw_fusion_ms; uint32_t last_gps_yaw_ms; // last time the EKF attempted to use the GPS yaw
uint32_t last_gps_yaw_fuse_ms; // last time the EKF successfully fused the GPS yaw
bool gps_yaw_mag_fallback_ok; bool gps_yaw_mag_fallback_ok;
bool gps_yaw_mag_fallback_active; bool gps_yaw_mag_fallback_active;
uint8_t gps_yaw_fallback_good_counter; uint8_t gps_yaw_fallback_good_counter;