Some baro cleanups to avoid using or exposing incomplete and

uninitialised baro data.

imu code size reduction (treym)

src/main/drivers/barometer_bmp085.c

@@ -167,8 +167,8 @@ bool bmp085Detect(baro_t *baro)
         bmp085.al_version = BMP085_GET_BITSLICE(data, BMP085_AL_VERSION); /* get AL Version */
         bmp085_get_cal_param(); /* readout bmp085 calibparam structure */
         bmp085InitDone = true;
-        baro->ut_delay = 6000;
+        baro->ut_delay = 6000; // 1.5ms margin according to the spec (4.5ms T convetion time)
-        baro->up_delay = 27000;
+        baro->up_delay = 27000; // 6000+21000=27000 1.5ms margin according to the spec (25.5ms P convetion time with OSS=3)
         baro->start_ut = bmp085_start_ut;
         baro->get_ut = bmp085_get_ut;
         baro->start_up = bmp085_start_up;

src/main/flight/imu.c

@@ -307,12 +307,6 @@ static void getEstimatedAttitude(void)
     accMag = accMag * 100 / ((int32_t)acc_1G * acc_1G);
 
     rotateV(&EstG.V, &deltaGyroAngle);
-    if (sensors(SENSOR_MAG)) {
-        rotateV(&EstM.V, &deltaGyroAngle);
-    } else {
-        rotateV(&EstN.V, &deltaGyroAngle);
-        normalizeV(&EstN.V, &EstN.V);
-    }
 
     // Apply complimentary filter (Gyro drift correction)
     // If accel magnitude >1.15G or <0.85G and ACC vector outside of the limit range => we neutralize the effect of accelerometers in the angle estimation.
@@ -325,14 +319,6 @@ static void getEstimatedAttitude(void)
             EstG.A[axis] = (EstG.A[axis] * imuRuntimeConfig->gyro_cmpf_factor + accSmooth[axis]) * invGyroComplimentaryFilterFactor;
     }
 
-    // FIXME what does the _M_ mean?
-    float invGyroComplimentaryFilter_M_Factor = (1.0f / (imuRuntimeConfig->gyro_cmpfm_factor + 1.0f));
-
-    if (sensors(SENSOR_MAG)) {
-        for (axis = 0; axis < 3; axis++)
-            EstM.A[axis] = (EstM.A[axis] * imuRuntimeConfig->gyro_cmpfm_factor + magADC[axis]) * invGyroComplimentaryFilter_M_Factor;
-    }
-
     f.SMALL_ANGLE = (EstG.A[Z] > smallAngle);
 
     // Attitude of the estimated vector
@@ -341,10 +327,19 @@ static void getEstimatedAttitude(void)
     inclination.values.rollDeciDegrees = lrintf(anglerad[AI_ROLL] * (1800.0f / M_PI));
     inclination.values.pitchDeciDegrees = lrintf(anglerad[AI_PITCH] * (1800.0f / M_PI));
 
-    if (sensors(SENSOR_MAG))
+    if (sensors(SENSOR_MAG)) {
+        rotateV(&EstM.V, &deltaGyroAngle);
+        // FIXME what does the _M_ mean?
+        float invGyroComplimentaryFilter_M_Factor = (1.0f / (imuRuntimeConfig->gyro_cmpfm_factor + 1.0f));
+        for (axis = 0; axis < 3; axis++) {
+            EstM.A[axis] = (EstM.A[axis] * imuRuntimeConfig->gyro_cmpfm_factor + magADC[axis]) * invGyroComplimentaryFilter_M_Factor;
+        }
         heading = calculateHeading(&EstM);
-    else
+    } else {
+        rotateV(&EstN.V, &deltaGyroAngle);
+        normalizeV(&EstN.V, &EstN.V);
         heading = calculateHeading(&EstN);
+    }
 
     acc_calc(deltaT); // rotate acc vector into earth frame
 }
@@ -433,7 +428,6 @@ void calculateEstimatedAltitude(uint32_t currentTime)
         accAlt = 0;
     }
     BaroAlt = baroCalculateAltitude();
-
     dt = accTimeSum * 1e-6f; // delta acc reading time in seconds
 
     // Integrator - velocity, cm/sec
@@ -443,16 +437,21 @@ void calculateEstimatedAltitude(uint32_t currentTime)
     // Integrator - Altitude in cm
     accAlt += (vel_acc * 0.5f) * dt + vel * dt;                                         // integrate velocity to get distance (x= a/2 * t^2)
     accAlt = accAlt * barometerConfig->baro_cf_alt + (float)BaroAlt * (1.0f - barometerConfig->baro_cf_alt);      // complementary filter for Altitude estimation (baro & acc)
-    EstAlt = accAlt;
     vel += vel_acc;
 
