Merge ef8e085f39 into acbab53d13

mk/source.mk

@@ -160,6 +160,8 @@ COMMON_SRC = \
             flight/alt_hold_wing.c \
             flight/autopilot_multirotor.c \
             flight/autopilot_wing.c \
+            flight/kalman_filter_1d.c \
+            flight/altitude.c \
             flight/dyn_notch_filter.c \
             flight/failsafe.c \
             flight/gps_rescue_multirotor.c \

src/main/common/filter.c

@@ -165,12 +165,23 @@ float filterGetNotchQ(float centerFreq, float cutoffFreq)
     return centerFreq * cutoffFreq / (centerFreq * centerFreq - cutoffFreq * cutoffFreq);
 }
 
+float filterGetBandQ(float lowerFreq, float higherFreq)
+{
+    float centerFreq = sqrtf(lowerFreq * higherFreq);
+    return centerFreq / (higherFreq - lowerFreq);
+}
+
 /* sets up a biquad filter as a 2nd order butterworth LPF */
 void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate)
 {
     biquadFilterInit(filter, filterFreq, refreshRate, BIQUAD_Q, FILTER_LPF, 1.0f);
 }
 
+void biquadFilterInitHPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate)
+{
+    biquadFilterInit(filter, filterFreq, refreshRate, BIQUAD_Q, FILTER_HPF, 1.0f);
+}
+
 void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType, float weight)
 {
     biquadFilterUpdate(filter, filterFreq, refreshRate, Q, filterType, weight);
@@ -198,6 +209,13 @@ FAST_CODE void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint
         filter->a1 = -2 * cs;
         filter->a2 = 1 - alpha;
         break;
+    case FILTER_HPF:
+        filter->b0 = 0.5f * (1 + cs);
+        filter->b1 = -1 * (1 + cs); // the negative sign here is not in the TI document
+        filter->b2 = filter->b0;
+        filter->a1 = -2 * cs;
+        filter->a2 = 1 - alpha;
+        break;
     case FILTER_NOTCH:
         filter->b0 = 1;
         filter->b1 = -2 * cs;

src/main/common/filter.h

@@ -38,6 +38,7 @@ typedef enum {
     FILTER_LPF,    // 2nd order Butterworth section
     FILTER_NOTCH,
     FILTER_BPF,
+    FILTER_HPF,
 } biquadFilterType_e;
 
 typedef struct pt1Filter_s {
@@ -116,8 +117,10 @@ void pt3FilterUpdateCutoff(pt3Filter_t *filter, float k);
 float pt3FilterApply(pt3Filter_t *filter, float input);
 
 float filterGetNotchQ(float centerFreq, float cutoffFreq);
+float filterGetBandQ(float lowerFreq, float higherFreq);
 
 void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate);
+void biquadFilterInitHPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate);
 void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType, float weight);
 void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType, float weight);
 void biquadFilterUpdateLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate);

src/main/fc/tasks.c

@@ -60,12 +60,12 @@
 #include "flight/pid.h"
 #include "flight/position.h"
 #include "flight/pos_hold.h"
+#include "flight/altitude.h"
 
 #include "io/asyncfatfs/asyncfatfs.h"
 #include "io/beeper.h"
 #include "io/dashboard.h"
 #include "io/flashfs.h"
-#include "io/gimbal_control.h"
 #include "io/gps.h"
 #include "io/ledstrip.h"
 #include "io/piniobox.h"
@@ -159,6 +159,7 @@ static void taskBatteryAlerts(timeUs_t currentTimeUs)
 static void taskUpdateAccelerometer(timeUs_t currentTimeUs)
 {
     accUpdate(currentTimeUs);
+    updateAccItegralCallback(currentTimeUs);
 }
 #endif
 
@@ -262,6 +263,7 @@ static void taskUpdateBaro(timeUs_t currentTimeUs)
         if (newDeadline != 0) {
             rescheduleTask(TASK_SELF, newDeadline);
         }
+        updateBaroStateCallback();
     }
 }
 #endif
@@ -475,9 +477,6 @@ task_attribute_t task_attributes[TASK_COUNT] = {
     [TASK_RC_STATS] = DEFINE_TASK("RC_STATS", NULL, NULL, rcStatsUpdate, TASK_PERIOD_HZ(100), TASK_PRIORITY_LOW),
 #endif
 
-#ifdef USE_GIMBAL
-    [TASK_GIMBAL] = DEFINE_TASK("GIMBAL", NULL, NULL, gimbalUpdate, TASK_PERIOD_HZ(100), TASK_PRIORITY_MEDIUM),
-#endif
 };
 
 task_t *getTask(unsigned taskId)
@@ -667,8 +666,4 @@ void tasksInit(void)
 #ifdef USE_RC_STATS
     setTaskEnabled(TASK_RC_STATS, true);
 #endif
-
-#ifdef USE_GIMBAL
-    setTaskEnabled(TASK_GIMBAL, true);
-#endif
 }

src/main/flight/altitude.c

