From 3eb6be64c9fcf035778ff39afbb02758fa828fbb Mon Sep 17 00:00:00 2001
From: Staroselskii Georgii <gosha371@gmail.com>
Date: Thu, 23 Jul 2015 16:29:39 +0300
Subject: [PATCH] AP_Compass: ignore first two readings in a clearer way

This way we don't mix two different conditionals: settling and data validation.
---
 libraries/AP_Compass/AP_Compass_HMC5843.cpp | 8 ++++++--
 1 file changed, 6 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_Compass/AP_Compass_HMC5843.cpp b/libraries/AP_Compass/AP_Compass_HMC5843.cpp
index 03b1019927..22323cbdb6 100644
--- a/libraries/AP_Compass/AP_Compass_HMC5843.cpp
+++ b/libraries/AP_Compass/AP_Compass_HMC5843.cpp
@@ -328,14 +328,18 @@ bool AP_Compass_HMC5843::_calibrate(uint8_t calibration_gain,
         // strap excitation. After that we accept values in a
         // reasonable range
 
+        if (numAttempts <= 2) {
+            continue;
+        }
+
 #define IS_CALIBRATION_VALUE_VALID(val) (val > 0.7f && val < 1.35f)
 
-        if (numAttempts > 2 &&
-            IS_CALIBRATION_VALUE_VALID(cal[0]) &&
+        if (IS_CALIBRATION_VALUE_VALID(cal[0]) &&
             IS_CALIBRATION_VALUE_VALID(cal[1]) &&
             IS_CALIBRATION_VALUE_VALID(cal[2])) {
             // hal.console->printf_P(PSTR("car=%.2f %.2f %.2f good\n"), cal[0], cal[1], cal[2]);
             good_count++;
+
             _scaling[0] += cal[0];
             _scaling[1] += cal[1];
             _scaling[2] += cal[2];