From 948c66817ea3da62cf35acb07f8838d72de49357 Mon Sep 17 00:00:00 2001
From: ellingo <elling.olsen@gmail.com>
Date: Sat, 18 Feb 2017 19:25:41 +0100
Subject: [PATCH] Barometer initialization improvements

Assuming 1 std atm instead of vacuum before first calibration.
Sets altitude to zero during calibration (copied from cleanflight).
Stops calibration early when measurements are stable.
---
 src/main/sensors/barometer.c | 23 +++++++++++++++++------
 1 file changed, 17 insertions(+), 6 deletions(-)

diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c
index 4b25477dc5..83c34ad5d5 100644
--- a/src/main/sensors/barometer.c
+++ b/src/main/sensors/barometer.c
@@ -51,7 +51,7 @@ static int32_t baroPressure = 0;
 static int32_t baroTemperature = 0;
 
 static int32_t baroGroundAltitude = 0;
-static int32_t baroGroundPressure = 0;
+static int32_t baroGroundPressure = 8*101325;
 static uint32_t baroPressureSum = 0;
 
 bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse)
@@ -222,20 +222,31 @@ int32_t baroCalculateAltitude(void)
 
     // calculates height from ground via baro readings
     // see: https://github.com/diydrones/ardupilot/blob/master/libraries/AP_Baro/AP_Baro.cpp#L140
-    BaroAlt_tmp = lrintf((1.0f - powf((float)(baroPressureSum / PRESSURE_SAMPLE_COUNT) / 101325.0f, 0.190295f)) * 4433000.0f); // in cm
-    BaroAlt_tmp -= baroGroundAltitude;
-    baro.BaroAlt = lrintf((float)baro.BaroAlt * barometerConfig()->baro_noise_lpf + (float)BaroAlt_tmp * (1.0f - barometerConfig()->baro_noise_lpf)); // additional LPF to reduce baro noise
-
+    if (isBaroCalibrationComplete()) {
+        BaroAlt_tmp = lrintf((1.0f - powf((float)(baroPressureSum / PRESSURE_SAMPLE_COUNT) / 101325.0f, 0.190295f)) * 4433000.0f); // in cm
+        BaroAlt_tmp -= baroGroundAltitude;
+        baro.BaroAlt = lrintf((float)baro.BaroAlt * barometerConfig()->baro_noise_lpf + (float)BaroAlt_tmp * (1.0f - barometerConfig()->baro_noise_lpf)); // additional LPF to reduce baro noise
+    }
+    else {
+        baro.BaroAlt = 0;
+    }
     return baro.BaroAlt;
 }
 
 void performBaroCalibrationCycle(void)
 {
+    static int32_t savedGroundPressure = 0;
+
     baroGroundPressure -= baroGroundPressure / 8;
     baroGroundPressure += baroPressureSum / PRESSURE_SAMPLE_COUNT;
     baroGroundAltitude = (1.0f - powf((baroGroundPressure / 8) / 101325.0f, 0.190295f)) * 4433000.0f;
 
-    calibratingB--;
+    if (baroGroundPressure == savedGroundPressure)
+      calibratingB = 0;
+    else {
+      calibratingB--;
+      savedGroundPressure=baroGroundPressure;
+    }
 }
 
 #endif /* BARO */