@@ -0,0 +1,589 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Betaflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Betaflight. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <math.h>
+#include <limits.h>
+
+#include "platform.h"
+
+#include "build/debug.h"
+
+#include "drivers/system.h"
+#include "drivers/time.h"
+
+#include "common/maths.h"
+#include "common/filter.h"
+#include "common/time.h"
+
+#include "sensors/sensors.h"
+#include "sensors/barometer.h"
+#include "sensors/acceleration.h"
+#include "sensors/rangefinder.h"
+
+
+#include "fc/runtime_config.h"
+
+#include "io/gps.h"
+
+#include "flight/imu.h"
+#include "flight/position.h"
+#include "flight/kalman_filter_1d.h"
+#include "flight/altitude.h"
+
+
+#define SENSOR_VEL_ERROR_THRESH   1000.0f // the error above this value excludes the sensor from the fusion and changes the offset
+#define SENSOR_VEL_MAX_ERROR      10000.0f
+#define SENSOR_MAX_PENALITY_ITERS 10
+#define SENSOR_MAX_OFFSET_ERROR   1000.0f
+
+#define BARO_VAR_ALT_COEFF       0.01f
+#define BARO_VAR_VEL_COEFF       0.01f
+#define BARO_VAR_TEMP_COEFF      0.01f
+#define BARO_VAR_VEL_ERROR_COEFF 1.00f
+#define BARO_TASK_INTERVAL_CALC_ITER 10 
+
+#define RANGEFINDER_ACC_ERROR_THRESH    50.0f
+#define RANGEFINDER_RAPID_ERR_THRESH    100.0f
+#define RANGEFINDER_VAR_VEL_ERROR_COEFF 2.0f
+#define RANGEFINDER_CONST_VAR           10.0f
+
+#define GPS_VAR_DOP_COEFF       1.0f
+#define GPS_VAR_VEL_ERROR_COEFF 2.0f
+#define GPS_RAPID_ERR_THRESH    100.0f
+#define GPS_ACC_ERROR_THRESH    200.0f
+#define GPS_PDOP_MIN_THRESHOLD  400.0f
+
+typedef enum {
+#ifdef USE_ACC
+    SF_ACC,
+#endif
+#ifdef USE_BARO
+    SF_BARO,
+#endif
+#ifdef USE_GPS
+    SF_GPS,
+#endif
+// the only must for the order of the sensors is that the local sensors like the rangefinder should be last after the global sensors like the GPS and Baro
+#ifdef USE_RANGEFINDER
+    SF_RANGEFINDER,
+#endif
+    SENSOR_COUNT
+    } altSensor_e;
+
+typedef struct sensorState_s {
+    float currentAltReadingCm;
+    float zeroAltOffsetCm;
+    float velocityAltCmS;
+    float variance;
+    uint32_t deltaTimeMs;
+    float offsetError;
+    uint32_t velError;
+    altSensor_e type;
+    uint8_t penalityIters;
+    bool isValid;
+    bool toFuse;
+    void (*updateReading)(struct sensorState_s *sensor);
+    void (*updateVariance)(struct sensorState_s *sensor);
+    void (*updateOffset)(struct sensorState_s *sensor);
+} sensorState_t;
+
+typedef struct velocity3D_s {
+    float value;
+    bool isValid;
+} velocity3D_t;
+
+#ifdef USE_ACC
+void updateAccReading(sensorState_t *sensor);
+void applyAccVelFilter(float *velocity);
+void updateAccItegralCallback(timeUs_t currentTimeUs);
+
+typedef struct accIntegral_s {
+    float vel[XYZ_AXIS_COUNT + 1]; // 3 axis + 1 for the 3D magnitude
+    timeDelta_t deltaTimeUs;
+} accIntegral_t;
+accIntegral_t accIntegral;
+#endif
+
+#ifdef USE_BARO
+void updateBaroReading(sensorState_t *sensor);
+void updateBaroVariance(sensorState_t *sensor);
+void updateBaroOffset(sensorState_t *sensor);
+void applyBaroFilter(float *dst, float newValue);
+#endif
+
+#ifdef USE_GPS
+void updateGpsReading(sensorState_t *sensor);
+void updateGpsVariance(sensorState_t *sensor);
+#endif
+
+#ifdef USE_RANGEFINDER
+void updateRangefinderReading(sensorState_t *sensor);
+void updateRangefinderVariance(sensorState_t *sensor);
+#endif
+
+void updateSensorOffset(sensorState_t *sensor);
+void updateVelError(sensorState_t *sensor);
+void updateSensorFusability(sensorState_t *sensor);
+bool sensorUpdateIteration(sensorState_t *sensor);
+void doNothing(sensorState_t *sensor);
+
+static sensorState_t altSenFusSources[SENSOR_COUNT];
+static KalmanFilter kf;
+static velocity3D_t velocity3DCmS;
+static altitudeState_t altitudeState;
+
+void altSensorFusionInit(void) {
+velocity3DCmS.value = 0;
+velocity3DCmS.isValid = false;
+
+#ifdef USE_ACC
+    altSenFusSources[SF_ACC].updateReading  = updateAccReading;
+    altSenFusSources[SF_ACC].updateVariance = doNothing;
+    altSenFusSources[SF_ACC].updateOffset   = doNothing;
+    altSenFusSources[SF_ACC].type = SF_ACC;
+    altSenFusSources[SF_ACC].isValid = false;
+    altSenFusSources[SF_ACC].toFuse = false;
+    velocity3DCmS.isValid = true;
+#endif
+    
+#ifdef USE_BARO
+    altSenFusSources[SF_BARO].updateReading  = updateBaroReading;
+    altSenFusSources[SF_BARO].updateVariance = updateBaroVariance;
+    altSenFusSources[SF_BARO].updateOffset   = updateBaroOffset;
+    altSenFusSources[SF_BARO].type = SF_BARO;
+    altSenFusSources[SF_BARO].isValid = isBaroReady();
+    altSenFusSources[SF_BARO].toFuse = true;
+#endif
+
+#ifdef USE_GPS
+    altSenFusSources[SF_GPS].updateReading  = updateGpsReading;
+    altSenFusSources[SF_GPS].updateVariance = updateGpsVariance;
+    altSenFusSources[SF_GPS].updateOffset   = updateSensorOffset;
+    altSenFusSources[SF_GPS].type = SF_GPS;
+    altSenFusSources[SF_GPS].isValid = false;
+    altSenFusSources[SF_GPS].toFuse = true;
+    velocity3DCmS.isValid = true;
+#endif
+
+#ifdef USE_RANGEFINDER
+    altSenFusSources[SF_RANGEFINDER].updateReading  = updateRangefinderReading;
+    altSenFusSources[SF_RANGEFINDER].updateVariance = updateRangefinderVariance;
+    altSenFusSources[SF_RANGEFINDER].updateOffset   = updateSensorOffset;
+    altSenFusSources[SF_RANGEFINDER].type = SF_RANGEFINDER;
+    altSenFusSources[SF_RANGEFINDER].isValid = false;
+    altSenFusSources[SF_RANGEFINDER].toFuse = true;
+#endif
+
+    kf_init(&kf, 0.0f, 1.0f, 10.0f);
+}
+
+bool sensorUpdateIteration(sensorState_t *sensor) {
+
+    sensor->updateReading(sensor);
+    updateVelError(sensor);
+    updateSensorFusability(sensor); // this should handle the case of sudden jumps in the sensor readings compared to the accelerometer velocity estimation
+    sensor->updateVariance(sensor);
+    sensor->updateOffset(sensor);
+    
+    if (sensor->isValid && sensor->toFuse) {
+        SensorMeasurement tempSensorMeas;
+        tempSensorMeas.value    = sensor->currentAltReadingCm - sensor->zeroAltOffsetCm;
+        tempSensorMeas.variance = sensor->variance;
+#ifdef USE_GPS
+        if (sensor->type == SF_GPS) { // ignore the GPS for now, TODO: add the gps to the fusion
+            return false;
+        }
+#endif
+        kf_update(&kf, tempSensorMeas);
+        return true;
+    }
+    return false;
+}
+
+bool altSensorFusionUpdate(void) {
+    static timeMs_t prevTimeMs = 0;
+    timeMs_t deltaTimeMs = millis() - prevTimeMs;
+    prevTimeMs = millis();
+
+    kf_update_variance(&kf);
+    float previousAltitude = altitudeState.distCm;
+    bool haveAltitude = false;
+    for (sensorState_t * sensor = altSenFusSources; sensor < altSenFusSources + SENSOR_COUNT; sensor++) {
+        haveAltitude |= sensorUpdateIteration(sensor);
+    }
+
+    altitudeState.distCm     = kf.estimatedValue; 
+    altitudeState.variance   = kf.estimatedVariance;
+    altitudeState.velocityCm = (altitudeState.distCm - previousAltitude) * 1000 / deltaTimeMs;
+    previousAltitude         = altitudeState.distCm;
+
+#ifdef USE_ACC
+    DEBUG_SET(DEBUG_ALTITUDE, 3, lrintf(altSenFusSources[SF_ACC].velocityAltCmS));
+#endif
+
+#ifdef USE_BARO    
+    DEBUG_SET(DEBUG_ALTITUDE, 1, lrintf(altSenFusSources[SF_BARO].currentAltReadingCm - altSenFusSources[SF_BARO].zeroAltOffsetCm));
+    DEBUG_SET(DEBUG_ALTITUDE, 5, lrintf(altSenFusSources[SF_BARO].zeroAltOffsetCm));
+#endif
+
+#ifdef USE_GPS
+    DEBUG_SET(DEBUG_ALTITUDE, 0, lrintf(10000 - gpsSol.dop.pdop));
+    DEBUG_SET(DEBUG_ALTITUDE, 2, lrintf(altSenFusSources[SF_GPS].currentAltReadingCm  - altSenFusSources[SF_GPS].zeroAltOffsetCm));
+#endif
+
+#ifdef USE_RANGEFINDER
+    DEBUG_SET(DEBUG_ALTITUDE, 6, lrintf(altSenFusSources[SF_RANGEFINDER].zeroAltOffsetCm));
+    DEBUG_SET(DEBUG_ALTITUDE, 4, lrintf(altSenFusSources[SF_RANGEFINDER].currentAltReadingCm - altSenFusSources[SF_RANGEFINDER].zeroAltOffsetCm));
+#endif
+    DEBUG_SET(DEBUG_ALTITUDE, 7, lrintf(altitudeState.distCm));
+
+
+    return haveAltitude;
+}
+
+void updateSensorOffset(sensorState_t *sensor) {
+    if (!sensor->isValid) {
+        return;
+    }
+
+    if (!ARMING_FLAG(ARMED)) {// default offset update when not armed
+        sensor->zeroAltOffsetCm = 0.2f * sensor->currentAltReadingCm + 0.8f * sensor->zeroAltOffsetCm;
+    } else { // when armed the offset should be updated according to the velocity error value
+        float newOffset = sensor->currentAltReadingCm - kf.estimatedValue;
+        if (sensor->penalityIters > 0) {
+            sensor->zeroAltOffsetCm = 0.5f * (newOffset + sensor->zeroAltOffsetCm);
+        } else { // detect a ramp in the sensor readings by accumulating the error
+            sensor->offsetError += newOffset - sensor->zeroAltOffsetCm;
+            if (fabsf(sensor->offsetError) > SENSOR_MAX_OFFSET_ERROR) {
+                sensor->zeroAltOffsetCm = 0.01f * newOffset + 0.99f * sensor->zeroAltOffsetCm;
+                sensor->offsetError = 0.99f * sensor->offsetError; // decaying the error
+            }
+        }
+    }
+}
+
+void updateVelError(sensorState_t *sensor) {
+    if (!sensor->isValid || sensor->type == SF_ACC) {
+        return;
+    }
+    sensor->velError = 0.1f * (sq(altSenFusSources[SF_ACC].velocityAltCmS - sensor->velocityAltCmS) / (100.f))
+                     + 0.9f * sensor->velError;
+
+    sensor->velError = constrain(sensor->velError, 0, SENSOR_VEL_MAX_ERROR);
+}
+
+void updateSensorFusability(sensorState_t *sensor) {
+    if (!sensor->isValid || sensor->type == SF_ACC) {
+        return;
+    }
+    
+    if (sensor->velError > SENSOR_VEL_ERROR_THRESH) {
+        sensor->penalityIters = SENSOR_MAX_PENALITY_ITERS;
+    } else if (sensor->penalityIters > 0) {
+        sensor->penalityIters--;
+    }
+    
+    sensor->toFuse = (sensor->penalityIters == 0);
+}
+
+altitudeState_t *getAltitudeState(void) {
+    return &altitudeState;
+}
+
+void doNothing(sensorState_t *sensor) {
+    UNUSED(sensor);
+}
+
+// ======================================================================================================
+// ==================================== Sensor specific functions =======================================
+// ======================================================================================================
+void updateAccItegralCallback(timeUs_t currentTimeUs) { // this is called in the acc update task
+    static bool firstRun = true;
+    static timeUs_t prevTimeUs = 0;
+    if (firstRun) {
+        prevTimeUs = currentTimeUs;
+        firstRun = false;
+        return;
+    }
+
+    timeDelta_t deltaTimeUs = cmpTimeUs(currentTimeUs, prevTimeUs);
+    for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
+        accIntegral.vel[i] += acc.accADC.v[i] * (float)deltaTimeUs / 1e6f;
+    }
+    accIntegral.vel[XYZ_AXIS_COUNT] += (acc.accMagnitude - 1.0f) * (float)deltaTimeUs / 1e6f;
+    accIntegral.deltaTimeUs += deltaTimeUs;
+    prevTimeUs = currentTimeUs;
+}
+
+void updateAccReading(sensorState_t *sensor) {
+    static float velDriftZ = 0.0f;
+
+    static float accVelZ = 0.0f;
+    // given the attiude roll and pitch angles of the drone, and the integrated acceleration in x,y and z
+    // calculate the integrated acceleration in the z direction in the world frame
+    float roll  = DEGREES_TO_RADIANS((float)attitude.values.roll / 10.0f); // integer devision to reduce noise
+    float pitch = DEGREES_TO_RADIANS((float)attitude.values.pitch / 10.0f);
+    
+    float cosPitch = cosf(pitch);
+
+    float velWorldZ = - accIntegral.vel[0] * sinf(pitch) 
+                    +   accIntegral.vel[1] * cosPitch * sinf(roll)
+                    +   accIntegral.vel[2] * cosPitch * cosf(roll);
+
+    float gravityVel = (float)accIntegral.deltaTimeUs / 1e6f; // g/Us to g/S
+    
+    float velCmSecZ = ((velWorldZ * acc.dev.acc_1G_rec) - gravityVel) * 981.0f; // g to cm/s
+
+    velCmSecZ = (int)(velCmSecZ * 10) / 10.0f;
+
+    accVelZ += velCmSecZ;
+
+    velDriftZ  = 0.005f * accVelZ  + (1.0f - 0.005f) * velDriftZ;
+
+    sensor->velocityAltCmS = accVelZ  - velDriftZ;
+
+    velocity3DCmS.value  = (0.1f * fabsf(accIntegral.vel[XYZ_AXIS_COUNT] * 981.0f)) + (0.9f * velocity3DCmS.value);
+    // applyAccVelFilter(&sensor->velocityAltCmS);
+    // sensor->currentAltReadingCm += sensor->velocityAltCmS * ((float)accIntegral.deltaTimeUs/1e6f);
+
+    for (int i = 0; i <= XYZ_AXIS_COUNT; i++) {
+        accIntegral.vel[i] = 0;
+    }
+    accIntegral.deltaTimeUs = 0;
+
+}
+
+void applyAccVelFilter(float *velocity) {
+    static pt2Filter_t velFilter;
+    static bool firstRun = true;
+
+    if (firstRun) {
+        pt2FilterInit(&velFilter, 0.5);
+        firstRun = false;
+    }
+    
+    *velocity = pt2FilterApply(&velFilter, *velocity);
+}
+
+
+#ifdef USE_BARO
+void updateBaroStateCallback(void) {
+    applyBaroFilter(&altSenFusSources[SF_BARO].currentAltReadingCm, getBaroAltitude());
+}
+
+void updateBaroReading(sensorState_t *sensor) {
+    if (!sensor->isValid) {
+        return;
+    }
+
+    static bool firstRun = true;
+    static float previousAltitude = 0.0f;
+    static uint32_t prevTimeMs = 0;
+
+    if (firstRun) { // init
+        previousAltitude        = sensor->currentAltReadingCm;
+        sensor->zeroAltOffsetCm = sensor->currentAltReadingCm; // init the offset with the first reading
+        firstRun = false;
+        prevTimeMs = millis();
+    }
+
+    sensor->deltaTimeMs = millis() - prevTimeMs;
+
+    sensor->velocityAltCmS = (sensor->currentAltReadingCm - previousAltitude) * 1000 / sensor->deltaTimeMs;
+    previousAltitude = sensor->currentAltReadingCm;
+
+    prevTimeMs = millis();
+}
+
+void updateBaroVariance(sensorState_t *sensor) {
+    if (!sensor->isValid) {
+        return;
+    }
+
+    static float stationaryVariance = 0;
+    static float stationaryMean = 0;
+    static uint16_t n = 0;
+    
+    float velocity = 0;
+
+    if (!ARMING_FLAG(ARMED)) { // approximating the mean and variance of the baro readings during the stationary phase
+        stationaryMean     +=    (sensor->currentAltReadingCm - stationaryMean)                       / (n + 1);
+        stationaryVariance += (sq(sensor->currentAltReadingCm - stationaryMean) - stationaryVariance) / (n + 1);
+        n++;        
+        velocity = 0;
+    } else {
+        velocity = velocity3DCmS.isValid ? (float)velocity3DCmS.value : (altitudeState.velocityCm);
+    }
+
+    float newVariance = stationaryVariance
+                      + BARO_VAR_VEL_ERROR_COEFF * (float)sensor->velError
+                      + BARO_VAR_VEL_COEFF       * fabsf(velocity)
+                      + BARO_VAR_TEMP_COEFF      * fabsf((float)getBaroTemperature()) 
+                      + BARO_VAR_ALT_COEFF       * fabsf(sensor->currentAltReadingCm);
+
+    sensor->variance = 0.9f * sensor->variance + 0.1f * newVariance;
+}
+
+void updateBaroOffset(sensorState_t *sensor) {
+    if (!sensor->isValid) {
+        return;
+    }
+
+    if (!ARMING_FLAG(ARMED)) { // default offset update when not armed
+        sensor->zeroAltOffsetCm = 0.2f * sensor->currentAltReadingCm + 0.8f * sensor->zeroAltOffsetCm;
+    }
+}
+
+void applyBaroFilter(float *dst, float newValue) {
+    static pt2Filter_t baroLpfFilter;
+    static bool firstRun = true;
+    // calculate the task interval for the first few iterations in ms (can this be done better ? from the baro dev ?)
+    static int8_t taskIntervalIter = BARO_TASK_INTERVAL_CALC_ITER;
+    static uint16_t taskInterval = 0;
+    if (taskIntervalIter > 0) {
+        static bool firstIter = true;
+        static uint16_t prevTimeMs = 0;
+        if (firstIter) {
+            prevTimeMs = millis();
+            firstIter = false;
+            return;
+        }
+        taskInterval += millis() - prevTimeMs;
+        taskIntervalIter--;
+        prevTimeMs = millis();
+        if (taskIntervalIter == 0) {
+            taskInterval /= BARO_TASK_INTERVAL_CALC_ITER;
+        }
+        return;
+    }
+    if (firstRun) {
+        const float altitudeCutoffHz = positionConfig()->altitude_lpf / 100.0f;
+        const float altitudeGain     = pt2FilterGain(altitudeCutoffHz,  taskInterval);
+        pt2FilterInit(&baroLpfFilter, altitudeGain);
+        firstRun = false;
+    }
+
+    *dst = pt2FilterApply(&baroLpfFilter, newValue);
+}
+#endif // USE_BARO
+
+#ifdef USE_GPS
+void updateGpsReading(sensorState_t *sensor) {
+    static uint32_t prevTimeStamp = 0;
+    static bool firstRun = true;
+    static float previousAltitude = 0.0f;
+    bool hasNewData = gpsSol.time != prevTimeStamp;
+    bool dopIsGood = (gpsSol.dop.pdop > 0 && gpsSol.dop.pdop < GPS_PDOP_MIN_THRESHOLD)
+                   || (gpsSol.dop.hdop > 0 && gpsSol.dop.hdop < GPS_PDOP_MIN_THRESHOLD);
+    
+    sensor->isValid = gpsIsHealthy()
+                   && sensors(SENSOR_GPS)
+                   && STATE(GPS_FIX) 
+                   && hasNewData
+                   && dopIsGood;
+
+    if (!sensor->isValid) {
+#ifndef USE_ACC
+        velocity3DCmS.isValid = false;
+#endif
+        return;
+    }
+
+    if (firstRun) {
+        previousAltitude = gpsSol.llh.altCm;
+        sensor->zeroAltOffsetCm = previousAltitude;
+        prevTimeStamp = gpsSol.time;
+        firstRun = false;
+    }
+
+    sensor->currentAltReadingCm = gpsSol.llh.altCm;
+
+#ifndef USE_ACC
+    velocity3DCmS.value = gpsSol.speed3d * 10;
+    velocity3DCmS.isValid = true;
+#endif
+    sensor->deltaTimeMs = gpsSol.time - prevTimeStamp;
+    sensor->velocityAltCmS = ((sensor->currentAltReadingCm - previousAltitude) * 1000.0f) / sensor->deltaTimeMs;
+    previousAltitude = sensor->currentAltReadingCm;
+    prevTimeStamp = gpsSol.time;
+}
+
+void updateGpsVariance(sensorState_t *sensor) {
+    if (!sensor->isValid) {
+        return;
+    }
+    
+    float newVariance = GPS_VAR_VEL_ERROR_COEFF * sensor->velError;
+
+    if (gpsSol.dop.vdop != 0) {
+        newVariance += GPS_VAR_DOP_COEFF * gpsSol.dop.vdop;
+    } else if (gpsSol.dop.pdop != 0) {
+        newVariance += GPS_VAR_DOP_COEFF * gpsSol.dop.pdop;
+    } else {
+        newVariance += 10000.0f; 
+    }
+
+    sensor->variance = newVariance;
+}
+
+#endif // USE_GPS
+
+#ifdef USE_RANGEFINDER
+
+void updateRangefinderReading(sensorState_t *sensor) {
+    static bool firstRun = true;
+    static float previousAltitude = 0.0f;
+    static float prevReadingTime = 0;
+    int32_t rfAlt   = getRangefinder()->calculatedAltitude;
+    bool hasNewData = getRangefinder()->lastValidResponseTimeMs != prevReadingTime;
+
+    sensor->isValid = rangefinderIsHealthy() 
+                   && sensors(SENSOR_RANGEFINDER) 
+                   && rfAlt >= 0 
+                   && hasNewData;
+
+    if (!sensor->isValid) {
+        return;
+    }
+
+    if (firstRun) {
+        previousAltitude = rfAlt;
+        firstRun = false;
+        prevReadingTime = getRangefinder()->lastValidResponseTimeMs;
+    }
+
+    sensor->deltaTimeMs = getRangefinder()->lastValidResponseTimeMs - prevReadingTime;
+    sensor->currentAltReadingCm = rfAlt;
+    sensor->velocityAltCmS = (sensor->currentAltReadingCm - previousAltitude) * 1000.0f / sensor->deltaTimeMs;
+    previousAltitude = sensor->currentAltReadingCm;
+    prevReadingTime = getRangefinder()->lastValidResponseTimeMs;
+}
+
+void updateRangefinderVariance(sensorState_t *sensor) {
+    if (!sensor->isValid) {
+        return;
+    }
+
+    float newVariance = RANGEFINDER_VAR_VEL_ERROR_COEFF * sensor->velError + RANGEFINDER_CONST_VAR;
+    sensor->variance = 0.9f * sensor->variance + 0.1f * newVariance;
+}
+
+#endif // USE_RANGEFINDER

