Basic functions to check if sensors are healthy

src/main/io/gps.c

@@ -460,4 +460,8 @@ bool gpsMagDetect(magDev_t *mag)
     return true;
 }
 
+bool isGPSHealthy(void)
+{
+    return true;
+}
 #endif

src/main/io/gps.h

@@ -139,3 +139,4 @@ struct serialConfig_s;
 void gpsInit(struct serialConfig_s *serialConfig, gpsConfig_t *initialGpsConfig);
 void gpsThread(void);
 void updateGpsIndicator(timeUs_t currentTimeUs);
+bool isGPSHealthy(void);

src/main/sensors/acceleration.c

@@ -224,3 +224,8 @@ void setAccelerationFilter(uint8_t initialAccLpfCutHz)
         }
     }
 }
+
+bool isAccelerometerHealthy(void)
+{
+    return true;
+}

src/main/sensors/acceleration.h

@@ -58,3 +58,4 @@ union flightDynamicsTrims_u;
 void setAccelerationZero(union flightDynamicsTrims_u * accZeroToUse);
 void setAccelerationGain(union flightDynamicsTrims_u * accGainToUse);
 void setAccelerationFilter(uint8_t initialAccLpfCutHz);
+bool isAccelerometerHealthy(void);

src/main/sensors/barometer.c

@@ -148,4 +148,9 @@ int32_t baroCalculateAltitude(void)
     return baro.BaroAlt;
 }
 
+bool isBarometerHealthy(void)
+{
+    return true;
+}
+
 #endif /* BARO */

src/main/sensors/barometer.h

@@ -49,4 +49,5 @@ void baroSetCalibrationCycles(uint16_t calibrationCyclesRequired);
 uint32_t baroUpdate(void);
 bool isBaroReady(void);
 int32_t baroCalculateAltitude(void);
+bool isBarometerHealthy(void);
 #endif

src/main/sensors/diagnostics.c

@@ -7,13 +7,13 @@
 #include "common/axis.h"
 #include "common/maths.h"
 
-#include "flight/navigation_rewrite.h"
-
 #include "config/config.h"
 #include "config/feature.h"
 
 #include "fc/runtime_config.h"
 
+#include "io/gps.h"
+
 #include "sensors/sensors.h"
 #include "sensors/diagnostics.h"
 #include "sensors/gyro.h"
@@ -33,8 +33,9 @@ hardwareSensorStatus_e getHwGyroStatus(void)
 
 hardwareSensorStatus_e getHwAccelerometerStatus(void)
 {
+#if defined(ACC)
     if (detectedSensors[SENSOR_INDEX_ACC] != ACC_NONE) {
-        if (true) { // isAccelerometerHealthy()
+        if (isAccelerometerHealthy()) {
             return HW_SENSOR_OK;
         }
         else {
@@ -51,6 +52,9 @@ hardwareSensorStatus_e getHwAccelerometerStatus(void)
             return HW_SENSOR_NONE;
         }
     }
+#else
+    return HW_SENSOR_NONE;
+#endif
 }
 
 hardwareSensorStatus_e getHwCompassStatus(void)
@@ -81,8 +85,9 @@ hardwareSensorStatus_e getHwCompassStatus(void)
 
 hardwareSensorStatus_e getHwBarometerStatus(void)
 {
+#if defined(BARO)
     if (detectedSensors[SENSOR_INDEX_BARO] != BARO_NONE) {
-        if (true) { // isBarometerHealthy()
+        if (isBarometerHealthy()) {
             return HW_SENSOR_OK;
         }
         else {
@@ -99,12 +104,16 @@ hardwareSensorStatus_e getHwBarometerStatus(void)
             return HW_SENSOR_NONE;
         }
     }
+#else
+    return HW_SENSOR_NONE;
+#endif
 }
 
 hardwareSensorStatus_e getHwRangefinderStatus(void)
 {
+#if defined(SONAR)
     if (detectedSensors[SENSOR_INDEX_RANGEFINDER] != RANGEFINDER_NONE) {
-        if (true) { // isBarometerHealthy()
+        if (isRangefinderHealthy())
             return HW_SENSOR_OK;
         }
         else {
@@ -121,12 +130,16 @@ hardwareSensorStatus_e getHwRangefinderStatus(void)
             return HW_SENSOR_NONE;
         }
     }
+#else
+    return HW_SENSOR_NONE;
+#endif
 }
 
 hardwareSensorStatus_e getHwGPSStatus(void)
 {
+#if defined(GPS)
     if (sensors(SENSOR_GPS)) {
-        if (true) { // isGPSHealthy()
+        if (isGPSHealthy()) {
             return HW_SENSOR_OK;
         }
         else {
@@ -143,6 +156,9 @@ hardwareSensorStatus_e getHwGPSStatus(void)
             return HW_SENSOR_NONE;
         }
     }
+#else
+    return HW_SENSOR_NONE;
+#endif
 }
 
 bool isHardwareHealthy(void)

src/main/sensors/rangefinder.c

@@ -210,5 +210,10 @@ int32_t rangefinderGetLatestAltitude(void)
 {
     return calculatedAltitude;
 }
+
+bool isRangefinderHealthy(void)
+{
+    return true;
+}
 #endif
 

src/main/sensors/rangefinder.h

@@ -43,4 +43,4 @@ rangefinderType_e rangefinderDetect(void);
 void rangefinderInit(rangefinderType_e rangefinderType);
 void rangefinderUpdate(void);
 int32_t rangefinderRead(void);
-
+bool isRangefinderHealthy(void);