Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 15:23:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF : Compensate mag bias states for external copass offset changes

Browse Source
This commit is contained in:
Paul Riseborough 2015-03-16 15:18:40 -07:00 committed by Andrew Tridgell
parent 14795719f6
commit a1351e73ab
2 changed files with 18 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -3654,12 +3654,12 @@ void NavEKF::getMagXYZ(Vector3f &magXYZ) const
// return true if offsets are valid
bool NavEKF::getMagOffsets(Vector3f &magOffsets) const
{
// compass offsets are valid if we have finalised magnetic field initialisation and magnetic field learning is not prohibited
if (secondMagYawInit && (_magCal != 2)) {
magOffsets = _ahrs->get_compass()->get_offsets() - state.body_magfield*1000.0f;
// compass offsets are valid if we have finalised magnetic field initialisation and magnetic field learning is not prohibited and primary compass is valid
if (secondMagYawInit && (_magCal != 2) && _ahrs->get_compass()->healthy(0)) {
magOffsets = _ahrs->get_compass()->get_offsets(0) - state.body_magfield*1000.0f;
return true;
} else {
magOffsets = _ahrs->get_compass()->get_offsets();
magOffsets = _ahrs->get_compass()->get_offsets(0);
return false;
}
}
@ -4084,6 +4084,19 @@ void NavEKF::readMagData()
// let other processes know that new compass data has arrived
newDataMag = true;
// check if compass offsets have ben changed and adjust EKF bias states to maintain consistent innovations
if (_ahrs->get_compass()->healthy(0)) {
Vector3f nowMagOffsets = _ahrs->get_compass()->get_offsets(0);
bool changeDetected = ((nowMagOffsets.x != lastMagOffsets.x) || (nowMagOffsets.y != lastMagOffsets.y) || (nowMagOffsets.z != lastMagOffsets.z));
// Ignore bias changes before final mag field and yaw initialisation, as there may have been a compass calibration
if (changeDetected && secondMagYawInit) {
state.body_magfield.x += (nowMagOffsets.x - lastMagOffsets.x) * 0.001f;
state.body_magfield.y += (nowMagOffsets.y - lastMagOffsets.y) * 0.001f;
state.body_magfield.z += (nowMagOffsets.z - lastMagOffsets.z) * 0.001f;
}
lastMagOffsets = nowMagOffsets;
}
} else {
newDataMag = false;
}

1
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -625,6 +625,7 @@ private:
uint32_t startTimeLAND_ms; // time in msec that the landing condition started - used to compensate for ground effect on baro height
float gpsSpdAccuracy; // estimated speed accuracy in m/s returned by the UBlox GPS receiver
uint32_t lastGpsVelFail_ms; // time of last GPS vertical velocity consistency check fail
Vector3f lastMagOffsets; // magnetometer offsets returned by compass object from previous update
// Used by smoothing of state corrections
Vector10 gpsIncrStateDelta; // vector of corrections to attitude, velocity and position to be applied over the period between the current and next GPS measurement