Merge pull request #8858 from RomanLut/submit-simulator-sensor-failures

Enable possibility to simulate HW sensor failures in HITL

src/main/fc/fc_msp.c

@@ -2526,6 +2526,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
             gpsSol.flags.validVelNE = false;
             gpsSol.flags.validVelD = false;
             gpsSol.flags.validEPE = false;
+            gpsSol.flags.validTime = false;
             gpsSol.numSat = sbufReadU8(src);
             gpsSol.llh.lat = sbufReadU32(src);
             gpsSol.llh.lon = sbufReadU32(src);
@@ -3533,6 +3534,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
 						gpsSol.flags.validVelNE = true;
 						gpsSol.flags.validVelD = true;
 						gpsSol.flags.validEPE = true;
+						gpsSol.flags.validTime = false;
 
 						gpsSol.llh.lat = sbufReadU32(src);
 						gpsSol.llh.lon = sbufReadU32(src);
@@ -3587,7 +3589,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
 
 				if (sensors(SENSOR_MAG)) {
 					mag.magADC[X] = ((int16_t)sbufReadU16(src)) / 20;  // 16000 / 20 = 800uT
-					mag.magADC[Y] = ((int16_t)sbufReadU16(src)) / 20;
+					mag.magADC[Y] = ((int16_t)sbufReadU16(src)) / 20;   //note that mag failure is simulated by setting all readings to zero
 					mag.magADC[Z] = ((int16_t)sbufReadU16(src)) / 20;
 				} else {
 					sbufAdvance(src, sizeof(uint16_t) * XYZ_AXIS_COUNT);
@@ -3605,6 +3607,14 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
 
                 if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) {
                     simulatorData.airSpeed = sbufReadU16(src);
+			    } else {
+                    if (SIMULATOR_HAS_OPTION(HITL_EXTENDED_FLAGS)) {
+                        sbufReadU16(src); 
+                    }
+                }
+
+                if (SIMULATOR_HAS_OPTION(HITL_EXTENDED_FLAGS)) {
+                    simulatorData.flags |= ((uint16_t)sbufReadU8(src)) << 8;
                 }
 			} else {
 				DISABLE_STATE(GPS_FIX);

src/main/fc/runtime_config.h

@@ -181,7 +181,10 @@ typedef enum {
     HITL_USE_IMU			    = (1 << 3), // Use the Acc and Gyro data provided by XPlane to calculate Attitude (i.e. 100% of the calculations made by AHRS from INAV)
     HITL_HAS_NEW_GPS_DATA		= (1 << 4),
     HITL_EXT_BATTERY_VOLTAGE	= (1 << 5), // Extend MSP_SIMULATOR format 2
-    HITL_AIRSPEED               = (1 << 6) 
+    HITL_AIRSPEED               = (1 << 6),
+    HITL_EXTENDED_FLAGS         = (1 << 7), // Extend MSP_SIMULATOR format 2
+    HITL_GPS_TIMEOUT            = (1 << 8),
+    HITL_PITOT_FAILURE          = (1 << 9)
 } simulatorFlags_t;
 
 typedef struct {

src/main/io/gps.c

@@ -343,10 +343,21 @@ bool gpsUpdate(void)
 
 #ifdef USE_SIMULATOR
     if (ARMING_FLAG(SIMULATOR_MODE_HITL)) {
-        gpsUpdateTime();
+        if ( SIMULATOR_HAS_OPTION(HITL_GPS_TIMEOUT))
+        {
+            gpsSetState(GPS_LOST_COMMUNICATION);
+            sensorsClear(SENSOR_GPS);
+            gpsStats.timeouts = 5;
+            return false;
+        }
+        else
+        {
             gpsSetState(GPS_RUNNING);
             sensorsSet(SENSOR_GPS);
-        return gpsSol.flags.hasNewData;
+            bool res = gpsSol.flags.hasNewData;
+            gpsSol.flags.hasNewData = false;
+            return res;
+        }
     }
 #endif
 

src/main/sensors/diagnostics.c

@@ -62,12 +62,17 @@ hardwareSensorStatus_e getHwAccelerometerStatus(void)
 
 hardwareSensorStatus_e getHwCompassStatus(void)
 {
+#if defined(USE_MAG)
 #ifdef USE_SIMULATOR
 	if ((ARMING_FLAG(SIMULATOR_MODE_HITL) || ARMING_FLAG(SIMULATOR_MODE_SITL)) && sensors(SENSOR_MAG)) {
+        if (compassIsHealthy()) {
             return HW_SENSOR_OK;
         }
+        else {
+            return HW_SENSOR_UNHEALTHY;
+        }
+	}
 #endif
-#if defined(USE_MAG)
     if (detectedSensors[SENSOR_INDEX_MAG] != MAG_NONE) {
         if (compassIsHealthy()) {
             return HW_SENSOR_OK;

src/main/sensors/pitotmeter.c

@@ -194,6 +194,13 @@ STATIC_PROTOTHREAD(pitotThread)
     pt1FilterInit(&pitot.lpfState, pitotmeterConfig()->pitot_lpf_milli_hz / 1000.0f, 0.0f);
 
     while(1) {
+#ifdef USE_SIMULATOR
+    	while (SIMULATOR_HAS_OPTION(HITL_AIRSPEED) && SIMULATOR_HAS_OPTION(HITL_PITOT_FAILURE))
+        {
+            ptDelayUs(10000);
+    	}
+#endif
+
         // Start measurement
         if (pitot.dev.start(&pitot.dev)) {
             pitot.lastSeenHealthyMs = millis();