From fdff13163fcda92e9eedc04ce5acdf1d23b2675e Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Sun, 6 Oct 2024 13:45:25 +0100 Subject: [PATCH] Add accelerometer --- docs/Settings.md | 10 ++ src/main/CMakeLists.txt | 3 +- src/main/fc/settings.yaml | 6 + .../navigation/navigation_pos_estimator.c | 14 ++- src/main/sensors/acceleration.c | 15 +-- src/main/sensors/acceleration.h | 3 +- src/main/sensors/barometer.c | 61 +--------- src/main/sensors/barometer.h | 7 -- src/main/sensors/sensors.c | 105 ++++++++++++++++++ src/main/sensors/sensors.h | 13 +++ 10 files changed, 157 insertions(+), 80 deletions(-) create mode 100644 src/main/sensors/sensors.c diff --git a/docs/Settings.md b/docs/Settings.md index a5d4bfaeaa2..8c9c534bde7 100644 --- a/docs/Settings.md +++ b/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. diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index b6ff0e10985..d31a3680ff1 100755 --- a/src/main/CMakeLists.txt +++ b/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 diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index c171c5a3681..7c061c4c51f 100644 --- a/src/main/fc/settings.yaml +++ b/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 diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index d20087f9e32..6bf4b05078f 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/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; diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index 4e46f178843..16d10624c69 100644 --- a/src/main/sensors/acceleration.c +++ b/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; diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index a25063ec4fd..2e2bfa461e0 100644 --- a/src/main/sensors/acceleration.h +++ b/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); diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c index 9d0ea677761..09eb594050b 100644 --- a/src/main/sensors/barometer.c +++ b/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; diff --git a/src/main/sensors/barometer.h b/src/main/sensors/barometer.h index 4a0e35a5210..0e8e918d70f 100644 --- a/src/main/sensors/barometer.h +++ b/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); diff --git a/src/main/sensors/sensors.c b/src/main/sensors/sensors.c new file mode 100644 index 00000000000..fe7d0f7be31 --- /dev/null +++ b/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 . + */ + +#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; +} \ No newline at end of file diff --git a/src/main/sensors/sensors.h b/src/main/sensors/sensors.h index 2b706f7dfc6..aca0bfb53c9 100644 --- a/src/main/sensors/sensors.h +++ b/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];