hardware status monitoring - generic interface

Makefile

@@ -452,6 +452,7 @@ COMMON_SRC = \
             sensors/compass.c \
             sensors/gyro.c \
             sensors/initialisation.c \
+            sensors/diagnostics.c \
             $(CMSIS_SRC) \
             $(DEVICE_STDPERIPH_SRC)
 

src/main/flight/imu.c

@@ -423,11 +423,6 @@ static int imuCalculateAccelerometerConfidence(void)
     return (nearness > MAX_ACC_SQ_NEARNESS) ? 0 : MAX_ACC_SQ_NEARNESS - nearness;
 }
 
-static bool isMagnetometerHealthy(void)
-{
-    return (mag.magADC[X] != 0) && (mag.magADC[Y] != 0) && (mag.magADC[Z] != 0);
-}
-
 static void imuCalculateEstimatedAttitude(float dT)
 {
     const bool canUseMAG = sensors(SENSOR_MAG) && isMagnetometerHealthy();

src/main/io/ledstrip.c

@@ -533,10 +533,8 @@ static void applyLedWarningLayer(bool updateNow, timeUs_t *timer)
                 warningFlags |= 1 << WARNING_FAILSAFE;
             if (!ARMING_FLAG(ARMED) && !ARMING_FLAG(OK_TO_ARM))
                 warningFlags |= 1 << WARNING_ARMING_DISABLED;
-#ifdef MAG
-            if (masterConfig.mag_hardware != MAG_NONE && !compassIsWorking())
+            if (!isHardwareHealthy())
                 warningFlags |= 1 << WARNING_HW_ERROR;
-#endif
         }
         *timer += LED_STRIP_HZ(10);
     }

src/main/sensors/compass.c

@@ -63,9 +63,9 @@ bool compassInit(const compassConfig_t *compassConfig)
     return ret;
 }
 
-bool compassIsWorking(void)
+bool isCompassHealthy(void)
 {
-	return (magADC[X] | magADC[Y] | magADC[Z]) != 0;
+    return (magADC[X] != 0) && (magADC[Y] != 0) && (magADC[Z] != 0);
 }
 
 bool isCompassReady(void)

src/main/sensors/compass.h

@@ -55,4 +55,4 @@ bool compassInit(const compassConfig_t *compassConfig);
 union flightDynamicsTrims_u;
 void compassUpdate(timeUs_t currentTimeUs, union flightDynamicsTrims_u *magZero);
 bool isCompassReady(void);
-bool compassIsWorking(void);
+bool isCompassHealthy(void);

src/main/sensors/diagnostics.c

@@ -0,0 +1,27 @@
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "platform.h"
+#include "build/debug.h"
+
+#include "common/axis.h"
+#include "common/maths.h"
+
+#include "flight/navigation_rewrite.h"
+
+#include "config/config.h"
+#include "config/config_master.h"
+
+#include "sensors/sensors.h"
+#include "sensors/compass.h"
+
+
+bool isHardwareHealthy(void)
+{
+#ifdef MAG
+	if (masterConfig.mag_hardware != MAG_NONE && !isCompassHealthy())
+		return false;
+#endif
+
+	return true;
+}

src/main/sensors/diagnostics.h

@@ -0,0 +1,8 @@
+
+/**
+ * Collects hardware failure information from devices.
+ * No hardware checking should be actually performed,
+ * rather already known hardware status should be returned.
+ */
+bool isHardwareHealthy(void);
+