mirror of https://github.com/ArduPilot/ardupilot
AP_NavEKF: revert AP_Math class change
This commit is contained in:
parent
7c9e3d4b58
commit
a140a5e77f
|
@ -4262,7 +4262,7 @@ void NavEKF::readMagData()
|
|||
// 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 = (!AP_Math::is_equal(nowMagOffsets.x,lastMagOffsets.x) || !AP_Math::is_equal(nowMagOffsets.y,lastMagOffsets.y) || !AP_Math::is_equal(nowMagOffsets.z,lastMagOffsets.z));
|
||||
bool changeDetected = (!is_equal(nowMagOffsets.x,lastMagOffsets.x) || !is_equal(nowMagOffsets.y,lastMagOffsets.y) || !is_equal(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;
|
||||
|
|
Loading…
Reference in New Issue