From ac60901b0c6cde8d3f42c559e02d87123fe8dfda Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 21 Apr 2016 09:56:22 +1000 Subject: [PATCH] AP_NavEKF2: use vector comparison for new mag vector --- libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp index d7c5e24817..a91a7fc321 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp @@ -185,7 +185,7 @@ void NavEKF2_core::readMagData() // detect changes to magnetometer offset parameters and reset states Vector3f nowMagOffsets = _ahrs->get_compass()->get_offsets(magSelectIndex); - bool changeDetected = lastMagOffsetsValid && (!is_equal(nowMagOffsets.x,lastMagOffsets.x) || !is_equal(nowMagOffsets.y,lastMagOffsets.y) || !is_equal(nowMagOffsets.z,lastMagOffsets.z)); + bool changeDetected = lastMagOffsetsValid && (nowMagOffsets != lastMagOffsets); if (changeDetected) { // zero the learned magnetometer bias states stateStruct.body_magfield.zero();