Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
26 changes: 23 additions & 3 deletions Tools/autotest/arducopter.py
Original file line number Diff line number Diff line change
Expand Up @@ -23587,6 +23587,15 @@ def CompassLearnCopyFromEKF(self):
self.assert_prearm_failure("Check mag field", other_prearm_failures_fatal=False)
self.context_pop()
self.wait_ready_to_arm()
new_compass_ofs_x = self.learn_compass_offsets_in_flight()

self.do_RTL()
self.assert_parameter_value("COMPASS_OFS_X", new_compass_ofs_x, epsilon=30)
self.reboot_sitl()
self.assert_parameter_value("COMPASS_OFS_X", new_compass_ofs_x, epsilon=30)

def learn_compass_offsets_in_flight(self):
'''take off and fly so the EKF learns a new compass X offset, returns the offset it should learn'''
self.takeoff(30, mode='ALT_HOLD')
# prevent EKF switching to good compass:
self.set_parameters({
Expand Down Expand Up @@ -23627,11 +23636,21 @@ def CompassLearnCopyFromEKF(self):
self.set_rc(1, 1500)
self.set_rc(2, 1500)
self.set_rc(4, 1500)
return new_compass_ofs_x

self.do_RTL()
self.assert_parameter_value("COMPASS_OFS_X", new_compass_ofs_x, epsilon=30)
def CompassLearnDisabledDoesNotSave(self):
'''check EKF-learned compass offsets are not saved if COMPASS_LEARN is turned off before landing'''
self.context_preserve_parameters([
"COMPASS_OFS_X", "COMPASS_OFS_Y", "COMPASS_OFS_Z",
"COMPASS_OFS2_X", "COMPASS_OFS2_Y", "COMPASS_OFS2_Z",
"COMPASS_OFS3_X", "COMPASS_OFS3_Y", "COMPASS_OFS3_Z",
])
self.reboot_sitl()
self.assert_parameter_value("COMPASS_OFS_X", new_compass_ofs_x, epsilon=30)
self.wait_ready_to_arm()
self.learn_compass_offsets_in_flight()
self.set_parameter("COMPASS_LEARN", 0)
self.do_RTL()
self.assert_parameter_value("COMPASS_OFS_X", 20, epsilon=30)

def CompassLearnCopyFromEKFAffinity(self):
'''check EKF-learned offsets are saved for several compasses at once'''
Expand Down Expand Up @@ -25294,6 +25313,7 @@ def tests2d(self):
self.CommonOrigin,
self.RC_OPTIONS_1_FS_THR_ENABLE_0,
self.CompassLearnCopyFromEKFAffinity,
self.CompassLearnDisabledDoesNotSave,
self.AHRSAutoTrim,
self.AHRSTrim,
self.PeriphMultiUARTTunnel,
Expand Down
9 changes: 5 additions & 4 deletions libraries/AP_Arming/AP_Arming.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -2023,10 +2023,11 @@ bool AP_Arming::disarm(const AP_Arming::Method method, bool do_disarm_checks)
#endif // HAL_HAVE_SAFETY_SWITCH

#if AP_COMPASS_LEARN_COPY_FROM_EKF_ENABLED
// save any compass offsets the EKF has learned. This must be done
// before the vehicle calls hal.util->set_soft_armed(false); once the
// EKF sees onGround it clears finalInflightMagInit and will no
// longer hand out learned offsets.
// save any compass offsets the EKF has learned
// EKF3 holds the learned field until the next arm so it is still available after landing.
// EKF2 does not so this must be done before the vehicle calls hal.util->set_soft_armed(false)
// because once EKF2 sees onGround it clears finalInflightMagInit and stops handing out
// learned offsets.
AP::compass().save_ekf_learned_offsets();
#endif // AP_COMPASS_LEARN_COPY_FROM_EKF_ENABLED

Expand Down
15 changes: 15 additions & 0 deletions libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -126,6 +126,11 @@ void NavEKF3_core::setWindMagStateLearningMode()
bool magCalDenied = !use_compass() || (effectiveMagCal == MagCal::NEVER) ||
(onGround && effectiveMagCal != MagCal::ALWAYS && effectiveMagCal != MagCal::GROUND_AND_INFLIGHT);

// if the compass is no longer being used clear the learned mag field
if (!use_compass()) {
learnedMagField.clear();
}

// Inhibit the magnetic field calibration if not requested or denied
bool setMagInhibit = !magCalRequested || magCalDenied;
if (!inhibitMagStates && setMagInhibit) {
Expand Down Expand Up @@ -191,6 +196,16 @@ void NavEKF3_core::setWindMagStateLearningMode()
magFieldLearned = false;
}

// if the body magnetic field is valid, update the learnedMagField
const ftype maxMagVar = 5E-6f;
if (finalInflightMagInit &&
!inhibitMagStates &&
use_compass() &&
dal.compass().healthy(magSelectIndex) &&
(P[19][19] < maxMagVar) && (P[20][20] < maxMagVar) && (P[21][21] < maxMagVar)) {
learnedMagField.set(magSelectIndex, stateStruct.body_magfield);
}

updateStateIndexLim();
}

Expand Down
4 changes: 4 additions & 0 deletions libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -293,6 +293,8 @@ void NavEKF3_core::tryChangeCompass(uint8_t mag_index)
magFieldLearned = false;
// reset body mag variances on next CovariancePrediction
needMagBodyVarReset = true;
// clear the learned magnetic field
learnedMagField.clear();
return;
}
}
Expand Down Expand Up @@ -347,6 +349,8 @@ void NavEKF3_core::readMagData()
stateStruct.body_magfield.zero();
// clear the measurement buffer
storedMag.reset();
// clear the learned magnetic field
learnedMagField.clear();

