aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF2: don't do 3D mag fusion on 2nd EKF2 core

Browse Source 
this reduces the risk that mag fusion errors will badly affect
attitude estimation.
This commit is contained in:
Andrew Tridgell 2016-05-27 21:22:19 +10:00
parent 084a37d4fa
commit 97e2203e70
3 changed files with 23 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp
View File

@ -41,6 +41,19 @@ void NavEKF2_core::controlFilterModes()
}
/*
return effective value for _magCal for this core
*/
uint8_t NavEKF2_core::effective_magCal(void) const
{
// if we are on the 2nd core and _magCal is 3 then treat it as
// 2. This is a workaround for a mag fusion problem
if (frontend->_magCal ==3 && imu_index == 1) {
return 2;
}
return frontend->_magCal;
}
// Determine if learning of wind and magnetic field will be enabled and set corresponding indexing limits to
// avoid unnecessary operations
void NavEKF2_core::setWindMagStateLearningMode()
@ -81,15 +94,16 @@ void NavEKF2_core::setWindMagStateLearningMode()
}
// Determine if learning of magnetic field states has been requested by the user
uint8_t magCal = effective_magCal();
bool magCalRequested =
((frontend->_magCal == 0) && inFlight) || // when flying
((frontend->_magCal == 1) && manoeuvring) || // when manoeuvring
((frontend->_magCal == 3) && firstMagYawInit) || // when initial in-air yaw and field reset has completed
(frontend->_magCal == 4); // all the time
((magCal == 0) && inFlight) || // when flying
((magCal == 1) && manoeuvring) || // when manoeuvring
((magCal == 3) && firstMagYawInit) || // when initial in-air yaw and field reset has completed
(magCal == 4); // all the time
// Deny mag calibration request if we aren't using the compass, it has been inhibited by the user,
// we do not have an absolute position reference or are on the ground (unless explicitly requested by the user)
bool magCalDenied = !use_compass() || (frontend->_magCal == 2) ||(onGround && frontend->_magCal != 4);
bool magCalDenied = !use_compass() || (magCal == 2) ||(onGround && magCal != 4);
// Inhibit the magnetic field calibration if not requested or denied
bool setMagInhibit = !magCalRequested || magCalDenied;

2
libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp
View File

@ -350,7 +350,7 @@ void NavEKF2_core::getMagXYZ(Vector3f &magXYZ) const
bool NavEKF2_core::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const
{
// compass offsets are valid if we have finalised magnetic field initialisation and magnetic field learning is not prohibited and primary compass is valid
if (mag_idx == magSelectIndex && firstMagYawInit && (frontend->_magCal != 2) && _ahrs->get_compass()->healthy(magSelectIndex)) {
if (mag_idx == magSelectIndex && firstMagYawInit && (effective_magCal() != 2) && _ahrs->get_compass()->healthy(magSelectIndex)) {
magOffsets = _ahrs->get_compass()->get_offsets(magSelectIndex) - stateStruct.body_magfield*1000.0f;
return true;
} else {

3
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -611,6 +611,9 @@ private:
// zero attitude state covariances, but preserve variances
void zeroAttCovOnly();
// effective value of MAG_CAL
uint8_t effective_magCal(void) const;
// Length of FIFO buffers used for non-IMU sensor data.
// Must be larger than the time period defined by IMU_BUFFER_LENGTH
static const uint32_t OBS_BUFFER_LENGTH = 5;