diff --git a/src/main/io/gps.c b/src/main/io/gps.c
index 5bcec62f3a..60eb87ec39 100755
--- a/src/main/io/gps.c
+++ b/src/main/io/gps.c
@@ -460,4 +460,8 @@ bool gpsMagDetect(magDev_t *mag)
     return true;
 }
 
+bool isGPSHealthy(void)
+{
+    return true;
+}
 #endif
diff --git a/src/main/io/gps.h b/src/main/io/gps.h
index ab69be4ae7..75ab957095 100644
--- a/src/main/io/gps.h
+++ b/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);
diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c
index 0bd5495247..14b32512a0 100644
--- a/src/main/sensors/acceleration.c
+++ b/src/main/sensors/acceleration.c
@@ -224,3 +224,8 @@ void setAccelerationFilter(uint8_t initialAccLpfCutHz)
         }
     }
 }
+
+bool isAccelerometerHealthy(void)
+{
+    return true;
+}
diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h
index d4e0bc0343..520e69f9a9 100644
--- a/src/main/sensors/acceleration.h
+++ b/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);
diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c
index 6122b8ee08..6ffc7d1eb4 100644
--- a/src/main/sensors/barometer.c
+++ b/src/main/sensors/barometer.c
@@ -148,4 +148,9 @@ int32_t baroCalculateAltitude(void)
     return baro.BaroAlt;
 }
 
+bool isBarometerHealthy(void)
+{
+    return true;
+}
+
 #endif /* BARO */
diff --git a/src/main/sensors/barometer.h b/src/main/sensors/barometer.h
index 8efd7f9bd0..7a7f495d3f 100644
--- a/src/main/sensors/barometer.h
+++ b/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
diff --git a/src/main/sensors/diagnostics.c b/src/main/sensors/diagnostics.c
index a0e7c29192..660103c5e0 100644
--- a/src/main/sensors/diagnostics.c
+++ b/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)
diff --git a/src/main/sensors/rangefinder.c b/src/main/sensors/rangefinder.c
index ecaa109f92..81833b53c7 100644
--- a/src/main/sensors/rangefinder.c
+++ b/src/main/sensors/rangefinder.c
@@ -210,5 +210,10 @@ int32_t rangefinderGetLatestAltitude(void)
 {
     return calculatedAltitude;
 }
+
+bool isRangefinderHealthy(void)
+{
+    return true;
+}
 #endif
 
diff --git a/src/main/sensors/rangefinder.h b/src/main/sensors/rangefinder.h
index d5b32d3fce..e2d3c111a3 100644
--- a/src/main/sensors/rangefinder.h
+++ b/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);