Add accelerometer

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.

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

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

@@ -60,7 +60,7 @@ 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,
@@ -312,62 +312,6 @@ void baroStartCalibration(void)
     zeroCalibrationStartS(&zeroCalibration, CALIBRATING_BARO_TIME_MS, acceptedPressureVariance, false);
 }
 
-float processBaroTempCorrection(void)
-{
-    float setting = barometerConfig()->baro_temp_correction;
-
-    if (setting == 0.0f) {
-        return 0.0f;
-    }
-
-    static float correctionFactor = 0.0f;
-    static baroTempCalState_e calibrationState = BARO_TEMP_CAL_INITIALISE;
-    static int16_t baroTemp1 = 0.0f;
-    static timeMs_t startTimeMs = 0;
-
-    if (calibrationState == BARO_TEMP_CAL_COMPLETE) {
-        return correctionFactor * CENTIDEGREES_TO_DEGREES(baroTemp1 - baro.baroTemperature);
-    }
-
-    if (!ARMING_FLAG(WAS_EVER_ARMED)) {
-        static float baroAlt1 = 0.0f;
-        static int16_t baroTemp2 = 0.0f;
-        float newBaroAlt = pressureToAltitude(baro.baroPressure) - baroGroundAltitude;
-
-        if (calibrationState == BARO_TEMP_CAL_INITIALISE) {  // set initial correction reference temps/pressures
-            baroTemp1 = baroTemp2 = baro.baroTemperature;
-            baroAlt1 = newBaroAlt;
-            calibrationState = BARO_TEMP_CAL_IN_PROGRESS;
-            startTimeMs = millis();
-        }
-
-        if (setting == 51.0f) {  // Auto calibration triggered with setting = 51
-            /* Min 3 deg reference temperature difference required for valid calibration.
-             * Correction adjusted only if temperature difference to reference temperature increasing */
-            float referenceDeltaTemp = ABS(baro.baroTemperature - baroTemp1);
-            if (referenceDeltaTemp > 300 && referenceDeltaTemp > ABS(baroTemp2 - baroTemp1)) {
-                baroTemp2 = baro.baroTemperature;
-                correctionFactor = 0.8f * correctionFactor + 0.2f * (newBaroAlt - baroAlt1) / CENTIDEGREES_TO_DEGREES(baroTemp2 - baroTemp1);
-                correctionFactor = constrainf(correctionFactor, -50.0f, 50.0f);
-            }
-        } else {
-            correctionFactor = setting;
-            calibrationState = BARO_TEMP_CAL_COMPLETE;
-        }
-    }
-
-    // Calibration ends on first Arm or after 5 min timeout
-    if (calibrationState == BARO_TEMP_CAL_IN_PROGRESS && (ARMING_FLAG(WAS_EVER_ARMED) || millis() > startTimeMs + 300000)) {
-        barometerConfigMutable()->baro_temp_correction = correctionFactor;
-        calibrationState = BARO_TEMP_CAL_COMPLETE;
-        if (!ARMING_FLAG(WAS_EVER_ARMED)) {
-            beeper(correctionFactor ? BEEPER_ACTION_SUCCESS : BEEPER_ACTION_FAIL);
-        }
-    }
-
-    return 0.0f;
-}
-
 int32_t baroCalculateAltitude(void)
 {
     if (!baroIsCalibrationComplete()) {
@@ -383,7 +327,8 @@ int32_t baroCalculateAltitude(void)
     }
     else {
         // calculates height from ground via baro readings
-        baro.BaroAlt = pressureToAltitude(baro.baroPressure) - baroGroundAltitude + processBaroTempCorrection();
+        baro.BaroAlt = pressureToAltitude(baro.baroPressure) - baroGroundAltitude;
+        baro.BaroAlt += applySensorTempCompensation(baro.baroTemperature, baro.BaroAlt, SENSOR_INDEX_BARO);
    }
 
     return baro.BaroAlt;

src/main/sensors/barometer.h

@@ -38,12 +38,6 @@ typedef enum {
     BARO_MAX    = BARO_FAKE
 } baroSensor_e;
 
-typedef enum {
-    BARO_TEMP_CAL_INITIALISE,
-    BARO_TEMP_CAL_IN_PROGRESS,
-    BARO_TEMP_CAL_COMPLETE,
-} baroTempCalState_e;
-
 typedef struct baro_s {
     baroDev_t dev;
     int32_t BaroAlt;
@@ -63,7 +57,6 @@ typedef struct barometerConfig_s {
 
 PG_DECLARE(barometerConfig_t, barometerConfig);
 
-
 bool baroInit(void);
 bool baroIsCalibrationComplete(void);
 void baroStartCalibration(void);

src/main/sensors/sensors.c

@@ -0,0 +1,105 @@
+/*
+ * 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;
+    } else if (sensorType == SENSOR_INDEX_BARO) {
+        setting = barometerConfig()->baro_temp_correction;
+    }
+
+    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
+            static float referenceMeasurement = 0.0f;
+            static int16_t lastTemp = 0.0f;
+
+            if (sensor_comp_data[sensorType].referenceTemp == sensorTemp) {
+                referenceMeasurement = sensorMeasurement;
+                lastTemp = sensorTemp;
+                startTimeMs = millis();
+            }
+
+            float referenceDeltaTemp = ABS(sensorTemp - sensor_comp_data[sensorType].referenceTemp);    // centidegrees
+            if (referenceDeltaTemp > 300 && referenceDeltaTemp > ABS(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 */
+                lastTemp = sensorTemp;
+                sensor_comp_data[sensorType].correctionFactor = 0.9f * sensor_comp_data[sensorType].correctionFactor + 0.1f * (sensorMeasurement - referenceMeasurement) / CENTIDEGREES_TO_DEGREES(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;
+        } else if (sensorType == SENSOR_INDEX_BARO) {
+            barometerConfigMutable()->baro_temp_correction = sensor_comp_data[sensorType].correctionFactor;
+        }
+        sensor_comp_data[sensorType].calibrationState = SENSOR_TEMP_CAL_COMPLETE;
+        startTimeMs = 0;
+    }
+
+    return 0.0f;
+}

src/main/sensors/sensors.h

@@ -59,5 +59,18 @@ 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;
+    int16_t referenceTemp;
+    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];