src/main/flight/altitude.h

@@ -0,0 +1,32 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Betaflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Betaflight. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+typedef struct altitudeState_s {
+    float distCm;
+    float variance;
+    float velocityCm;
+} altitudeState_t;
+
+#ifdef USE_ACC
+void updateAccItegralCallback(timeUs_t currentTimeUs);
+#endif
+void altSensorFusionInit(void);
+bool altSensorFusionUpdate(void);
+void updateBaroStateCallback(void);
+altitudeState_t * getAltitudeState(void);

src/main/flight/kalman_filter_1d.c

@@ -0,0 +1,42 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Betaflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Betaflight. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "platform.h"
+#include "math.h"
+#include <stdbool.h>
+
+#include "kalman_filter_1d.h"
+
+
+void kf_init(KalmanFilter *kf, float initialValue, float initialVariance, float processVariance) {
+    kf->estimatedValue    = initialValue;
+    kf->estimatedVariance = initialVariance;
+    kf->processVariance   = processVariance;
+}
+
+void kf_update_variance(KalmanFilter *kf) {
+    // Increase the estimated variance by the process variance
+    kf->estimatedVariance += kf->processVariance;
+}
+
+void kf_update(KalmanFilter *kf, SensorMeasurement sensorMeas) {
+    float kalmanGain = kf->estimatedVariance / (kf->estimatedVariance + sensorMeas.variance);
+
+    kf->estimatedValue += kalmanGain * (sensorMeas.value - kf->estimatedValue);
+
+    kf->estimatedVariance = (1 - kalmanGain) * kf->estimatedVariance;
+}

