Accelerometer and Barometer temperature compensation

docs/Settings.md

@@ -122,6 +122,16 @@ Frequency of the software notch filter to remove mechanical vibrations from the
 
 ---
 
+### acc_temp_correction
+
+Accelerometer temperature correction factor to compensate for acceleromter drift with changes in acceleromter temperature [cm/s2 per Degs C]. Internally limited to between -50 and 50. Typical setting for MPU6000 acceleromter is around 2.5. Setting to 51 initiates auto calibration which ends after 5 minutes or on first Arm.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 0 | -50 | 51 |
+
+---
+
 ### accgain_x
 
 Calculated value after '6 position avanced calibration'. Uncalibrated value is 4096. See Wiki page.
@@ -452,6 +462,16 @@ Selection of baro hardware. See Wiki Sensor auto detect and hardware failure det
 
 ---
 
+### baro_temp_correction
+
+Baro temperature correction factor to compensate for Baro altitude drift with changes in Baro temperature [cm/Degs C]. Internally limited to between -50 and 50. Typical setting for BMP280 Baro is around 20. Setting to 51 initiates auto calibration which ends after 5 minutes or on first Arm.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 0 | -50 | 51 |
+
+---
+
 ### bat_cells
 
 Number of cells of the battery (0 = auto-detect), see battery documentation. 7S, 9S and 11S batteries cannot be auto-detected.

src/main/CMakeLists.txt

