src/main/sensors/diagnostics.c

   1 #include <stdbool.h>
   2 #include <stdint.h>
   3
   4 #include "platform.h"
   5 #include "build/debug.h"
   6
   7 #include "common/axis.h"
   8 #include "common/maths.h"
   9 #include "common/time.h"
  10
  11 #include "config/feature.h"
  12
  13 #include "fc/config.h"
  14 #include "fc/runtime_config.h"
  15
  16 #include "io/gps.h"
  17
  18 #include "sensors/sensors.h"
  19 #include "sensors/diagnostics.h"
  20 #include "sensors/gyro.h"
  21 #include "sensors/compass.h"
  22 #include "sensors/acceleration.h"
  23 #include "sensors/barometer.h"
  24 #include "sensors/rangefinder.h"
  25 #include "sensors/pitotmeter.h"
  26 #include "sensors/opflow.h"
  27
  28 extern uint8_t requestedSensors[SENSOR_INDEX_COUNT];
  29 extern uint8_t detectedSensors[SENSOR_INDEX_COUNT];
  30
  31 hardwareSensorStatus_e getHwGyroStatus(void)
  32 {
  33     // Gyro is assumed to be always healthy, but it must be present
  34     if (detectedSensors[SENSOR_INDEX_GYRO] == GYRO_NONE) {
  35         return HW_SENSOR_UNAVAILABLE;
  36     }
  37
  38     return HW_SENSOR_OK;
  39 }
  40
  41 hardwareSensorStatus_e getHwAccelerometerStatus(void)
  42 {
  43     if (detectedSensors[SENSOR_INDEX_ACC] != ACC_NONE) {
  44         if (accIsHealthy()) {
  45             return HW_SENSOR_OK;
  46         } else {
  47             return HW_SENSOR_UNHEALTHY;
  48         }
  49     } else {
  50         if (requestedSensors[SENSOR_INDEX_ACC] != ACC_NONE) {
  51             // Selected but not detected
  52             return HW_SENSOR_UNAVAILABLE;
  53         }
  54         else {
  55             // Not selected and not detected
  56             return HW_SENSOR_NONE;
  57         }
  58     }
  59 }
  60
  61 hardwareSensorStatus_e getHwCompassStatus(void)
  62 {
  63 #if defined(USE_MAG)
  64 #ifdef USE_SIMULATOR
  65     if ((ARMING_FLAG(SIMULATOR_MODE_HITL) || ARMING_FLAG(SIMULATOR_MODE_SITL)) && sensors(SENSOR_MAG)) {
  66         if (compassIsHealthy()) {
  67             return HW_SENSOR_OK;
  68         } else {
  69             return HW_SENSOR_UNHEALTHY;
  70         }
  71     }
  72 #endif
  73     if (detectedSensors[SENSOR_INDEX_MAG] != MAG_NONE) {
  74         if (compassIsHealthy()) {
  75             return HW_SENSOR_OK;
  76         } else {
  77             return HW_SENSOR_UNHEALTHY;
  78         }
  79     } else {
  80         if (requestedSensors[SENSOR_INDEX_MAG] != MAG_NONE) {
  81             // Selected but not detected
  82             return HW_SENSOR_UNAVAILABLE;
  83         } else {
  84             // Not selected and not detected
  85             return HW_SENSOR_NONE;
  86         }
  87     }
  88 #else
  89     return HW_SENSOR_NONE;
  90 #endif
  91 }
  92
  93 hardwareSensorStatus_e getHwBarometerStatus(void)
  94 {
  95 #if defined(USE_BARO)
  96 #ifdef USE_SIMULATOR
  97     if (ARMING_FLAG(SIMULATOR_MODE_HITL) || ARMING_FLAG(SIMULATOR_MODE_SITL)) {
  98         if (requestedSensors[SENSOR_INDEX_BARO] == BARO_NONE) {
  99             return HW_SENSOR_NONE;
 100         } else if (baroIsHealthy()) {
 101             return HW_SENSOR_OK;
 102         } else {
 103             return HW_SENSOR_UNHEALTHY;
 104         }
 105     }
 106 #endif
 107     if (detectedSensors[SENSOR_INDEX_BARO] != BARO_NONE) {
 108         if (baroIsHealthy()) {
 109             return HW_SENSOR_OK;
 110         }
 111         else {
 112             return HW_SENSOR_UNHEALTHY;
 113         }
 114     }
 115     else {
 116         if (requestedSensors[SENSOR_INDEX_BARO] != BARO_NONE) {
 117             // Selected but not detected
 118             return HW_SENSOR_UNAVAILABLE;
 119         }
 120         else {
 121             // Not selected and not detected
 122             return HW_SENSOR_NONE;
 123         }
 124     }
 125 #else
 126     return HW_SENSOR_NONE;
 127 #endif
 128 }
 129
 130 hardwareSensorStatus_e getHwRangefinderStatus(void)
 131 {
 132 #if defined(USE_RANGEFINDER)
 133     if (detectedSensors[SENSOR_INDEX_RANGEFINDER] != RANGEFINDER_NONE) {
 134         if (rangefinderIsHealthy()) {
 135             return HW_SENSOR_OK;
 136         } else {
 137             return HW_SENSOR_UNHEALTHY;
 138         }
 139     }
 140     else {
 141         if (requestedSensors[SENSOR_INDEX_RANGEFINDER] != RANGEFINDER_NONE) {
 142             // Selected but not detected
 143             return HW_SENSOR_UNAVAILABLE;
 144         } else {
 145             // Not selected and not detected
 146             return HW_SENSOR_NONE;
 147         }
 148     }
 149 #else
 150     return HW_SENSOR_NONE;
 151 #endif
 152 }
 153
 154 hardwareSensorStatus_e getHwPitotmeterStatus(void)
 155 {
 156 #if defined(USE_PITOT)
 157     if (detectedSensors[SENSOR_INDEX_PITOT] != PITOT_NONE) {
 158         if (pitotIsHealthy()) {
 159             return HW_SENSOR_OK;
 160         } else {
 161             return HW_SENSOR_UNHEALTHY;
 162         }
 163     }
 164     else {
 165         if (requestedSensors[SENSOR_INDEX_PITOT] != PITOT_NONE) {
 166             // Selected but not detected
 167             return HW_SENSOR_UNAVAILABLE;
 168         } else {
 169             // Not selected and not detected
 170             return HW_SENSOR_NONE;
 171         }
 172     }
 173 #else
 174     return HW_SENSOR_NONE;
 175 #endif
 176 }
 177
 178 hardwareSensorStatus_e getHwGPSStatus(void)
 179 {
 180 #if defined(USE_GPS)
 181     if (sensors(SENSOR_GPS)) {
 182         if (isGPSHealthy()) {
 183             return HW_SENSOR_OK;
 184         } else {
 185             return HW_SENSOR_UNHEALTHY;
 186         }
 187     }
 188     else {
 189         if (feature(FEATURE_GPS) && gpsStats.timeouts > 4) {
 190             // Selected but not detected
 191             return HW_SENSOR_UNAVAILABLE;
 192         } else {
 193             // Not selected and not detected
 194             return HW_SENSOR_NONE;
 195         }
 196     }
 197 #else
 198     return HW_SENSOR_NONE;
 199 #endif
 200 }
 201
 202 hardwareSensorStatus_e getHwOpticalFlowStatus(void)
 203 {
 204 #if defined(USE_OPFLOW)
 205     if (detectedSensors[SENSOR_INDEX_OPFLOW] != OPFLOW_NONE) {
 206         if (opflowIsHealthy()) {
 207             return HW_SENSOR_OK;
 208         } else {
 209             return HW_SENSOR_UNHEALTHY;
 210         }
 211     }
 212     else {
 213         if (requestedSensors[SENSOR_INDEX_OPFLOW] != OPFLOW_NONE) {
 214             // Selected but not detected
 215             return HW_SENSOR_UNAVAILABLE;
 216         } else {
 217             // Not selected and not detected
 218             return HW_SENSOR_NONE;
 219         }
 220     }
 221 #else
 222     return HW_SENSOR_NONE;
 223 #endif
 224 }
 225
 226 bool isHardwareHealthy(void)
 227 {
 228     const hardwareSensorStatus_e gyroStatus = getHwGyroStatus();
 229     const hardwareSensorStatus_e accStatus = getHwAccelerometerStatus();
 230     const hardwareSensorStatus_e baroStatus = getHwBarometerStatus();
 231     const hardwareSensorStatus_e magStatus = getHwCompassStatus();
 232     const hardwareSensorStatus_e rangefinderStatus = getHwRangefinderStatus();
 233     const hardwareSensorStatus_e pitotStatus = getHwPitotmeterStatus();
 234     const hardwareSensorStatus_e gpsStatus = getHwGPSStatus();
 235     const hardwareSensorStatus_e opflowStatus = getHwOpticalFlowStatus();
 236
 237     // Sensor is considered failing if it's either unavailable (selected but not detected) or unhealthy (returning invalid readings)
 238     if (gyroStatus == HW_SENSOR_UNAVAILABLE || gyroStatus == HW_SENSOR_UNHEALTHY)
 239         return false;
 240
 241     if (accStatus == HW_SENSOR_UNAVAILABLE || accStatus == HW_SENSOR_UNHEALTHY)
 242         return false;
 243
 244     if (baroStatus == HW_SENSOR_UNAVAILABLE || baroStatus == HW_SENSOR_UNHEALTHY)
 245         return false;
 246
 247     if (magStatus == HW_SENSOR_UNAVAILABLE || magStatus == HW_SENSOR_UNHEALTHY)
 248         return false;
 249
 250     if (rangefinderStatus == HW_SENSOR_UNAVAILABLE || rangefinderStatus == HW_SENSOR_UNHEALTHY)
 251         return false;
 252
 253     if (pitotStatus == HW_SENSOR_UNAVAILABLE || pitotStatus == HW_SENSOR_UNHEALTHY)
 254         return false;
 255
 256     if (gpsStatus == HW_SENSOR_UNAVAILABLE || gpsStatus == HW_SENSOR_UNHEALTHY)
 257         return false;
 258
 259     if (opflowStatus == HW_SENSOR_UNAVAILABLE || opflowStatus == HW_SENSOR_UNHEALTHY)
 260         return false;
 261
 262     return true;
 263 }