src/main/flight/kalman_filter_1d.h

@@ -0,0 +1,36 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+typedef struct {
+    float estimatedValue;
+    float estimatedVariance;
+    float processVariance;
+} KalmanFilter;
+
+typedef struct {
+    float value;
+    float variance;
+} SensorMeasurement;
+
+void kf_init(KalmanFilter *kf, float initialValue, float initialVariance, float processVariance);
+void kf_update_variance(KalmanFilter *kf);
+void kf_update(KalmanFilter *kf, SensorMeasurement sensorMeas);

src/main/flight/position.c

@@ -36,6 +36,8 @@
 #include "flight/position.h"
 #include "flight/imu.h"
 #include "flight/pid.h"
+#include "flight/kalman_filter_1d.h"
+#include "flight/altitude.h"
 
 #include "io/gps.h"
 
@@ -43,34 +45,24 @@
 
 #include "sensors/sensors.h"
 #include "sensors/barometer.h"
+#include "sensors/rangefinder.h"
 
 #include "pg/pg.h"
 #include "pg/pg_ids.h"
 
+
 static float displayAltitudeCm = 0.0f;
 static bool altitudeAvailable = false;
 
 static float zeroedAltitudeCm = 0.0f;
 static float zeroedAltitudeDerivative = 0.0f;
 