@@ -216,7 +216,7 @@ main_sources(COMMON_SRC
     drivers/pitotmeter/pitotmeter_ms4525.c
     drivers/pitotmeter/pitotmeter_ms4525.h
     drivers/pitotmeter/pitotmeter_dlvr_l10d.c
-    drivers/pitotmeter/pitotmeter_dlvr_l10d.h	
+    drivers/pitotmeter/pitotmeter_dlvr_l10d.h
     drivers/pitotmeter/pitotmeter_msp.c
     drivers/pitotmeter/pitotmeter_msp.h
     drivers/pitotmeter/pitotmeter_virtual.c
@@ -460,6 +460,7 @@ main_sources(COMMON_SRC
     sensors/esc_sensor.h
     sensors/irlock.c
     sensors/irlock.h
+    sensors/sensors.c
     sensors/temperature.c
     sensors/temperature.h
 

src/main/fc/settings.yaml

@@ -456,6 +456,12 @@ groups:
         default_value: "BIQUAD"
         field: acc_soft_lpf_type
         table: filter_type
+      - name: acc_temp_correction
+        description: "Accelerometer temperature correction factor to compensate for acceleromter drift with changes in acceleromter temperature [cm/s2 per Degs C]. Internally limited to between -50 and 50. Typical setting for MPU6000 acceleromter is around 2.5. Setting to 51 initiates auto calibration which ends after 5 minutes or on first Arm."
+        field: acc_temp_correction
+        min: -50
+        max: 51
+        default_value: 0
       - name: acczero_x
         description: "Calculated value after '6 position avanced calibration'. See Wiki page."
         default_value: 0
@@ -631,6 +637,12 @@ groups:
         field: baro_calibration_tolerance
         min: 0
         max: 1000
+      - name: baro_temp_correction
+        description: "Baro temperature correction factor to compensate for Baro altitude drift with changes in Baro temperature [cm/Degs C]. Internally limited to between -50 and 50. Typical setting for BMP280 Baro is around 20. Setting to 51 initiates auto calibration which ends after 5 minutes or on first Arm."
+        field: baro_temp_correction
+        min: -50
+        max: 51
+        default_value: 0
 
   - name: PG_PITOTMETER_CONFIG
     type: pitotmeterConfig_t

src/main/navigation/navigation_pos_estimator.c

@@ -50,8 +50,9 @@
 #include "sensors/barometer.h"
 #include "sensors/compass.h"
 #include "sensors/gyro.h"
-#include "sensors/pitotmeter.h"
 #include "sensors/opflow.h"
+#include "sensors/pitotmeter.h"
+#include "sensors/sensors.h"
 
 navigationPosEstimator_t posEstimator;
 static float initialBaroAltitudeOffset = 0.0f;
@@ -280,12 +281,12 @@ void updatePositionEstimator_BaroTopic(timeUs_t currentTimeUs)
 {
     float newBaroAlt = baroCalculateAltitude();
 
-    /* If we are required - keep altitude at zero */
-    if (shouldResetReferenceAltitude()) {
-        initialBaroAltitudeOffset = newBaroAlt;
-    }
-
     if (sensors(SENSOR_BARO) && baroIsCalibrationComplete()) {
+        /* If required - keep altitude at zero */
+        if (shouldResetReferenceAltitude()) {
+            initialBaroAltitudeOffset = newBaroAlt;
+        }
+
         const timeUs_t baroDtUs = currentTimeUs - posEstimator.baro.lastUpdateTime;
 
         posEstimator.baro.alt = newBaroAlt - initialBaroAltitudeOffset;
@@ -431,6 +432,7 @@ static void updateIMUTopic(timeUs_t currentTimeUs)
             }
 #endif
             posEstimator.imu.accelNEU.z -= posEstimator.imu.calibratedGravityCMSS;
+            posEstimator.imu.accelNEU.z += applySensorTempCompensation(10 * gyroGetTemperature(), imuMeasuredAccelBF.z, SENSOR_INDEX_ACC);
         }
         else {
             posEstimator.imu.accelNEU.x = 0.0f;

src/main/sensors/acceleration.c

@@ -85,7 +85,7 @@ static EXTENDED_FASTRAM void *accNotchFilter[XYZ_AXIS_COUNT];
 static EXTENDED_FASTRAM float fAccZero[XYZ_AXIS_COUNT];
 static EXTENDED_FASTRAM float fAccGain[XYZ_AXIS_COUNT];
 
-PG_REGISTER_WITH_RESET_FN(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 5);
+PG_REGISTER_WITH_RESET_FN(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 6);
 
 void pgResetFn_accelerometerConfig(accelerometerConfig_t *instance)
 {
@@ -94,7 +94,8 @@ void pgResetFn_accelerometerConfig(accelerometerConfig_t *instance)
         .acc_lpf_hz = SETTING_ACC_LPF_HZ_DEFAULT,
         .acc_notch_hz = SETTING_ACC_NOTCH_HZ_DEFAULT,
         .acc_notch_cutoff = SETTING_ACC_NOTCH_CUTOFF_DEFAULT,
-        .acc_soft_lpf_type = SETTING_ACC_LPF_TYPE_DEFAULT
+        .acc_soft_lpf_type = SETTING_ACC_LPF_TYPE_DEFAULT,
+        .acc_temp_correction = SETTING_ACC_TEMP_CORRECTION_DEFAULT
     );
     RESET_CONFIG_2(flightDynamicsTrims_t, &instance->accZero,
         .raw[X] = SETTING_ACCZERO_X_DEFAULT,
@@ -557,8 +558,8 @@ void accUpdate(void)
 
     if (!ARMING_FLAG(SIMULATOR_MODE_SITL)) {
         performAcclerationCalibration();
-        applyAccelerationZero();  
-    } 
+        applyAccelerationZero();
+    }
 
     applySensorAlignment(accADC, accADC, acc.dev.accAlign);
     applyBoardAlignment(accADC);
@@ -637,7 +638,7 @@ bool accIsClipped(void)
 
 void accSetCalibrationValues(void)
 {
-    if (!ARMING_FLAG(SIMULATOR_MODE_SITL) && 
+    if (!ARMING_FLAG(SIMULATOR_MODE_SITL) &&
         ((accelerometerConfig()->accZero.raw[X] == 0) && (accelerometerConfig()->accZero.raw[Y] == 0) && (accelerometerConfig()->accZero.raw[Z] == 0) &&
         (accelerometerConfig()->accGain.raw[X] == 4096) && (accelerometerConfig()->accGain.raw[Y] == 4096) &&(accelerometerConfig()->accGain.raw[Z] == 4096))) {
         DISABLE_STATE(ACCELEROMETER_CALIBRATED);
@@ -648,12 +649,12 @@ void accSetCalibrationValues(void)
 }
 
 void accInitFilters(void)
-{   
+{
     accSoftLpfFilterApplyFn = nullFilterApply;
 
     if (acc.accTargetLooptime && accelerometerConfig()->acc_lpf_hz) {
 
-        switch (accelerometerConfig()->acc_soft_lpf_type) 
+        switch (accelerometerConfig()->acc_soft_lpf_type)
         {
         case FILTER_PT1:
             accSoftLpfFilterApplyFn = (filterApplyFnPtr)pt1FilterApply;

src/main/sensors/acceleration.h

@@ -74,7 +74,8 @@ typedef struct accelerometerConfig_s {
     flightDynamicsTrims_t accGain;          // Accelerometer gain to read exactly 1G
     uint8_t acc_notch_hz;                   // Accelerometer notch filter frequency
     uint8_t acc_notch_cutoff;               // Accelerometer notch filter cutoff frequency
-    uint8_t acc_soft_lpf_type;              // Accelerometer LPF type 
+    uint8_t acc_soft_lpf_type;              // Accelerometer LPF type
+    float acc_temp_correction;              // Accelerometer temperature compensation factor
 } accelerometerConfig_t;
 
 PG_DECLARE(accelerometerConfig_t, accelerometerConfig);

src/main/sensors/barometer.c

@@ -58,11 +58,12 @@ baro_t baro;                        // barometer access functions
 
 #ifdef USE_BARO
 
-PG_REGISTER_WITH_RESET_TEMPLATE(barometerConfig_t, barometerConfig, PG_BAROMETER_CONFIG, 4);
+PG_REGISTER_WITH_RESET_TEMPLATE(barometerConfig_t, barometerConfig, PG_BAROMETER_CONFIG, 5);
 
 PG_RESET_TEMPLATE(barometerConfig_t, barometerConfig,
     .baro_hardware = SETTING_BARO_HARDWARE_DEFAULT,
-    .baro_calibration_tolerance = SETTING_BARO_CAL_TOLERANCE_DEFAULT
+    .baro_calibration_tolerance = SETTING_BARO_CAL_TOLERANCE_DEFAULT,
+    .baro_temp_correction = SETTING_BARO_TEMP_CORRECTION_DEFAULT,
 );
 
 static zeroCalibrationScalar_t zeroCalibration;
@@ -325,6 +326,7 @@ int32_t baroCalculateAltitude(void)
     else {
         // calculates height from ground via baro readings
         baro.BaroAlt = pressureToAltitude(baro.baroPressure) - baroGroundAltitude;
+        baro.BaroAlt += applySensorTempCompensation(baro.baroTemperature, baro.BaroAlt, SENSOR_INDEX_BARO);
    }
 
     return baro.BaroAlt;
@@ -336,7 +338,7 @@ int32_t baroGetLatestAltitude(void)
 }
 
 int16_t baroGetTemperature(void)
-{   
+{
     return CENTIDEGREES_TO_DECIDEGREES(baro.baroTemperature);
 }
 

src/main/sensors/barometer.h

@@ -52,11 +52,11 @@ extern baro_t baro;
 typedef struct barometerConfig_s {
     uint8_t baro_hardware;                  // Barometer hardware to use
     uint16_t baro_calibration_tolerance;    // Baro calibration tolerance (cm at sea level)
+    float baro_temp_correction;             // Baro temperature correction value (cm/K)
 } barometerConfig_t;
 
 PG_DECLARE(barometerConfig_t, barometerConfig);
 
-
 bool baroInit(void);
 bool baroIsCalibrationComplete(void);
 void baroStartCalibration(void);

src/main/sensors/sensors.c

@@ -0,0 +1,107 @@
+/*
+ * This file is part of INAV.
+ *
+ * INAV 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.
+ *
+ * INAV 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 INAV.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "platform.h"
+
+#include "build/debug.h"
+
+#include "common/maths.h"
+
+#include "config/parameter_group.h"
+#include "config/parameter_group_ids.h"
+
+#include "drivers/time.h"
+
+#include "fc/runtime_config.h"
+
+#include "io/beeper.h"
+
+#include "sensors/acceleration.h"
+#include "sensors/barometer.h"
+#include "sensors/sensors.h"
+
+sensor_compensation_t sensor_comp_data[SENSOR_INDEX_COUNT];
+
+float applySensorTempCompensation(int16_t sensorTemp, float sensorMeasurement, sensorIndex_e sensorType)
+{
+    float setting = 0.0f;
+    if (sensorType == SENSOR_INDEX_ACC) {
+        setting = accelerometerConfig()->acc_temp_correction;
+    }
+#ifdef USE_BARO
+    else if (sensorType == SENSOR_INDEX_BARO) {
+        setting = barometerConfig()->baro_temp_correction;
+    }
+#endif
+    if (!setting) {
+        return 0.0f;
+    }
+
+    if (sensor_comp_data[sensorType].calibrationState == SENSOR_TEMP_CAL_COMPLETE) {
+        return sensor_comp_data[sensorType].correctionFactor * CENTIDEGREES_TO_DEGREES(sensor_comp_data[sensorType].referenceTemp - sensorTemp);
+    }
+
+    static timeMs_t startTimeMs = 0;
+
+    if (!ARMING_FLAG(WAS_EVER_ARMED)) {
+        if (sensor_comp_data[sensorType].calibrationState == SENSOR_TEMP_CAL_INITIALISE) {
+            sensor_comp_data[sensorType].referenceTemp = sensorTemp;
+            sensor_comp_data[sensorType].calibrationState = SENSOR_TEMP_CAL_IN_PROGRESS;
+        }
+
+        if (setting == 51.0f) {   // initiate auto calibration
+            if (sensor_comp_data[sensorType].referenceTemp == sensorTemp) {
+                sensor_comp_data[sensorType].referenceMeasurement = sensorMeasurement;
+                sensor_comp_data[sensorType].lastTemp = sensorTemp;
+                startTimeMs = millis();
+            }
+
+            float referenceDeltaTemp = ABS(sensorTemp - sensor_comp_data[sensorType].referenceTemp);    // centidegrees
+            if (referenceDeltaTemp > 300 && referenceDeltaTemp > ABS(sensor_comp_data[sensorType].lastTemp - sensor_comp_data[sensorType].referenceTemp)) {
+                /* Min 3 deg reference temperature difference required for valid calibration.
+                 * Correction adjusted only if temperature difference to reference temperature increasing
+                 * Calibration assumes a simple linear relationship */
+                sensor_comp_data[sensorType].lastTemp = sensorTemp;
+                sensor_comp_data[sensorType].correctionFactor = 0.9f * sensor_comp_data[sensorType].correctionFactor + 0.1f * (sensorMeasurement - sensor_comp_data[sensorType].referenceMeasurement) / CENTIDEGREES_TO_DEGREES(sensor_comp_data[sensorType].lastTemp - sensor_comp_data[sensorType].referenceTemp);
+                sensor_comp_data[sensorType].correctionFactor = constrainf(sensor_comp_data[sensorType].correctionFactor, -50.0f, 50.0f);
+            }
+        } else {
+            sensor_comp_data[sensorType].correctionFactor = setting;
+            sensor_comp_data[sensorType].calibrationState = SENSOR_TEMP_CAL_COMPLETE;
+        }
+    }
+
+    // Calibration ends on first Arm or after 5 min timeout
+    if (sensor_comp_data[sensorType].calibrationState == SENSOR_TEMP_CAL_IN_PROGRESS && (ARMING_FLAG(WAS_EVER_ARMED) || millis() > startTimeMs + 300000)) {
+        if (!ARMING_FLAG(WAS_EVER_ARMED)) {
+            beeper(sensor_comp_data[sensorType].correctionFactor ? BEEPER_ACTION_SUCCESS : BEEPER_ACTION_FAIL);
+        }
+
+        if (sensorType == SENSOR_INDEX_ACC) {
+            accelerometerConfigMutable()->acc_temp_correction = sensor_comp_data[sensorType].correctionFactor;
+        }
+#ifdef USE_BARO
+        else if (sensorType == SENSOR_INDEX_BARO) {
+            barometerConfigMutable()->baro_temp_correction = sensor_comp_data[sensorType].correctionFactor;
+        }
+#endif
+        sensor_comp_data[sensorType].calibrationState = SENSOR_TEMP_CAL_COMPLETE;
+        startTimeMs = 0;
+    }
+
+    return 0.0f;
+}

src/main/sensors/sensors.h

@@ -59,5 +59,20 @@ typedef enum {
     SENSOR_TEMP = 1 << 9
 } sensors_e;
 
+typedef enum {
+    SENSOR_TEMP_CAL_INITIALISE,
+    SENSOR_TEMP_CAL_IN_PROGRESS,
+    SENSOR_TEMP_CAL_COMPLETE,
+} sensorTempCalState_e;
+
+typedef struct sensor_compensation_s {
+    float correctionFactor;
+    float referenceMeasurement;
+    int16_t referenceTemp;
+    int16_t lastTemp;
+    sensorTempCalState_e calibrationState;
+} sensor_compensation_t;
+
+float applySensorTempCompensation(int16_t sensorTemp, float sensorMeasurement, sensorIndex_e sensorType);
 extern uint8_t requestedSensors[SENSOR_INDEX_COUNT];
 extern uint8_t detectedSensors[SENSOR_INDEX_COUNT];