From 80c0e7aa8a9e83cf167022e789ef5c37d896f88a Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Mon, 23 Sep 2024 23:39:20 +0100 Subject: [PATCH] CR134 --- docs/Settings.md | 20 +++++++++ src/main/fc/settings.yaml | 6 +-- .../navigation/navigation_pos_estimator.c | 1 - src/main/sensors/barometer.c | 41 +++++++++++-------- src/main/sensors/barometer.h | 6 +-- src/main/target/common.h | 2 +- 6 files changed, 52 insertions(+), 24 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index fdfc7590082..d6a240c5a6b 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -452,6 +452,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. @@ -3512,6 +3522,16 @@ D gain of altitude PID controller (Fixedwing) --- +### nav_fw_pos_z_ff + +FF gain of altitude PID controller (Fixedwing) + +| Default | Min | Max | +| --- | --- | --- | +| 10 | 0 | 255 | + +--- + ### nav_fw_pos_z_i I gain of altitude PID controller (Fixedwing) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 89b546ef735..150010e9e95 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -634,14 +634,14 @@ groups: field: baro_calibration_tolerance min: 0 max: 1000 - # CR131 - - name: inav_baro_temp_correction + # CR134 + - 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 - + # CR134 - name: PG_PITOTMETER_CONFIG type: pitotmeterConfig_t headers: ["sensors/pitotmeter.h"] diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index ad5ec20cacd..004dc6aa62d 100644 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -282,7 +282,6 @@ void onNewGPSData(void) void updatePositionEstimator_BaroTopic(timeUs_t currentTimeUs) { float newBaroAlt = baroCalculateAltitude(); - DEBUG_SET(DEBUG_ALWAYS, 4, newBaroAlt); /* If we are required - keep altitude at zero */ if (shouldResetReferenceAltitude()) { diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c index 53b5efd7a1e..0d72e5c0b34 100644 --- a/src/main/sensors/barometer.c +++ b/src/main/sensors/barometer.c @@ -50,6 +50,8 @@ #include "sensors/barometer.h" #include "sensors/sensors.h" +#include "io/beeper.h" // CR134 + #ifdef USE_HARDWARE_REVISION_DETECTION #include "hardware_revision.h" #endif @@ -63,7 +65,7 @@ PG_REGISTER_WITH_RESET_TEMPLATE(barometerConfig_t, barometerConfig, PG_BAROMETER PG_RESET_TEMPLATE(barometerConfig_t, barometerConfig, .baro_hardware = SETTING_BARO_HARDWARE_DEFAULT, .baro_calibration_tolerance = SETTING_BARO_CAL_TOLERANCE_DEFAULT, - .baro_temp_correction = SETTING_INAV_BARO_TEMP_CORRECTION_DEFAULT, // CR131 + .baro_temp_correction = SETTING_BARO_TEMP_CORRECTION_DEFAULT, // CR134 ); static zeroCalibrationScalar_t zeroCalibration; @@ -309,18 +311,28 @@ void baroStartCalibration(void) const float acceptedPressureVariance = (101325.0f - altitudeToPressure(barometerConfig()->baro_calibration_tolerance)); // max 30cm deviation during calibration (at sea level) zeroCalibrationStartS(&zeroCalibration, CALIBRATING_BARO_TIME_MS, acceptedPressureVariance, false); } - -// CR131 +// CR134 float processBaroTempCorrection(void) { + float setting = barometerConfig()->baro_temp_correction; +DEBUG_SET(DEBUG_ALWAYS, 1, baro.baroTemperature); + if (!setting) { + 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; - float setting = barometerConfig()->baro_temp_correction; DEBUG_SET(DEBUG_ALWAYS, 0, correctionFactor * 100); - DEBUG_SET(DEBUG_ALWAYS, 3, baro.baroTemperature); + if (calibrationState == BARO_TEMP_CAL_COMPLETE) { + float tempCal = correctionFactor * CENTIDEGREES_TO_DEGREES(baroTemp1 - baro.baroTemperature); + DEBUG_SET(DEBUG_ALWAYS, 2, tempCal); + return tempCal; + // 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; @@ -335,9 +347,10 @@ float processBaroTempCorrection(void) if (setting == 51.0f) { float referenceDeltaTemp = ABS(baro.baroTemperature - baroTemp1); - if (referenceDeltaTemp > 100 && referenceDeltaTemp > ABS(baroTemp2 - baroTemp1)) { // centidegrees + if (referenceDeltaTemp > 300 && referenceDeltaTemp > ABS(baroTemp2 - baroTemp1)) { // centidegrees baroTemp2 = baro.baroTemperature; - correctionFactor = 0.8f * correctionFactor + 0.2f * (newBaroAlt - baroAlt1) / CENTIDEGREES_TO_DEGREES(baroTemp2 - baroTemp1)); + correctionFactor = 0.8f * correctionFactor + 0.2f * (newBaroAlt - baroAlt1) / CENTIDEGREES_TO_DEGREES(baroTemp2 - baroTemp1); + correctionFactor = constrainf(correctionFactor, -50.0f, 50.0f); } } else { correctionFactor = setting; @@ -348,18 +361,14 @@ float processBaroTempCorrection(void) 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 (calibrationState == BARO_TEMP_CAL_COMPLETE) { - float tempCal = constrainf(correctionFactor, -50.0f, 50.0f) * CENTIDEGREES_TO_DEGREES(baroTemp1 - baro.baroTemperature); - DEBUG_SET(DEBUG_ALWAYS, 2, tempCal); - return tempCal; - // return constrainf(correctionFactor, -50.0f, 50.0f) * CENTIDEGREES_TO_DEGREES(baroTemp1 - baro.baroTemperature); + if (!ARMING_FLAG(WAS_EVER_ARMED)) { + beeper(correctionFactor ? BEEPER_ACTION_SUCCESS : BEEPER_ACTION_FAIL); + } } return 0.0f; } -// CR131 +// CR134 int32_t baroCalculateAltitude(void) { @@ -376,7 +385,7 @@ int32_t baroCalculateAltitude(void) } else { // calculates height from ground via baro readings - baro.BaroAlt = pressureToAltitude(baro.baroPressure) - baroGroundAltitude + processBaroTempCorrection(); // CR131 + baro.BaroAlt = pressureToAltitude(baro.baroPressure) - baroGroundAltitude + processBaroTempCorrection(); // CR134 } return baro.BaroAlt; diff --git a/src/main/sensors/barometer.h b/src/main/sensors/barometer.h index 0701ca0575d..0aaca14279c 100644 --- a/src/main/sensors/barometer.h +++ b/src/main/sensors/barometer.h @@ -37,13 +37,13 @@ typedef enum { BARO_FAKE = 12, BARO_MAX = BARO_FAKE } baroSensor_e; -// CR131 +// CR134 typedef enum { BARO_TEMP_CAL_INITIALISE, BARO_TEMP_CAL_IN_PROGRESS, BARO_TEMP_CAL_COMPLETE, } baroTempCalState_e; -// CR131 +// CR134 typedef struct baro_s { baroDev_t dev; int32_t BaroAlt; @@ -58,7 +58,7 @@ 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; // CR131 + float baro_temp_correction; // CR134 } barometerConfig_t; PG_DECLARE(barometerConfig_t, barometerConfig); diff --git a/src/main/target/common.h b/src/main/target/common.h index 9a10a8d3ecf..7cf4ba540bb 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -177,7 +177,7 @@ // Wind estimator #define USE_WIND_ESTIMATOR -#define USE_SIMULATOR +// #define USE_SIMULATOR #define USE_PITOT_VIRTUAL #define USE_FAKE_BATT_SENSOR