-static pt2Filter_t altitudeLpf;
-static pt2Filter_t altitudeDerivativeLpf;
-
 #ifdef USE_VARIO
 static int16_t estimatedVario = 0; // in cm/s
 #endif
 
-void positionInit(void)
+void positionInit(void) {
-{
+altSensorFusionInit();
-    const float sampleTimeS = HZ_TO_INTERVAL(TASK_ALTITUDE_RATE_HZ);
-
-    const float altitudeCutoffHz = positionConfig()->altitude_lpf / 100.0f;
-    const float altitudeGain = pt2FilterGain(altitudeCutoffHz, sampleTimeS);
-    pt2FilterInit(&altitudeLpf, altitudeGain);
-
-    const float altitudeDerivativeCutoffHz = positionConfig()->altitude_d_lpf / 100.0f;
-    const float altitudeDerivativeGain = pt2FilterGain(altitudeDerivativeCutoffHz, sampleTimeS);
-    pt2FilterInit(&altitudeDerivativeLpf, altitudeDerivativeGain);
 }
 
 typedef enum {
@@ -88,134 +80,20 @@ PG_RESET_TEMPLATE(positionConfig_t, positionConfig,
     .altitude_d_lpf = 100,
 );
 
-#if defined(USE_BARO) || defined(USE_GPS)
+#if defined(USE_BARO) || defined(USE_GPS) || defined(USE_RANGEFINDER)
-void calculateEstimatedAltitude(void)
+void calculateEstimatedAltitude(void) {
-{
+    altitudeAvailable = altSensorFusionUpdate();
-    static bool wasArmed = false;
+    if (altitudeAvailable) {
-    static bool useZeroedGpsAltitude = false; // whether a zero for the GPS altitude value exists
+        zeroedAltitudeCm = getAltitudeState()->distCm;
-    static float gpsAltCm = 0.0f; // will hold last value on transient loss of 3D fix
+        zeroedAltitudeDerivative = getAltitudeState()->velocityCm;
-    static float gpsAltOffsetCm = 0.0f;
+        displayAltitudeCm = zeroedAltitudeCm;
-    static float baroAltOffsetCm = 0.0f;
-    static float newBaroAltOffsetCm = 0.0f;
-
-    float baroAltCm = 0.0f;
-    float gpsTrust = 0.3f; // if no pDOP value, use 0.3, intended range 0-1;
-    bool haveBaroAlt = false; // true if baro exists and has been calibrated on power up
-    bool haveGpsAlt = false; // true if GPS is connected and while it has a 3D fix, set each run to false
-
-    // *** Get sensor data
-#ifdef USE_BARO
-    if (sensors(SENSOR_BARO)) {
-        baroAltCm = getBaroAltitude();
-        haveBaroAlt = true; // false only if there is no sensor on the board, or it has failed
-    }
-#endif
-#ifdef USE_GPS
-    if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
-        // GPS_FIX means a 3D fix, which requires min 4 sats.
-        // On loss of 3D fix, gpsAltCm remains at the last value, haveGpsAlt becomes false, and gpsTrust goes to zero.
-        gpsAltCm = gpsSol.llh.altCm; // static, so hold last altitude value if 3D fix is lost to prevent fly to moon
-        haveGpsAlt = true; // goes false and stays false if no 3D fix
-        if (gpsSol.dop.pdop != 0) {
-            // pDOP of 1.0 is good.  100 is very bad.  Our gpsSol.dop.pdop values are *100
-            // When pDOP is a value less than 3.3, GPS trust will be stronger than default.
-            gpsTrust = 100.0f / gpsSol.dop.pdop;
-            // *** TO DO - investigate if we should use vDOP or vACC with UBlox units;
-        }
-        // always use at least 10% of other sources besides gps if available
-        gpsTrust = MIN(gpsTrust, 0.9f);
-    }
-#endif
-
-    //  ***  DISARMED  ***
-    if (!ARMING_FLAG(ARMED)) {
-        if (wasArmed) { // things to run once, on disarming, after being armed
-            useZeroedGpsAltitude = false; // reset, and wait for valid GPS data to zero the GPS signal
-            wasArmed = false;
-        }
-
-        newBaroAltOffsetCm = 0.2f * baroAltCm + 0.8f * newBaroAltOffsetCm; // smooth some recent baro samples
-        displayAltitudeCm = baroAltCm - baroAltOffsetCm; // if no GPS altitude, show un-smoothed Baro altitude in OSD and sensors tab, using most recent offset.
-
-        if (haveGpsAlt) { // watch for valid GPS altitude data to get a zero value from
-            gpsAltOffsetCm = gpsAltCm; // update the zero offset value with the most recent valid gps altitude reading
-            useZeroedGpsAltitude = true; // we can use this offset to zero the GPS altitude on arming
-            if (!(positionConfig()->altitude_source == BARO_ONLY)) {
-                displayAltitudeCm = gpsAltCm; // estimatedAltitude shows most recent ASL GPS altitude in OSD and sensors, while disarmed
-            }
-        }
-        zeroedAltitudeCm = 0.0f; // always hold relativeAltitude at zero while disarmed
-        DEBUG_SET(DEBUG_ALTITUDE, 2, gpsAltCm / 100.0f); // Absolute altitude ASL in metres, max 32,767m
-    //  ***  ARMED  ***
-    } else {
-        if (!wasArmed) { // things to run once, on arming, after being disarmed
-            baroAltOffsetCm = newBaroAltOffsetCm;
-            wasArmed = true;
-        }
-
-        baroAltCm -= baroAltOffsetCm; // use smoothed baro with most recent zero from disarm period
-
-        if (haveGpsAlt) { // update relativeAltitude with every new gpsAlt value, or hold the previous value until 3D lock recovers
-            if (!useZeroedGpsAltitude && haveBaroAlt) { // armed without zero offset, can use baro values to zero later
-                gpsAltOffsetCm = gpsAltCm - baroAltCm; // not very accurate
-                useZeroedGpsAltitude = true;
-            }
-            if (useZeroedGpsAltitude) { // normal situation
-                zeroedAltitudeCm = gpsAltCm - gpsAltOffsetCm; // now that we have a GPS offset value, we can use it to zero relativeAltitude
-            }
-        } else {
-            gpsTrust = 0.0f;
-            // TO DO - smoothly reduce GPS trust, rather than immediately dropping to zero for what could be only a very brief loss of 3D fix
-        }
-        DEBUG_SET(DEBUG_ALTITUDE, 2, lrintf(zeroedAltitudeCm / 10.0f)); // Relative altitude above takeoff, to 0.1m, rolls over at 3,276.7m
-
-        // Empirical mixing of GPS and Baro altitudes
-        if (useZeroedGpsAltitude && (positionConfig()->altitude_source == DEFAULT || positionConfig()->altitude_source == GPS_ONLY)) {
-            if (haveBaroAlt && positionConfig()->altitude_source == DEFAULT) {
-                // mix zeroed GPS with Baro altitude data, if Baro data exists if are in default altitude control mode
-                const float absDifferenceM = fabsf(zeroedAltitudeCm - baroAltCm) / 100.0f * positionConfig()->altitude_prefer_baro / 100.0f;
-                if (absDifferenceM > 1.0f) { // when there is a large difference, favour Baro
-                    gpsTrust /=  absDifferenceM;
-                }
-                zeroedAltitudeCm = zeroedAltitudeCm * gpsTrust + baroAltCm * (1.0f - gpsTrust);
-            }
-        } else if (haveBaroAlt && (positionConfig()->altitude_source == DEFAULT || positionConfig()->altitude_source == BARO_ONLY)) {
-            zeroedAltitudeCm = baroAltCm; // use Baro if no GPS data, or we want Baro only
-        }
-    }
-
-    zeroedAltitudeCm = pt2FilterApply(&altitudeLpf, zeroedAltitudeCm);
-    // NOTE: this filter must receive 0 as its input, for the whole disarmed time, to ensure correct zeroed values on arming
-
-    if (wasArmed) {
-        displayAltitudeCm = zeroedAltitudeCm; // while armed, show filtered relative altitude in OSD / sensors tab
-    }
-
-    // *** calculate Vario signal
-    static float previousZeroedAltitudeCm = 0.0f;
-    zeroedAltitudeDerivative = (zeroedAltitudeCm - previousZeroedAltitudeCm) * TASK_ALTITUDE_RATE_HZ; // cm/s
-    previousZeroedAltitudeCm = zeroedAltitudeCm;
-
-    zeroedAltitudeDerivative = pt2FilterApply(&altitudeDerivativeLpf, zeroedAltitudeDerivative);
-
 #ifdef USE_VARIO
-    estimatedVario = lrintf(zeroedAltitudeDerivative);
+        estimatedVario = applyDeadband(lrintf(zeroedAltitudeDerivative), 10);
-    estimatedVario = applyDeadband(estimatedVario, 10); // ignore climb rates less than 0.1 m/s
 #endif
-
+    }
-    // *** set debugs
-    DEBUG_SET(DEBUG_ALTITUDE, 0, (int32_t)(100 * gpsTrust));
-    DEBUG_SET(DEBUG_ALTITUDE, 1, lrintf(baroAltCm / 10.0f)); // Relative altitude above takeoff, to 0.1m, rolls over at 3,276.7m
-#ifdef USE_VARIO
-    DEBUG_SET(DEBUG_ALTITUDE, 3, estimatedVario);
-#endif
-    DEBUG_SET(DEBUG_RTH, 1, lrintf(displayAltitudeCm / 10.0f));
-    DEBUG_SET(DEBUG_AUTOPILOT_ALTITUDE, 2, lrintf(zeroedAltitudeCm));
-
-    altitudeAvailable = haveGpsAlt || haveBaroAlt;
 }
 
-#endif //defined(USE_BARO) || defined(USE_GPS)
+#endif //defined(USE_BARO) || defined(USE_GPS) || defined(USE_RANGEFINDER)
 
 float getAltitudeCm(void)
 {
@@ -224,7 +102,7 @@ float getAltitudeCm(void)
 
 float getAltitudeDerivative(void)
 {
-    return zeroedAltitudeDerivative; // cm/s
+    return zeroedAltitudeDerivative;
 }
 
 bool isAltitudeAvailable(void) {
@@ -233,7 +111,7 @@ bool isAltitudeAvailable(void) {
 
 int32_t getEstimatedAltitudeCm(void)
 {
-    return lrintf(displayAltitudeCm);
+    return displayAltitudeCm; // cm
 }
 
 #ifdef USE_GPS

src/main/flight/position.h

@@ -41,4 +41,3 @@ int32_t getEstimatedAltitudeCm(void);
 float getAltitudeAsl(void);
 int16_t getEstimatedVario(void);
 bool isAltitudeAvailable(void);
-

src/main/sensors/barometer.c

@@ -527,4 +527,9 @@ static void performBaroCalibrationCycle(const float altitude)
     }
 }
 
+int32_t getBaroTemperature(void)
+{
+    return baro.temperature;
+} 
+
 #endif /* BARO */

src/main/sensors/barometer.h

@@ -70,3 +70,4 @@ void baroSetGroundLevel(void);
 uint32_t baroUpdate(timeUs_t currentTimeUs);
 bool isBaroReady(void);
 float getBaroAltitude(void);
+int32_t getBaroTemperature(void);

src/main/sensors/rangefinder.c

@@ -353,5 +353,10 @@ bool rangefinderIsHealthy(void)
 {
     return (millis() - rangefinder.lastValidResponseTimeMs) < RANGEFINDER_HARDWARE_TIMEOUT_MS;
 }
+
+rangefinder_t * getRangefinder(void)
+{
+    return &rangefinder;
+}
 #endif
 

src/main/sensors/rangefinder.h

@@ -65,3 +65,4 @@ int32_t rangefinderGetLatestRawAltitude(void);
 void rangefinderUpdate(void);
 bool rangefinderProcess(float cosTiltAngle);
 bool rangefinderIsHealthy(void);
+rangefinder_t * getRangefinder(void);

src/test/unit/arming_prevention_unittest.cc

@@ -43,6 +43,7 @@ extern "C" {
     #include "flight/pid.h"
     #include "flight/position.h"
     #include "flight/servos.h"
+    #include "flight/altitude.h"
 
     #include "io/beeper.h"
     #include "io/gps.h"

src/test/unit/flight_imu_unittest.cc

@@ -46,6 +46,8 @@ extern "C" {
     #include "flight/mixer.h"
     #include "flight/pid.h"
     #include "flight/position.h"
+    #include "flight/kalman_filter_1d.h"
+    #include "flight/altitude.h"
 
     #include "io/gps.h"
 
@@ -68,6 +70,36 @@ extern "C" {
                              const float dcmKpGain);
     float imuCalcMagErr(void);
     float imuCalcCourseErr(float courseOverGround);
+    void kf_init(KalmanFilter *kf, float initialValue, float initialVariance, float processVariance){
+        UNUSED(kf);
+        UNUSED(initialValue);
+        UNUSED(initialVariance);
+        UNUSED(processVariance);
+    }
+    void kf_update_variance(KalmanFilter *kf) {
+        UNUSED(kf);
+    }
+    void kf_update(KalmanFilter *kf, SensorMeasurement sensorMeas) {
+       UNUSED(sensorMeas);
+       UNUSED(kf);
+    }
+    void updateBaroVariance(SensorMeasurement * sensorMeas, float baroNonZeroedAlt) {
+        UNUSED(sensorMeas);
+        UNUSED(baroNonZeroedAlt);
+    }
+    void altSensorFusionInit(void) {
+
+    }
+    bool altSensorFusionUpdate(void) {
+        return true;
+    }
+    void updateBaroStateCallback(void) {
+
+    }
+    altitudeState_t * getAltitudeState(void) {
+        return nullptr;
+    }
+
     extern quaternion_t q;
     extern matrix33_t rMat;
     extern bool attitudeIsEstablished;

src/test/unit/scheduler_unittest.cc

@@ -22,6 +22,7 @@ extern "C" {
     #include "platform.h"
     #include "scheduler/scheduler.h"
     #include "scheduler_stubs.h"
+    #include "flight/altitude.h"
 }
 
 #include "unittest_macros.h"