Merge pull request #10382 from breadoven/abo_baro_temp_compensation

Accelerometer and Barometer temperature compensation

docs/Sensors.md

@@ -0,0 +1,17 @@
+# Sensors
+
+## Temperature Correction
+Temperature related drift for the Nav Accelerometer Z axis and Barometer can be corrected using settings `acc_temp_correction` and `baro_temp_correction`. The settings are temperature correction factor in cms-2/K and cm/K respectively, limited to -50 to +50, and are based on a linear correction characteristic. It is possible to perform an auto calibration of the required correction value using a setting of 51 with the auto calibration ending after a 5 minute timeout or on first arm. It's also possible to simply enter a value and see how the sensor drifts as the FC warms up, e.g. a BMP280 barometer requires a value around 20 cm/K for the `baro_temp_correction` setting.
+
+### Barometer Calibration
+Best calibrated by checking barometer altitude in the Configurator Sensor tab, powered just from USB or the main battery. First check Barometer altitude drift with `baro_temp_correction` set to 0 starting with a cold FC. Then change the setting to 51, power off and allow the FC to cool then power on again and recheck the barometer altitude in the Configurator Sensors tab. The barometer altitude will drift as before until 5 mins after bootup at which point, if the calibration has worked, the barometer altitude should fall close to 0. This will be accompanied by a "success" beep (may need battery power for this rather than just USB). The calibrated setting can be saved by switching to the CLI and hitting "Save Settings" (or use the Save stick command or CMS). Power off and allow the FC to cool down then recheck barometer altitude again, it should show much reduced altitude drift as it warms up.
+
+### Nav Acceleromter Z Axis Calibration
+The Nav accelerometer Z axis is calibrated in a similar way to the Barometer except it isn't so easy to check while calibrating given the lack of direct feedback. The acc Z value in the Configurator Sensors tab isn't the corrected value so can't be used. Instead set `debug_mode` to `ALTITUDE` and check the behaviour of `Debug 3` in the Configurator Sensors Tab for Debug output. It will drift during calibration then show a sudden change when calibration is finished. After saving and rebooting check `Debug 3` in the Configurator Sensors tab, it should show much less drift from zero if the calibration was successful. Successful calibration can also be checked by making sure `acc_temp_correction` shows a value other than 0 or 51.
+
+
+
+
+
+
+

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

@@ -467,6 +467,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
@@ -642,6 +648,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;
@@ -283,12 +284,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;
@@ -434,6 +435,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 {      // If calibration is incomplete - report zero acceleration
             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];