From 502f4f950b5e04e5d3785043165dfa11cf4b93f7 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Sun, 6 Oct 2024 13:49:41 +0100 Subject: [PATCH] CR134 --- src/main/fc/settings.yaml | 2 +- .../navigation/navigation_pos_estimator.c | 2 +- src/main/sensors/sensors.c | 38 +++++++++---------- 3 files changed, 20 insertions(+), 22 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index e4cc970309c..58f8ac7ba9d 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -461,7 +461,7 @@ groups: table: filter_type # CR134 - name: acc_temp_correction - description: "Accelerometer temperature correction factor to compensate for acceleromter altitude drift with changes in acceleromter temperature [cm/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." + 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 diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 094785c2fd1..59f99883071 100644 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -285,7 +285,7 @@ void updatePositionEstimator_BaroTopic(timeUs_t currentTimeUs) float newBaroAlt = baroCalculateAltitude(); if (sensors(SENSOR_BARO) && baroIsCalibrationComplete()) { - /* If we are required - keep altitude at zero */ + /* If we are required - keep altitude at zero */ // CR134 if (shouldResetReferenceAltitude()) { initialBaroAltitudeOffset = newBaroAlt; } diff --git a/src/main/sensors/sensors.c b/src/main/sensors/sensors.c index a99209ddc03..7136844e9de 100644 --- a/src/main/sensors/sensors.c +++ b/src/main/sensors/sensors.c @@ -30,9 +30,6 @@ #include "io/beeper.h" -#include "navigation/navigation.h" -#include "navigation/navigation_pos_estimator_private.h" - #include "sensors/acceleration.h" #include "sensors/barometer.h" #include "sensors/sensors.h" @@ -43,11 +40,9 @@ float applySensorTempCompensation(int16_t sensorTemp, float sensorMeasurement, s { float setting = 0.0f; if (sensorType == SENSOR_INDEX_ACC) { - // setting = positionEstimationConfig()->acc_temp_correction; setting = accelerometerConfig()->acc_temp_correction; } else if (sensorType == SENSOR_INDEX_BARO) { setting = barometerConfig()->baro_temp_correction; - // setting = positionEstimationConfig()->baro_temp_correction; } if (!setting) { @@ -56,31 +51,34 @@ float applySensorTempCompensation(int16_t sensorTemp, float sensorMeasurement, s DEBUG_SET(DEBUG_ALWAYS, 1, sensorTemp); DEBUG_SET(DEBUG_ALWAYS, 0, sensor_comp_data[sensorType].correctionFactor * 100); DEBUG_SET(DEBUG_ALWAYS, 5, sensorMeasurement); - static timeMs_t startTimeMs = 0; if (sensor_comp_data[sensorType].calibrationState == SENSOR_TEMP_CAL_COMPLETE) { - startTimeMs = 0; float tempCal = sensor_comp_data[sensorType].correctionFactor * CENTIDEGREES_TO_DEGREES(sensor_comp_data[sensorType].referenceTemp - sensorTemp); DEBUG_SET(DEBUG_ALWAYS, 2, tempCal); return tempCal; - // return correctionFactor * CENTIDEGREES_TO_DEGREES(referenceTemp - baro.baroTemperature); + // 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)) { - static float referenceMeasurement = 0.0f; - static int16_t lastTemp = 0.0f; - if (sensor_comp_data[sensorType].calibrationState == SENSOR_TEMP_CAL_INITIALISE) { - sensor_comp_data[sensorType].referenceTemp = lastTemp = sensorTemp; - referenceMeasurement = sensorMeasurement; + sensor_comp_data[sensorType].referenceTemp = sensorTemp; sensor_comp_data[sensorType].calibrationState = SENSOR_TEMP_CAL_IN_PROGRESS; - startTimeMs = millis(); } if (setting == 51.0f) { + 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); if (referenceDeltaTemp > 300 && referenceDeltaTemp > ABS(lastTemp - sensor_comp_data[sensorType].referenceTemp)) { // centidegrees lastTemp = sensorTemp; - sensor_comp_data[sensorType].correctionFactor = 0.8f * sensor_comp_data[sensorType].correctionFactor + 0.2f * (sensorMeasurement - referenceMeasurement) / CENTIDEGREES_TO_DEGREES(lastTemp - sensor_comp_data[sensorType].referenceTemp); + 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 { @@ -90,17 +88,17 @@ float applySensorTempCompensation(int16_t sensorTemp, float sensorMeasurement, s } 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) { - // positionEstimationConfigMutable()->acc_temp_correction = sensor_comp_data[sensorType].correctionFactor; accelerometerConfigMutable()->acc_temp_correction = sensor_comp_data[sensorType].correctionFactor; } else if (sensorType == SENSOR_INDEX_BARO) { - // positionEstimationConfigMutable()->baro_temp_correction = sensor_comp_data[sensorType].correctionFactor; barometerConfigMutable()->baro_temp_correction = sensor_comp_data[sensorType].correctionFactor; } sensor_comp_data[sensorType].calibrationState = SENSOR_TEMP_CAL_COMPLETE; - if (!ARMING_FLAG(WAS_EVER_ARMED)) { - beeper(sensor_comp_data[sensorType].correctionFactor ? BEEPER_ACTION_SUCCESS : BEEPER_ACTION_FAIL); - } + startTimeMs = 0; } return 0.0f;