From 3116ef40084fe909c3c054ada8efed6813e72b03 Mon Sep 17 00:00:00 2001
From: Martin Budden <mjbudden@gmail.com>
Date: Sat, 20 Jan 2018 11:59:20 +0000
Subject: [PATCH] Moved compass health check into compass.c

---
 src/main/flight/imu.c                | 9 +++------
 src/main/sensors/compass.c           | 7 ++++++-
 src/main/sensors/compass.h           | 5 +++--
 src/test/unit/flight_imu_unittest.cc | 1 +
 4 files changed, 13 insertions(+), 9 deletions(-)

diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c
index ca51c8e7a2..53ce0b76c8 100644
--- a/src/main/flight/imu.c
+++ b/src/main/flight/imu.c
@@ -402,11 +402,6 @@ static bool imuIsAccelerometerHealthy(void)
     return (81 < accMagnitude) && (accMagnitude < 121);
 }
 
-static bool isMagnetometerHealthy(void)
-{
-    return (mag.magADC[X] != 0) && (mag.magADC[Y] != 0) && (mag.magADC[Z] != 0);
-}
-
 static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
 {
     static uint32_t previousIMUUpdateTime;
@@ -422,9 +417,11 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
         useAcc = true;
     }
 
-    if (sensors(SENSOR_MAG) && isMagnetometerHealthy()) {
+#ifdef USE_MAG
+    if (sensors(SENSOR_MAG) && compassIsHealthy()) {
         useMag = true;
     }
+#endif
 #if defined(USE_GPS)
     else if (STATE(FIXED_WING) && sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5 && gpsSol.groundSpeed >= 300) {
         // In case of a fixed-wing aircraft we can use GPS course over ground to correct heading
diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c
index 0a8cf9d8b7..a7505e134d 100644
--- a/src/main/sensors/compass.c
+++ b/src/main/sensors/compass.c
@@ -262,6 +262,11 @@ bool compassInit(void)
     return true;
 }
 
+bool compassIsHealthy(void)
+{
+    return (mag.magADC[X] != 0) && (mag.magADC[Y] != 0) && (mag.magADC[Z] != 0);
+}
+
 void compassUpdate(timeUs_t currentTimeUs)
 {
     static timeUs_t tCal = 0;
@@ -310,4 +315,4 @@ void compassUpdate(timeUs_t currentTimeUs)
         }
     }
 }
-#endif
+#endif // USE_MAG
diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h
index bc3c57bcb4..a54db5a63f 100644
--- a/src/main/sensors/compass.h
+++ b/src/main/sensors/compass.h
@@ -17,9 +17,9 @@
 
 #pragma once
 
+#include "common/time.h"
 #include "drivers/io_types.h"
 #include "drivers/sensor.h"
-#include "common/time.h"
 #include "pg/pg.h"
 #include "sensors/sensors.h"
 
@@ -56,6 +56,7 @@ typedef struct compassConfig_s {
 
 PG_DECLARE(compassConfig_t, compassConfig);
 
-bool compassInit(void);
+bool compassIsHealthy(void);
 void compassUpdate(timeUs_t currentTime);
+bool compassInit(void);
 
diff --git a/src/test/unit/flight_imu_unittest.cc b/src/test/unit/flight_imu_unittest.cc
index 9468ec38ef..5c041b28b8 100644
--- a/src/test/unit/flight_imu_unittest.cc
+++ b/src/test/unit/flight_imu_unittest.cc
@@ -234,6 +234,7 @@ bool sensors(uint32_t mask)
 uint32_t millis(void) { return 0; }
 uint32_t micros(void) { return 0; }
 
+bool compassIsHealthy(void) { return true; }
 bool isBaroCalibrationComplete(void) { return true; }
 void performBaroCalibrationCycle(void) {}
 int32_t baroCalculateAltitude(void) { return 0; }