CR134

breadoven · Oct 6, 2024 · 502f4f9 · 502f4f9
1 parent 9141dc5
commit 502f4f9
Show file tree

Hide file tree

Showing 3 changed files with 20 additions and 22 deletions.
diff --git 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
@@ -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
@@ -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;