Lock altitude task to 100Hz

+ fix baro calibration
+ baro refactoring

src/main/blackbox/blackbox.c

@@ -217,7 +217,7 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = {
     {"magADC",      2, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(TAG8_8SVB), CONDITION(MAG)},
 #endif
 #ifdef USE_BARO
-    {"BaroAlt",    -1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(TAG8_8SVB), CONDITION(BARO)},
+    {"baroAlt",    -1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(TAG8_8SVB), CONDITION(BARO)},
 #endif
 #ifdef USE_RANGEFINDER
     {"surfaceRaw",   -1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(TAG8_8SVB), CONDITION(RANGEFINDER)},
@@ -319,7 +319,7 @@ typedef struct blackboxMainState_s {
     int32_t amperageLatest;
 
 #ifdef USE_BARO
-    int32_t BaroAlt;
+    int32_t baroAlt;
 #endif
 #ifdef USE_MAG
     int16_t magADC[XYZ_AXIS_COUNT];
@@ -614,7 +614,7 @@ static void writeIntraframe(void)
 
 #ifdef USE_BARO
     if (testBlackboxCondition(CONDITION(BARO))) {
-        blackboxWriteSignedVB(blackboxCurrent->BaroAlt);
+        blackboxWriteSignedVB(blackboxCurrent->baroAlt);
     }
 #endif
 
@@ -762,7 +762,7 @@ static void writeInterframe(void)
 
 #ifdef USE_BARO
     if (testBlackboxCondition(CONDITION(BARO))) {
-        deltas[optionalFieldCount++] = blackboxCurrent->BaroAlt - blackboxLast->BaroAlt;
+        deltas[optionalFieldCount++] = blackboxCurrent->baroAlt - blackboxLast->baroAlt;
     }
 #endif
 
@@ -1101,7 +1101,7 @@ static void loadMainState(timeUs_t currentTimeUs)
     blackboxCurrent->amperageLatest = getAmperageLatest();
 
 #ifdef USE_BARO
-    blackboxCurrent->BaroAlt = baro.BaroAlt;
+    blackboxCurrent->baroAlt = baro.altitude;
 #endif
 
 #ifdef USE_RANGEFINDER

src/main/fc/init.c

@@ -98,6 +98,7 @@
 #include "flight/gps_rescue.h"
 #include "flight/pid.h"
 #include "flight/pid_init.h"
+#include "flight/position.h"
 #include "flight/servos.h"
 
 #include "io/asyncfatfs/asyncfatfs.h"

src/main/fc/tasks.c

@@ -294,23 +294,7 @@ void taskUpdateRangefinder(timeUs_t currentTimeUs)
 #endif
 
 #if defined(USE_BARO) || defined(USE_GPS)
-static bool taskAltitudeCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs)
+static void taskCalculateAltitude(timeUs_t currentTimeUs)
-{
-    UNUSED(currentTimeUs);
-
-#if defined(USE_BARO)
-    if (isBaroSampleReady()) {
-        return true;
-    }
-#endif
-
-    if (currentDeltaTimeUs > getTask(TASK_ALTITUDE)->attribute->desiredPeriodUs) {
-        return true;
-    }
-
-    return false;
- }
- static void taskCalculateAltitude(timeUs_t currentTimeUs)
 {
     UNUSED(currentTimeUs);
     calculateEstimatedAltitude();
@@ -395,7 +379,7 @@ task_attribute_t task_attributes[TASK_COUNT] = {
 #endif
 
 #if defined(USE_BARO) || defined(USE_GPS)
-    [TASK_ALTITUDE] = DEFINE_TASK("ALTITUDE", NULL, taskAltitudeCheck, taskCalculateAltitude, TASK_PERIOD_HZ(TASK_ALTITUDE_RATE_HZ), TASK_PRIORITY_LOW),
+    [TASK_ALTITUDE] = DEFINE_TASK("ALTITUDE", NULL, NULL, taskCalculateAltitude, TASK_PERIOD_HZ(TASK_ALTITUDE_RATE_HZ), TASK_PRIORITY_LOW),
 #endif
 
 #ifdef USE_DASHBOARD

src/main/flight/position.h

@@ -22,7 +22,8 @@
 
 #include "common/time.h"
 
-#define TASK_ALTITUDE_RATE_HZ 120
+#define TASK_ALTITUDE_RATE_HZ 100
+
 typedef struct positionConfig_s {
     uint8_t altitude_source;
     uint8_t altitude_prefer_baro;

src/main/sensors/barometer.c

@@ -137,19 +137,13 @@ void pgResetFn_barometerConfig(barometerConfig_t *barometerConfig)
     barometerConfig->baro_xclr_tag = IO_TAG(BARO_XCLR_PIN);
 }
 
+#define NUM_CALIBRATION_CYCLES   100        // 10 seconds init_delay + 100 * 25 ms = 12.5 seconds before valid baro altitude
+#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 bool baroCalibrated = false;
-static int32_t baroPressure = 0;
-static int32_t baroTemperature = 0;
 static float baroGroundAltitude = 0.0f;
-static float baroAltitudeRaw = 0.0f;
+static bool baroCalibrated = false;
-
-
-#define CALIBRATING_BARO_CYCLES 100 // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles
-#define SET_GROUND_LEVEL_BARO_CYCLES 10 // calibrate baro to new ground level (10 * 25 ms = ~250 ms non blocking)
-
 static bool baroReady = false;
-static bool baroSampleReady = false;
 
 void baroPreInit(void)
 {
@@ -296,8 +290,7 @@ static bool baroDetect(baroDev_t *baroDev, baroSensor_e baroHardwareToUse)
 
 void baroInit(void)
 {
-    baroDetect(&baro.dev, barometerConfig()->baro_hardware);
+    baroReady = baroDetect(&baro.dev, barometerConfig()->baro_hardware);
-    baroReady = true;
 }
 
 bool baroIsCalibrated(void)
@@ -312,13 +305,15 @@ static void baroSetCalibrationCycles(uint16_t calibrationCyclesRequired)
 
 void baroStartCalibration(void)
 {
-    baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES);
+    baroSetCalibrationCycles(NUM_CALIBRATION_CYCLES);
+    baroGroundAltitude = 0;
     baroCalibrated = false;
 }
 
 void baroSetGroundLevel(void)
 {
-    baroSetCalibrationCycles(SET_GROUND_LEVEL_BARO_CYCLES);
+    baroSetCalibrationCycles(NUM_GROUND_LEVEL_CYCLES);
+    baroGroundAltitude = 0;
     baroCalibrated = false;
 }
 
@@ -338,20 +333,13 @@ bool isBaroReady(void)
     return baroReady;
 }
 
-bool isBaroSampleReady(void)
-{
-     if (baroSampleReady) {
-         baroSampleReady = false;
-         return true;
-     }
-     return false;
- }
-
 static float pressureToAltitude(const float pressure)
 {
     return (1.0f - powf(pressure / 101325.0f, 0.190295f)) * 4433000.0f;
 }
 
+static void performBaroCalibrationCycle(const float altitude);
+
 uint32_t baroUpdate(timeUs_t currentTimeUs)
 {
     static timeUs_t baroStateDurationUs[BARO_STATE_COUNT];
@@ -415,24 +403,23 @@ uint32_t baroUpdate(timeUs_t currentTimeUs)
                 schedulerIgnoreTaskExecTime();
                 break;
             }
-            baro.dev.calculate(&baroPressure, &baroTemperature);
-            baro.baroPressure = baroPressure;
-            baro.baroTemperature = baroTemperature;
 
-            baroAltitudeRaw = pressureToAltitude(baroPressure);
+            // update baro data
-
+            baro.dev.calculate(&baro.pressure, &baro.temperature);
-            performBaroCalibrationCycle();
+            baro.altitude = pressureToAltitude(baro.pressure);
 
             if (baroIsCalibrated()) {
-                baro.BaroAlt = baroAltitudeRaw - baroGroundAltitude;
+                // zero baro altitude
+                baro.altitude -= baroGroundAltitude;
             } else {
-                baro.BaroAlt = 0.0f;
+                // establish stable baroGroundAltitude value to zero baro altitude with
+                performBaroCalibrationCycle(baro.altitude);
+                baro.altitude = 0.0f;
             }
 
-            DEBUG_SET(DEBUG_BARO, 1, baro.baroTemperature);
+            DEBUG_SET(DEBUG_BARO, 1, lrintf(baro.pressure / 100.0f));   // hPa
-            DEBUG_SET(DEBUG_BARO, 2, baro.BaroAlt);
+            DEBUG_SET(DEBUG_BARO, 2, baro.temperature);                 // c°C
-
+            DEBUG_SET(DEBUG_BARO, 3, lrintf(baro.altitude));            // cm
-            baroSampleReady = true;
 
             if (baro.dev.combined_read) {
                 state = BARO_STATE_PRESSURE_START;
@@ -458,24 +445,23 @@ uint32_t baroUpdate(timeUs_t currentTimeUs)
     return sleepTime;
 }
 
-// baroAltitude samples baro.BaroAlt the ALTITUDE task rate
 float getBaroAltitude(void)
 {
-    return baro.BaroAlt;
+    return baro.altitude;
 }
 
-void performBaroCalibrationCycle(void)
+static void performBaroCalibrationCycle(const float altitude)
 {
     static uint16_t cycleCount = 0;
-    if (!baroCalibrated) {
+
+    baroGroundAltitude += altitude;
     cycleCount++;
-        baroGroundAltitude += baroAltitudeRaw;
+
-        if (cycleCount == calibrationCycles) {
+    if (cycleCount >= calibrationCycles) {
-            baroGroundAltitude = baroGroundAltitude / calibrationCycles; // simple average
+        baroGroundAltitude /= cycleCount;  // simple average
         baroCalibrated = true;
         cycleCount = 0;
     }
-    }
 }
 
 #endif /* BARO */

src/main/sensors/barometer.h

@@ -22,7 +22,6 @@
 
 #include "pg/pg.h"
 #include "drivers/barometer/barometer.h"
-#include "flight/position.h"
 
 typedef enum {
     BARO_DEFAULT = 0,
@@ -49,15 +48,13 @@ typedef struct barometerConfig_s {
 
 PG_DECLARE(barometerConfig_t, barometerConfig);
 
-// #define TASK_BARO_DENOM       3
+#define TASK_BARO_RATE_HZ 40                // Will be overwritten by the baro device driver
-// #define TASK_BARO_RATE_HZ     (TASK_ALTITUDE_RATE_HZ / TASK_BARO_DENOM)
-#define TASK_BARO_RATE_HZ        40
 
 typedef struct baro_s {
     baroDev_t dev;
-    float BaroAlt;
+    float altitude;
-    int32_t baroTemperature;             // Use temperature for telemetry
+    int32_t temperature;                    // Use temperature for telemetry
-    int32_t baroPressure;                // Use pressure for telemetry
+    int32_t pressure;                       // Use pressure for telemetry
 } baro_t;
 
 extern baro_t baro;
@@ -69,6 +66,4 @@ void baroStartCalibration(void);
 void baroSetGroundLevel(void);
 uint32_t baroUpdate(timeUs_t currentTimeUs);
 bool isBaroReady(void);
-bool isBaroSampleReady(void);
 float getBaroAltitude(void);
-void performBaroCalibrationCycle(void);

src/main/telemetry/frsky_hub.c

@@ -219,9 +219,9 @@ static void sendTemperature1(void)
         data = escData->dataAge < ESC_DATA_INVALID ? escData->temperature : 0;
     }
 #elif defined(USE_BARO)
-    data = (baro.baroTemperature + 50)/ 100; // Airmamaf
+    data = lrintf(baro.temperature / 100.0f); // Airmamaf
 #else
-    data = gyroGetTemperature() / 10;
+    data = lrintf(gyroGetTemperature() / 10.0f);
 #endif
     frSkyHubWriteFrame(ID_TEMPRATURE1, data);
 }

src/main/telemetry/ibus_shared.c

@@ -32,6 +32,7 @@
 #include <stdint.h>
 #include <stdlib.h>
 #include <limits.h>
+#include <math.h>
 
 #include "platform.h"
 #include "telemetry/telemetry.h"
@@ -219,7 +220,7 @@ static uint16_t getTemperature(void)
     uint16_t temperature = gyroGetTemperature() * 10;
 #if defined(USE_BARO)
     if (sensors(SENSOR_BARO)) {
-        temperature = (uint16_t) ((baro.baroTemperature + 50) / 10);
+        temperature = lrintf(baro.temperature / 10.0f);
     }
 #endif
     return temperature + IBUS_TEMPERATURE_OFFSET;
@@ -425,10 +426,10 @@ static void setValue(uint8_t* bufferPtr, uint8_t sensorType, uint8_t length)
 #ifdef USE_BARO
         case IBUS_SENSOR_TYPE_ALT:
         case IBUS_SENSOR_TYPE_ALT_MAX:
-            value.int32 = baro.BaroAlt;
+            value.int32 = baro.altitude;
             break;
         case IBUS_SENSOR_TYPE_PRES:
-            value.uint32 = baro.baroPressure | (((uint32_t)getTemperature()) << 19);
+            value.uint32 = baro.pressure | (((uint32_t)getTemperature()) << 19);
             break;
 #endif
 #endif //defined(TELEMETRY_IBUS_EXTENDED)

src/test/unit/rx_ibus_unittest.cc

@@ -42,7 +42,7 @@ extern "C" {
     uint8_t batteryCellCount = 3;
     float rcCommand[4] = {0, 0, 0, 0};
     int16_t telemTemperature1 = 0;
-    baro_t baro = { .baroTemperature = 50 };
+    baro_t baro = { .temperature = 50 };
     telemetryConfig_t telemetryConfig_System;
     timeUs_t rxFrameTimeUs(void) { return 0; }
 }

src/test/unit/rx_sumd_unittest.cc

@@ -41,7 +41,7 @@ extern "C" {
     uint8_t batteryCellCount = 3;
     float rcCommand[4] = {0, 0, 0, 0};
     int16_t telemTemperature1 = 0;
-    baro_t baro = { .baroTemperature = 50 };
+    baro_t baro = { .temperature = 50 };
     telemetryConfig_t telemetryConfig_System;
     timeUs_t rxFrameTimeUs(void) { return 0; }
 }