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,7 +3607,15 @@ 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

@@ -175,20 +175,23 @@ flightModeForTelemetry_e getFlightModeForTelemetry(void);
 
 typedef enum {
     HITL_RESET_FLAGS            = (0 << 0),
-	HITL_ENABLE					= (1 << 0),
+    HITL_ENABLE					= (1 << 0),
-	HITL_SIMULATE_BATTERY		= (1 << 1),
+    HITL_SIMULATE_BATTERY		= (1 << 1),
-	HITL_MUTE_BEEPER			= (1 << 2),
+    HITL_MUTE_BEEPER			= (1 << 2),
-	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_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_HAS_NEW_GPS_DATA		= (1 << 4),
-	HITL_EXT_BATTERY_VOLTAGE	= (1 << 5), // Extend MSP_SIMULATOR format 2
+    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 {
-	simulatorFlags_t flags;
+    simulatorFlags_t flags;
-	uint8_t debugIndex;
+    uint8_t debugIndex;
     uint8_t vbat;      // 126 -> 12.6V
-	uint16_t airSpeed; // cm/s
+    uint16_t airSpeed; // cm/s
     int16_t input[4];
 } simulatorData_t;
 

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_RUNNING);
+        {
-        sensorsSet(SENSOR_GPS);
+            gpsSetState(GPS_LOST_COMMUNICATION);
-        return gpsSol.flags.hasNewData;
+            sensorsClear(SENSOR_GPS);
+            gpsStats.timeouts = 5;
+            return false;
+        }
+        else
+        {
+            gpsSetState(GPS_RUNNING);
+            sensorsSet(SENSOR_GPS);
+            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)) {
-		return HW_SENSOR_OK;
+        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();
@@ -208,9 +215,9 @@ STATIC_PROTOTHREAD(pitotThread)
 
         pitot.dev.calculate(&pitot.dev, &pitotPressureTmp, NULL);
 #ifdef USE_SIMULATOR
-        if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) {
+    	if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) {
-            pitotPressureTmp = sq(simulatorData.airSpeed) * SSL_AIR_DENSITY / 20000.0f + SSL_AIR_PRESSURE;     
+        	pitotPressureTmp = sq(simulatorData.airSpeed) * SSL_AIR_DENSITY / 20000.0f + SSL_AIR_PRESSURE;
-        }
+    	}
 #endif
 #if defined(USE_PITOT_FAKE)
         if (pitotmeterConfig()->pitot_hardware == PITOT_FAKE) { 
@@ -241,14 +248,14 @@ STATIC_PROTOTHREAD(pitotThread)
             pitot.airSpeed = 0.0f;
         }
 #ifdef USE_SIMULATOR
-        if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) {
+    	if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) {
             pitot.airSpeed = simulatorData.airSpeed;
-        }
+    	}
 #endif
 #if defined(USE_PITOT_FAKE)
         if (pitotmeterConfig()->pitot_hardware == PITOT_FAKE) { 
             pitot.airSpeed = fakePitotGetAirspeed();
-        }
+    }
 #endif
     }