// reset body mag variances on next
// CovariancePrediction. This copes with possible errors
Expand Down
16 changes: 6 additions & 10 deletions libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -427,16 +427,12 @@ bool NavEKF3_core::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const
return false;
}

// compass offsets are valid if we have finalised magnetic field initialisation, magnetic field learning is not prohibited,
// primary compass is valid and state variances have converged
const float maxMagVar = 5E-6f;
bool variancesConverged = (P[19][19] < maxMagVar) && (P[20][20] < maxMagVar) && (P[21][21] < maxMagVar);
if ((mag_idx == magSelectIndex) &&
finalInflightMagInit &&
!inhibitMagStates &&
compass.healthy(magSelectIndex) &&
variancesConverged) {
magOffsets = compass.get_offsets(magSelectIndex) - stateStruct.body_magfield.tofloat()*1000.0;
// compass offsets are valid if a learned field was copied for this compass while the state variances
// were converged (see setWindMagStateLearningMode) and the compass is healthy
Vector3F learnedField;
if (compass.healthy(mag_idx) &&
learnedMagField.get(mag_idx, learnedField)) {
magOffsets = compass.get_offsets(mag_idx) - learnedField.tofloat()*1000.0;
return true;
} else {
magOffsets = compass.get_offsets(magSelectIndex);
Expand Down
5 changes: 5 additions & 0 deletions libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -446,6 +446,11 @@ void NavEKF3_core::detectFlight()
yawEstimator->setGyroBias(gyroBias);
}

// clear learned mag field on takeoff
if (prevOnGround && !onGround) {
learnedMagField.clear();
}

// store current on-ground and in-air status for next time
prevOnGround = onGround;
prevInFlight = inFlight;
Expand Down
3 changes: 3 additions & 0 deletions libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -520,6 +520,9 @@ bool NavEKF3_core::InitialiseFilterBootstrap(void)
stateStruct.earth_magfield.zero();
stateStruct.body_magfield.zero();

// clear the learned body magnetic field
learnedMagField.clear();

// set the position, velocity and height
ResetVelocity(resetDataSource::DEFAULT);
ResetPosition(resetDataSource::DEFAULT);
Expand Down
28 changes: 28 additions & 0 deletions libraries/AP_NavEKF3/AP_NavEKF3_core.h
Original file line number Diff line number Diff line change
Expand Up @@ -1282,6 +1282,34 @@ class NavEKF3_core : public NavEKF_core_common
uint32_t wasLearningCompass_ms; // time when we were last waiting for compass learn to complete
Vector3F earthMagFieldVar; // NED earth mag field variances for last learned field (mGauss^2)
Vector3F bodyMagFieldVar; // XYZ body mag field variances for last learned field (mGauss^2)

// LearnedMagField maintains the learned body magnetic field so it can be saved after landing
class LearnedMagField {
public:
// store the learned body magnetic field of the given compass and mark it as valid
void set(uint8_t idx, const Vector3F &field) {
mag_idx = idx;
body_magfield = field;
valid = true;
}

// retrieve the learned body magnetic field of the given compass
// returns true if the learned field is valid and for this compass, false otherwise
bool get(uint8_t idx, Vector3F &field) const {
if (!valid || idx != mag_idx) { return false; }
field = body_magfield;
return true;
}

// clear the learned body magnetic field and mark it as invalid
void clear() { valid = false; }

private:
uint8_t mag_idx; // index of the compass the learned field is for
Vector3F body_magfield; // learned body magnetic field (Gauss)
bool valid; // true if the learned body magnetic field is valid, false otherwise
} learnedMagField;

bool delAngBiasLearned; // true when the gyro bias has been learned
nav_filter_status filterStatus; // contains the status of various filter outputs
ftype ekfOriginHgtVar; // Variance of the EKF WGS-84 origin height estimate (m^2)
Expand Down
Loading