Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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

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
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()) {
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;
}

21
libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
View File

@ -264,13 +264,18 @@ void NavEKF3_core::SelectMagFusion()
if (tiltAlignComplete && (!yawAlignComplete || yaw_source_reset)) {
alignYawAngle(yawAngDataDelayed);
yaw_source_reset = false;
have_fused_gps_yaw = true;
lastSynthYawTime_ms = imuSampleTime_ms;
last_gps_yaw_fuse_ms = imuSampleTime_ms;
} 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_fusion_ms = imuSampleTime_ms;
last_gps_yaw_ms = imuSampleTime_ms;
} 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
if(imuSampleTime_ms - lastSynthYawTime_ms > 140) {
yawAngDataDelayed.yawAngErr = MAX(frontend->_yawNoise, 0.05f);
@ -284,7 +289,11 @@ void NavEKF3_core::SelectMagFusion()
}
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) {
// no fallback
return;
@ -305,7 +314,7 @@ void NavEKF3_core::SelectMagFusion()
// we don't have GPS yaw data and are configured for
// 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
return;
}
@ -315,7 +324,7 @@ void NavEKF3_core::SelectMagFusion()
return;
}
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;
}
if (!gps_yaw_mag_fallback_active) {

3
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -1435,7 +1435,8 @@ private:
*/
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_active;
uint8_t gps_yaw_fallback_good_counter;