Fix MS5611

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)
 

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,6 +344,7 @@ bool baroIsCalibrated(void)
 
 static void baroSetCalibrationCycles(uint16_t calibrationCyclesRequired)
 {
+    cycleCount = 0;
     calibrationCycles = calibrationCyclesRequired;
 }
 
@@ -493,9 +495,14 @@ float getBaroAltitude(void)
     return baro.altitude;
 }
 
+// Hack to discard NaN/Inf returned from driver. With -ffinite-math-only (enabled by -ffast-math), compiler optimizes `isfinite` away.
+// TODO - fix MS5611, it should always return finite altitude.
+#if defined(USE_BARO_MS5611)
+__attribute__((optimize("-fno-finite-math-only")))
+#endif
 static void performBaroCalibrationCycle(const float altitude)
 {
-    static uint16_t cycleCount = 0;
+    if (!isfinite(altitude)) return; // NaN or Inf is passed occasionally from MS5611 driver 
 
     baroGroundAltitude += altitude;
     cycleCount++;