Skip to content

Commit

Permalink
CR134
Browse files Browse the repository at this point in the history
  • Loading branch information
breadoven committed Sep 23, 2024
1 parent f356f7b commit 80c0e7a
Show file tree
Hide file tree
Showing 6 changed files with 52 additions and 24 deletions.
20 changes: 20 additions & 0 deletions docs/Settings.md
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down Expand Up @@ -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)
Expand Down
6 changes: 3 additions & 3 deletions src/main/fc/settings.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -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"]
Expand Down
1 change: 0 additions & 1 deletion src/main/navigation/navigation_pos_estimator.c
Original file line number Diff line number Diff line change
Expand Up @@ -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()) {
Expand Down
41 changes: 25 additions & 16 deletions src/main/sensors/barometer.c
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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;
Expand Down Expand Up @@ -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;
Expand All @@ -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;
Expand All @@ -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)
{
Expand All @@ -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;
Expand Down
6 changes: 3 additions & 3 deletions src/main/sensors/barometer.h
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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);
Expand Down
2 changes: 1 addition & 1 deletion src/main/target/common.h
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down

0 comments on commit 80c0e7a

Please sign in to comment.