Fix MS5611

betaflight · May 16, 2023 · fba3c3d · fba3c3d
1 parent c41b458
commit fba3c3d
Show file tree

Hide file tree

Showing 2 changed files with 15 additions and 8 deletions.
diff --git a/src/main/flight/position.c b/src/main/flight/position.c
@@ -208,7 +208,7 @@ void calculateEstimatedAltitude(void)
     DEBUG_SET(DEBUG_ALTITUDE, 3, estimatedVario);
 #endif
     DEBUG_SET(DEBUG_RTH, 1, displayAltitudeCm);
-    DEBUG_SET(DEBUG_BARO, 3, baroAltCm);
+    // DEBUG_SET(DEBUG_BARO, 3, baroAltCm);
 }
 #endif //defined(USE_BARO) || defined(USE_GPS)
 

diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c
@@ -160,6 +160,7 @@ void pgResetFn_barometerConfig(barometerConfig_t *barometerConfig)
 #define NUM_GROUND_LEVEL_CYCLES   10        // calibrate baro to new ground level (10 * 25 ms = ~250 ms non blocking)
 
 static uint16_t calibrationCycles = 0;      // baro calibration = get new ground pressure value
+static uint16_t cycleCount = 0;
 static float baroGroundAltitude = 0.0f;
 static bool baroCalibrated = false;
 static bool baroReady = false;
@@ -343,14 +344,15 @@ bool baroIsCalibrated(void)
 
 static void baroSetCalibrationCycles(uint16_t calibrationCyclesRequired)
 {
+    // cycleCount = 0;
     calibrationCycles = calibrationCyclesRequired;
 }
 
 void baroStartCalibration(void)
 {
     baroSetCalibrationCycles(NUM_CALIBRATION_CYCLES);
-    baroGroundAltitude = 0;
     baroCalibrated = false;
+    baroGroundAltitude = 0;
 }
 
 void baroSetGroundLevel(void)
@@ -451,6 +453,10 @@ uint32_t baroUpdate(timeUs_t currentTimeUs)
             baro.dev.calculate(&baro.pressure, &baro.temperature);
             baro.altitude = pressureToAltitude(baro.pressure);
 
+            DEBUG_SET(DEBUG_BARO, 1, lrintf(baro.pressure / 100.0f));   // hPa
+            DEBUG_SET(DEBUG_BARO, 2, baro.temperature);                 // c°C
+            DEBUG_SET(DEBUG_BARO, 3, lrintf(baro.altitude));            // cm
+
             if (baroIsCalibrated()) {
                 // zero baro altitude
                 baro.altitude -= baroGroundAltitude;
@@ -460,10 +466,6 @@ uint32_t baroUpdate(timeUs_t currentTimeUs)
                 baro.altitude = 0.0f;
             }
 
-            DEBUG_SET(DEBUG_BARO, 1, lrintf(baro.pressure / 100.0f));   // hPa
-            DEBUG_SET(DEBUG_BARO, 2, baro.temperature);                 // c°C
-            DEBUG_SET(DEBUG_BARO, 3, lrintf(baro.altitude));            // cm
-
             if (baro.dev.combined_read) {
                 state = BARO_STATE_PRESSURE_START;
             } else {
@@ -493,10 +495,15 @@ float getBaroAltitude(void)
     return baro.altitude;
 }
 
+// Hack to avoid compiler optimising out the calibration code
+#if defined(USE_BARO_MS5611)
+__attribute__((optimize("-fno-finite-math-only")))
+#endif
 static void performBaroCalibrationCycle(const float altitude)
 {
-    static uint16_t cycleCount = 0;
-
+#if defined(USE_BARO_MS5611)
+    if (!isfinite(altitude)) return; // NaN or Inf is passed occasionally from MS5611 driver 
+#endif
     baroGroundAltitude += altitude;
     cycleCount++;