+    accSum_reset();
+
+    if (!isBaroCalibrationComplete()) {
+        return;
+    }
+
 #if 1
     debug[1] = accSum[2] / accSumCount; // acceleration
     debug[2] = vel;                     // velocity
     debug[3] = accAlt;                  // height
 #endif
 
-    accSum_reset();
+    EstAlt = accAlt;
 
     baroVel = (BaroAlt - lastBaroAlt) * 1000000.0f / dTime;
     lastBaroAlt = BaroAlt;

src/main/sensors/barometer.c

@@ -55,6 +55,10 @@ void baroSetCalibrationCycles(uint16_t calibrationCyclesRequired)
     calibratingB = calibrationCyclesRequired;
 }
 
+static bool baroReady = false;
+
+#define PRESSURE_SAMPLE_COUNT (barometerConfig->baro_sample_count - 1)
+
 static uint32_t recalculateBarometerTotal(uint8_t baroSampleCount, uint32_t pressureTotal, int32_t newPressureReading)
 {
     static int32_t barometerSamples[BARO_SAMPLE_COUNT_MAX];
@@ -65,10 +69,12 @@ static uint32_t recalculateBarometerTotal(uint8_t baroSampleCount, uint32_t pres
     nextSampleIndex = (currentSampleIndex + 1);
     if (nextSampleIndex == baroSampleCount) {
         nextSampleIndex = 0;
+        baroReady = true;
     }
     barometerSamples[currentSampleIndex] = newPressureReading;
 
     // recalculate pressure total
+    // Note, the pressure total is made up of baroSampleCount - 1 samples - See PRESSURE_SAMPLE_COUNT
     pressureTotal += barometerSamples[currentSampleIndex];
     pressureTotal -= barometerSamples[nextSampleIndex];
 
@@ -79,10 +85,10 @@ static uint32_t recalculateBarometerTotal(uint8_t baroSampleCount, uint32_t pres
 
 typedef enum {
     BAROMETER_NEEDS_SAMPLES = 0,
-    BAROMETER_NEEDS_CALCULATION
+    BAROMETER_NEEDS_CALCULATION,
+    BAROMETER_NEEDS_PROCESSING
 } barometerState_e;
 
-static bool baroReady = false;
 
 bool isBaroReady(void) {
 	return baroReady;
@@ -99,21 +105,24 @@ void baroUpdate(uint32_t currentTime)
     baroDeadline = currentTime;
 
     switch (state) {
+        case BAROMETER_NEEDS_SAMPLES:
+            baro.get_ut();
+            baro.start_up();
+            state = BAROMETER_NEEDS_CALCULATION;
+            baroDeadline += baro.up_delay;
+        break;
+
         case BAROMETER_NEEDS_CALCULATION:
             baro.get_up();
             baro.start_ut();
             baroDeadline += baro.ut_delay;
             baro.calculate(&baroPressure, &baroTemperature);
-            baroReady = true;
+            state = BAROMETER_NEEDS_PROCESSING;
-            state = BAROMETER_NEEDS_SAMPLES;
         break;
 
-        case BAROMETER_NEEDS_SAMPLES:
+        case BAROMETER_NEEDS_PROCESSING:
-            baro.get_ut();
+            state = BAROMETER_NEEDS_SAMPLES;
-            baro.start_up();
             baroPressureSum = recalculateBarometerTotal(barometerConfig->baro_sample_count, baroPressureSum, baroPressure);
-            state = BAROMETER_NEEDS_CALCULATION;
-            baroDeadline += baro.up_delay;
         break;
     }
 }
@@ -124,7 +133,7 @@ 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 / (barometerConfig->baro_sample_count - 1)) / 101325.0f, 0.190295f)) * 4433000.0f); // in cm
+    BaroAlt_tmp = lrintf((1.0f - powf((float)(baroPressureSum / PRESSURE_SAMPLE_COUNT) / 101325.0f, 0.190295f)) * 4433000.0f); // in cm
     BaroAlt_tmp -= baroGroundAltitude;
     BaroAlt = lrintf((float)BaroAlt * barometerConfig->baro_noise_lpf + (float)BaroAlt_tmp * (1.0f - barometerConfig->baro_noise_lpf)); // additional LPF to reduce baro noise
 
@@ -134,7 +143,7 @@ int32_t baroCalculateAltitude(void)
 void performBaroCalibrationCycle(void)
 {
     baroGroundPressure -= baroGroundPressure / 8;
-    baroGroundPressure += baroPressureSum / (barometerConfig->baro_sample_count - 1);
+    baroGroundPressure += baroPressureSum / PRESSURE_SAMPLE_COUNT;
     baroGroundAltitude = (1.0f - powf((baroGroundPressure / 8) / 101325.0f, 0.190295f)) * 4433000.0f;
 
     calibratingB--;