Merge pull request #9021 from RomanLut/submit-allow-hitl-run-with-hw-…

…baro-failure allow HITL to run with HW baro failure
iNavFlight · Jun 4, 2023 · b5f0883 · b5f0883
2 parents 1b5c671 + 9710caa
commit b5f0883
Show file tree

Hide file tree

Showing 3 changed files with 27 additions and 9 deletions.
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
@@ -3483,7 +3483,9 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
 				DISABLE_ARMING_FLAG(SIMULATOR_MODE_HITL);
 
 #ifdef USE_BARO
+            if ( requestedSensors[SENSOR_INDEX_BARO] != BARO_NONE ) {
 				baroStartCalibration();
+            }
 #endif
 #ifdef USE_MAG
 				DISABLE_STATE(COMPASS_CALIBRATED);
@@ -3494,10 +3496,15 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
 
 				disarm(DISARM_SWITCH);  // Disarm to prevent motor output!!!
 			}   
-		} else if (!areSensorsCalibrating()) {
+        } else {
 			if (!ARMING_FLAG(SIMULATOR_MODE_HITL)) { // Just once
 #ifdef USE_BARO
-				baroStartCalibration();
+                if ( requestedSensors[SENSOR_INDEX_BARO] != BARO_NONE ) {
+                    sensorsSet(SENSOR_BARO);
+                    setTaskEnabled(TASK_BARO, true);
+                    DISABLE_ARMING_FLAG(ARMING_DISABLED_HARDWARE_FAILURE);
+				    baroStartCalibration();
+                }
 #endif			
 
 #ifdef USE_MAG

diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c
@@ -254,6 +254,12 @@ uint32_t baroUpdate(void)
 {
     static barometerState_e state = BAROMETER_NEEDS_SAMPLES;
 
+#ifdef USE_SIMULATOR
+    if (ARMING_FLAG(SIMULATOR_MODE_HITL)) {
+        return 0;
+    }
+#endif
+
     switch (state) {
         default:
         case BAROMETER_NEEDS_SAMPLES:
@@ -274,14 +280,8 @@ uint32_t baroUpdate(void)
             if (baro.dev.start_ut) {
                 baro.dev.start_ut(&baro.dev);
             }
-#ifdef USE_SIMULATOR
-            if (!ARMING_FLAG(SIMULATOR_MODE_HITL)) {
-                //output: baro.baroPressure, baro.baroTemperature
-                baro.dev.calculate(&baro.dev, &baro.baroPressure, &baro.baroTemperature);
-            }
-#else
+            //output: baro.baroPressure, baro.baroTemperature
             baro.dev.calculate(&baro.dev, &baro.baroPressure, &baro.baroTemperature);
-#endif
             state = BAROMETER_NEEDS_SAMPLES;
             return baro.dev.ut_delay;
         break;

diff --git a/src/main/sensors/diagnostics.c b/src/main/sensors/diagnostics.c
@@ -94,6 +94,17 @@ hardwareSensorStatus_e getHwCompassStatus(void)
 hardwareSensorStatus_e getHwBarometerStatus(void)
 {
 #if defined(USE_BARO)
+#ifdef USE_SIMULATOR
+	if (ARMING_FLAG(SIMULATOR_MODE_HITL) || ARMING_FLAG(SIMULATOR_MODE_SITL)) {
+        if (requestedSensors[SENSOR_INDEX_BARO] == BARO_NONE) {
+            return HW_SENSOR_NONE;
+        } else if (baroIsHealthy()) {
+            return HW_SENSOR_OK;
+        } else {
+            return HW_SENSOR_UNHEALTHY;
+        }
+	}
+#endif
     if (detectedSensors[SENSOR_INDEX_BARO] != BARO_NONE) {
         if (baroIsHealthy()) {
             return HW_SENSOR_OK;