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

AP_NavEKF3: Change yaw source to stop compass use when calibrating

This commit is contained in:
Paul Riseborough 2024-04-17 10:01:21 +10:00 committed by Randy Mackay
parent 533b2ed7be
commit 3c8b3be7a7
4 changed files with 48 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -209,6 +209,24 @@ void NavEKF3_core::updateStateIndexLim()
} }
} }
// set the default yaw source
void NavEKF3_core::setYawSource()
{
AP_NavEKF_Source::SourceYaw yaw_source = frontend->sources.getYawSource();
if (wasLearningCompass_ms > 0) {
// can't use compass while it is being calibrated
if (yaw_source == AP_NavEKF_Source::SourceYaw::COMPASS) {
yaw_source = AP_NavEKF_Source::SourceYaw::NONE;
} else if (yaw_source == AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK) {
yaw_source = AP_NavEKF_Source::SourceYaw::GPS;
}
}
if (yaw_source != yaw_source_last) {
yaw_source_last = yaw_source;
yaw_source_reset = true;
}
}
// Set inertial navigation aiding mode // Set inertial navigation aiding mode
void NavEKF3_core::setAidingMode() void NavEKF3_core::setAidingMode()
{ {
@ -218,6 +236,8 @@ void NavEKF3_core::setAidingMode()
// Save the previous status so we can detect when it has changed // Save the previous status so we can detect when it has changed
PV_AidingModePrev = PV_AidingMode; PV_AidingModePrev = PV_AidingMode;
setYawSource();
// Check that the gyro bias variance has converged // Check that the gyro bias variance has converged
checkGyroCalStatus(); checkGyroCalStatus();
@ -583,9 +603,8 @@ bool NavEKF3_core::readyToUseExtNav(void) const
// return true if we should use the compass // return true if we should use the compass
bool NavEKF3_core::use_compass(void) const bool NavEKF3_core::use_compass(void) const
{ {
const AP_NavEKF_Source::SourceYaw yaw_source = frontend->sources.getYawSource(); if ((yaw_source_last != AP_NavEKF_Source::SourceYaw::COMPASS) &&
if ((yaw_source != AP_NavEKF_Source::SourceYaw::COMPASS) && (yaw_source_last != AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK)) {
(yaw_source != AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK)) {
// not using compass as a yaw source // not using compass as a yaw source
return false; return false;
} }
@ -598,14 +617,13 @@ bool NavEKF3_core::use_compass(void) const
// are we using (aka fusing) a non-compass yaw? // are we using (aka fusing) a non-compass yaw?
bool NavEKF3_core::using_noncompass_for_yaw(void) const bool NavEKF3_core::using_noncompass_for_yaw(void) const
{ {
const AP_NavEKF_Source::SourceYaw yaw_source = frontend->sources.getYawSource();
#if EK3_FEATURE_EXTERNAL_NAV #if EK3_FEATURE_EXTERNAL_NAV
if (yaw_source == AP_NavEKF_Source::SourceYaw::EXTNAV) { if (yaw_source_last == AP_NavEKF_Source::SourceYaw::EXTNAV) {
return ((imuSampleTime_ms - last_extnav_yaw_fusion_ms < 5000) || (imuSampleTime_ms - lastSynthYawTime_ms < 5000)); return ((imuSampleTime_ms - last_extnav_yaw_fusion_ms < 5000) || (imuSampleTime_ms - lastSynthYawTime_ms < 5000));
} }
#endif #endif
if (yaw_source == AP_NavEKF_Source::SourceYaw::GPS || yaw_source == AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK || if (yaw_source_last == AP_NavEKF_Source::SourceYaw::GPS || yaw_source_last == AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK ||
yaw_source == AP_NavEKF_Source::SourceYaw::GSF || !use_compass()) { yaw_source_last == AP_NavEKF_Source::SourceYaw::GSF || !use_compass()) {
return imuSampleTime_ms - last_gps_yaw_ms < 5000 || imuSampleTime_ms - lastSynthYawTime_ms < 5000; return imuSampleTime_ms - last_gps_yaw_ms < 5000 || imuSampleTime_ms - lastSynthYawTime_ms < 5000;
} }
return false; return false;
@ -615,7 +633,7 @@ bool NavEKF3_core::using_noncompass_for_yaw(void) const
bool NavEKF3_core::using_extnav_for_yaw() const bool NavEKF3_core::using_extnav_for_yaw() const
{ {
#if EK3_FEATURE_EXTERNAL_NAV #if EK3_FEATURE_EXTERNAL_NAV
if (frontend->sources.getYawSource() == AP_NavEKF_Source::SourceYaw::EXTNAV) { if (yaw_source_last == AP_NavEKF_Source::SourceYaw::EXTNAV) {
return ((imuSampleTime_ms - last_extnav_yaw_fusion_ms < 5000) || (imuSampleTime_ms - lastSynthYawTime_ms < 5000)); return ((imuSampleTime_ms - last_extnav_yaw_fusion_ms < 5000) || (imuSampleTime_ms - lastSynthYawTime_ms < 5000));
} }
#endif #endif
@ -685,9 +703,8 @@ void NavEKF3_core::checkGyroCalStatus(void)
{ {
// check delta angle bias variances // check delta angle bias variances
const ftype delAngBiasVarMax = sq(radians(0.15 * dtEkfAvg)); const ftype delAngBiasVarMax = sq(radians(0.15 * dtEkfAvg));
const AP_NavEKF_Source::SourceYaw yaw_source = frontend->sources.getYawSource(); if (!use_compass() && (yaw_source_last != AP_NavEKF_Source::SourceYaw::GPS) && (yaw_source_last != AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK) &&
if (!use_compass() && (yaw_source != AP_NavEKF_Source::SourceYaw::GPS) && (yaw_source != AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK) && (yaw_source_last != AP_NavEKF_Source::SourceYaw::EXTNAV)) {
(yaw_source != AP_NavEKF_Source::SourceYaw::EXTNAV)) {
// rotate the variances into earth frame and evaluate horizontal terms only as yaw component is poorly observable without a yaw reference // rotate the variances into earth frame and evaluate horizontal terms only as yaw component is poorly observable without a yaw reference
// which can make this check fail // which can make this check fail
const Vector3F delAngBiasVarVec { P[10][10], P[11][11], P[12][12] }; const Vector3F delAngBiasVarVec { P[10][10], P[11][11], P[12][12] };

35
libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
View File

@ -158,7 +158,7 @@ void NavEKF3_core::realignYawGPS(bool emergency_reset)
if (badMagYaw) { if (badMagYaw) {
// attempt to use EKF-GSF estimate if available as it is more robust to GPS glitches // attempt to use EKF-GSF estimate if available as it is more robust to GPS glitches
// by default fly forward vehicles use ground course for initial yaw unless the GSF is explicitly selected as the yaw source // by default fly forward vehicles use ground course for initial yaw unless the GSF is explicitly selected as the yaw source
const bool useGSF = !assume_zero_sideslip() || (frontend->sources.getYawSource() == AP_NavEKF_Source::SourceYaw::GSF); const bool useGSF = !assume_zero_sideslip() || (yaw_source_last == AP_NavEKF_Source::SourceYaw::GSF);
if (useGSF && EKFGSF_resetMainFilterYaw(emergency_reset)) { if (useGSF && EKFGSF_resetMainFilterYaw(emergency_reset)) {
return; return;
} }
@ -221,13 +221,6 @@ void NavEKF3_core::SelectMagFusion()
// used for load levelling // used for load levelling
magFusePerformed = false; magFusePerformed = false;
// get default yaw source
const AP_NavEKF_Source::SourceYaw yaw_source = frontend->sources.getYawSource();
if (yaw_source != yaw_source_last) {
yaw_source_last = yaw_source;
yaw_source_reset = true;
}
// Store yaw angle when moving for use as a static reference when not moving // Store yaw angle when moving for use as a static reference when not moving
if (!onGroundNotMoving) { if (!onGroundNotMoving) {
if (fabsF(prevTnb[0][2]) < fabsF(prevTnb[1][2])) { if (fabsF(prevTnb[0][2]) < fabsF(prevTnb[1][2])) {
@ -245,13 +238,13 @@ void NavEKF3_core::SelectMagFusion()
// Handle case where we are not using a yaw sensor of any type and attempt to reset the yaw in // Handle case where we are not using a yaw sensor of any type and attempt to reset the yaw in
// flight using the output from the GSF yaw estimator or GPS ground course. // flight using the output from the GSF yaw estimator or GPS ground course.
if ((yaw_source == AP_NavEKF_Source::SourceYaw::GSF) || if ((yaw_source_last == AP_NavEKF_Source::SourceYaw::GSF) ||
(!use_compass() && (!use_compass() &&
yaw_source != AP_NavEKF_Source::SourceYaw::GPS && yaw_source_last != AP_NavEKF_Source::SourceYaw::GPS &&
yaw_source != AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK && yaw_source_last != AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK &&
yaw_source != AP_NavEKF_Source::SourceYaw::EXTNAV)) { yaw_source_last != AP_NavEKF_Source::SourceYaw::EXTNAV)) {
if ((!yawAlignComplete || yaw_source_reset) && ((yaw_source != AP_NavEKF_Source::SourceYaw::GSF) || (EKFGSF_yaw_valid_count >= GSF_YAW_VALID_HISTORY_THRESHOLD))) { if ((!yawAlignComplete || yaw_source_reset) && ((yaw_source_last != AP_NavEKF_Source::SourceYaw::GSF) || (EKFGSF_yaw_valid_count >= GSF_YAW_VALID_HISTORY_THRESHOLD))) {
realignYawGPS(false); realignYawGPS(false);
yaw_source_reset = false; yaw_source_reset = false;
} else { } else {
@ -262,7 +255,7 @@ void NavEKF3_core::SelectMagFusion()
// use the EKF-GSF yaw estimator output as this is more robust than the EKF can achieve without a yaw measurement // use the EKF-GSF yaw estimator output as this is more robust than the EKF can achieve without a yaw measurement
// for non fixed wing platform types // for non fixed wing platform types
ftype gsfYaw, gsfYawVariance; ftype gsfYaw, gsfYawVariance;
const bool didUseEKFGSF = yawAlignComplete && (yaw_source == AP_NavEKF_Source::SourceYaw::GSF) && EKFGSF_getYaw(gsfYaw, gsfYawVariance) && !assume_zero_sideslip() && fuseEulerYaw(yawFusionMethod::GSF); const bool didUseEKFGSF = yawAlignComplete && (yaw_source_last == AP_NavEKF_Source::SourceYaw::GSF) && EKFGSF_getYaw(gsfYaw, gsfYawVariance) && !assume_zero_sideslip() && fuseEulerYaw(yawFusionMethod::GSF);
// fallback methods // fallback methods
if (!didUseEKFGSF) { if (!didUseEKFGSF) {
@ -283,7 +276,7 @@ void NavEKF3_core::SelectMagFusion()
} }
// Handle case where we are using GPS yaw sensor instead of a magnetomer // Handle case where we are using GPS yaw sensor instead of a magnetomer
if (yaw_source == AP_NavEKF_Source::SourceYaw::GPS || yaw_source == AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK) { if (yaw_source_last == AP_NavEKF_Source::SourceYaw::GPS || yaw_source_last == AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK) {
bool have_fused_gps_yaw = false; bool have_fused_gps_yaw = false;
if (storedYawAng.recall(yawAngDataDelayed,imuDataDelayed.time_ms)) { if (storedYawAng.recall(yawAngDataDelayed,imuDataDelayed.time_ms)) {
if (tiltAlignComplete && (!yawAlignComplete || yaw_source_reset)) { if (tiltAlignComplete && (!yawAlignComplete || yaw_source_reset)) {
@ -319,7 +312,7 @@ void NavEKF3_core::SelectMagFusion()
yaw_source_reset = true; yaw_source_reset = true;
} }
if (yaw_source == AP_NavEKF_Source::SourceYaw::GPS) { if (yaw_source_last == AP_NavEKF_Source::SourceYaw::GPS) {
// no fallback // no fallback
return; return;
} }
@ -362,7 +355,7 @@ void NavEKF3_core::SelectMagFusion()
#if EK3_FEATURE_EXTERNAL_NAV #if EK3_FEATURE_EXTERNAL_NAV
// Handle case where we are using an external nav for yaw // Handle case where we are using an external nav for yaw
const bool extNavYawDataToFuse = storedExtNavYawAng.recall(extNavYawAngDataDelayed, imuDataDelayed.time_ms); const bool extNavYawDataToFuse = storedExtNavYawAng.recall(extNavYawAngDataDelayed, imuDataDelayed.time_ms);
if (yaw_source == AP_NavEKF_Source::SourceYaw::EXTNAV) { if (yaw_source_last == AP_NavEKF_Source::SourceYaw::EXTNAV) {
if (extNavYawDataToFuse) { if (extNavYawDataToFuse) {
if (tiltAlignComplete && (!yawAlignComplete || yaw_source_reset)) { if (tiltAlignComplete && (!yawAlignComplete || yaw_source_reset)) {
alignYawAngle(extNavYawAngDataDelayed); alignYawAngle(extNavYawAngDataDelayed);
@ -397,7 +390,7 @@ void NavEKF3_core::SelectMagFusion()
magTimeout = true; magTimeout = true;
} }
if (yaw_source != AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK) { if (yaw_source_last != AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK) {
// check for and read new magnetometer measurements. We don't // check for and read new magnetometer measurements. We don't
// read for GPS_COMPASS_FALLBACK as it has already been read // read for GPS_COMPASS_FALLBACK as it has already been read
// above // above
@ -409,8 +402,8 @@ void NavEKF3_core::SelectMagFusion()
// Control reset of yaw and magnetic field states if we are using compass data // Control reset of yaw and magnetic field states if we are using compass data
if (magDataToFuse) { if (magDataToFuse) {
if (yaw_source_reset && (yaw_source == AP_NavEKF_Source::SourceYaw::COMPASS || if (yaw_source_reset && (yaw_source_last == AP_NavEKF_Source::SourceYaw::COMPASS ||
yaw_source == AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK)) { yaw_source_last == AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK)) {
magYawResetRequest = true; magYawResetRequest = true;
yaw_source_reset = false; yaw_source_reset = false;
} }
@ -1573,7 +1566,7 @@ bool NavEKF3_core::EKFGSF_resetMainFilterYaw(bool emergency_reset)
EKFGSF_yaw_reset_ms = imuSampleTime_ms; EKFGSF_yaw_reset_ms = imuSampleTime_ms;
EKFGSF_yaw_reset_count++; EKFGSF_yaw_reset_count++;
if ((frontend->sources.getYawSource() == AP_NavEKF_Source::SourceYaw::GSF) || if ((yaw_source_last == AP_NavEKF_Source::SourceYaw::GSF) ||
!use_compass() || (dal.compass().get_num_enabled() == 0)) { !use_compass() || (dal.compass().get_num_enabled() == 0)) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u yaw aligned using GPS",(unsigned)imu_index); GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u yaw aligned using GPS",(unsigned)imu_index);
} else { } else {

13
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -295,16 +295,10 @@ void NavEKF3_core::readMagData()
} }
if (compass.learn_offsets_enabled()) { if (compass.learn_offsets_enabled()) {
// while learning offsets keep all mag states reset
InitialiseVariablesMag();
wasLearningCompass_ms = imuSampleTime_ms; wasLearningCompass_ms = imuSampleTime_ms;
} else if (wasLearningCompass_ms != 0 && imuSampleTime_ms - wasLearningCompass_ms > 1000) { } else if (wasLearningCompass_ms != 0 && imuSampleTime_ms - wasLearningCompass_ms > 1000) {
// allow time for old data to clear the buffer before signalling other code that compass data can be used
wasLearningCompass_ms = 0; wasLearningCompass_ms = 0;
// force a new yaw alignment 1s after learning completes. The
// delay is to ensure any buffered mag samples are discarded
yawAlignComplete = false;
yawAlignGpsValidCount = 0;
InitialiseVariablesMag();
} }
// If the magnetometer has timed out (been rejected for too long), we find another magnetometer to use if available // If the magnetometer has timed out (been rejected for too long), we find another magnetometer to use if available
@ -1329,9 +1323,8 @@ ftype NavEKF3_core::MagDeclination(void) const
*/ */
void NavEKF3_core::updateMovementCheck(void) void NavEKF3_core::updateMovementCheck(void)
{ {
const AP_NavEKF_Source::SourceYaw yaw_source = frontend->sources.getYawSource(); const bool runCheck = onGround && (yaw_source_last == AP_NavEKF_Source::SourceYaw::GPS || yaw_source_last == AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK ||
const bool runCheck = onGround && (yaw_source == AP_NavEKF_Source::SourceYaw::GPS || yaw_source == AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK || yaw_source_last == AP_NavEKF_Source::SourceYaw::EXTNAV || yaw_source_last == AP_NavEKF_Source::SourceYaw::GSF || !use_compass());
yaw_source == AP_NavEKF_Source::SourceYaw::EXTNAV || yaw_source == AP_NavEKF_Source::SourceYaw::GSF || !use_compass());
if (!runCheck) if (!runCheck)
{ {
onGroundNotMoving = false; onGroundNotMoving = false;

3
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -886,6 +886,9 @@ private:
// Determine if we are flying or on the ground // Determine if we are flying or on the ground
void detectFlight(); void detectFlight();
// set the default yaw source
void setYawSource();
// Set inertial navigation aiding mode // Set inertial navigation aiding mode
void setAidingMode(); void setAidingMode();