From b2241b32c3d1677842650b25476b5a2fdb262bcc Mon Sep 17 00:00:00 2001 From: ctzsnooze Date: Tue, 2 Aug 2022 15:21:38 +1000 Subject: [PATCH] Refactor Baro to floats, filter at position rate convert pressure to altitude early remove median filter PT2 filtering upsampled to altitude function in position.c - thanks KarateBrot baro task synced to position task - thanks Steve PT2 implementation - thanks KarateBrot ground altitude from filtered altitude baro cali by average of calibration samples over cal period adjust vario and smoothing defaults don't say haveBaroAlt until cal is complete reduce PIDs since Baro is faster add baro smoothing values to blackbox header Co-Authored-By: Jan Post Co-Authored-By: Steve Evans --- src/main/blackbox/blackbox.c | 2 + src/main/cli/settings.c | 5 +- src/main/cms/cms_menu_firmware.c | 2 +- src/main/fc/core.c | 2 +- src/main/fc/parameter_names.h | 2 + src/main/fc/tasks.c | 24 ++- src/main/flight/gps_rescue.c | 6 +- src/main/flight/position.c | 76 ++++----- src/main/flight/position.h | 3 +- src/main/sensors/barometer.c | 168 +++++++++----------- src/main/sensors/barometer.h | 21 +-- src/main/sensors/initialisation.c | 2 +- src/test/unit/arming_prevention_unittest.cc | 2 +- src/test/unit/flight_imu_unittest.cc | 102 ++++++------ src/test/unit/telemetry_ibus_unittest.cc | 2 +- src/test/unit/vtx_unittest.cc | 2 +- 16 files changed, 209 insertions(+), 212 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 91689e1119..6afe5a5c39 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1447,6 +1447,8 @@ static bool blackboxWriteSysinfo(void) #endif #ifdef USE_BARO BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_BARO_HARDWARE, "%d", barometerConfig()->baro_hardware); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_BARO_NOISE_LPF, "%d", barometerConfig()->baro_noise_lpf); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_BARO_VARIO_LPF, "%d", barometerConfig()->baro_vario_lpf); #endif #ifdef USE_MAG BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_MAG_HARDWARE, "%d", compassConfig()->mag_hardware); diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index d04b1d2e4e..ec8ec58fc3 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -732,9 +732,8 @@ const clivalue_t valueTable[] = { { "baro_i2c_device", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, 5 }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_i2c_device) }, { "baro_i2c_address", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, I2C_ADDR7_MAX }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_i2c_address) }, { PARAM_NAME_BARO_HARDWARE, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_BARO_HARDWARE }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_hardware) }, - { "baro_tab_size", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 1, BARO_SAMPLE_COUNT_MAX }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_sample_count) }, - { "baro_noise_lpf", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_noise_lpf) }, - { "baro_cf_vel", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_cf_vel) }, + { "baro_noise_lpf", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 10, 1000 }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_noise_lpf) }, + { "baro_vario_lpf", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 10, 1000 }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_vario_lpf) }, #endif // PG_RX_CONFIG diff --git a/src/main/cms/cms_menu_firmware.c b/src/main/cms/cms_menu_firmware.c index 34f0075e3f..776fbe2ef6 100644 --- a/src/main/cms/cms_menu_firmware.c +++ b/src/main/cms/cms_menu_firmware.c @@ -81,7 +81,7 @@ static const void *cmsx_CalibrationOnDisplayUpdate(displayPort_t *pDisp, const O tfp_sprintf(accCalibrationStatus, sensors(SENSOR_ACC) ? accIsCalibrationComplete() ? accHasBeenCalibrated() ? CALIBRATION_STATUS_OK : CALIBRATION_STATUS_NOK : CALIBRATION_STATUS_WAIT: CALIBRATION_STATUS_OFF); #endif #if defined(USE_BARO) - tfp_sprintf(baroCalibrationStatus, sensors(SENSOR_BARO) ? baroIsCalibrationComplete() ? CALIBRATION_STATUS_OK : CALIBRATION_STATUS_WAIT: CALIBRATION_STATUS_OFF); + tfp_sprintf(baroCalibrationStatus, sensors(SENSOR_BARO) ? baroIsCalibrated() ? CALIBRATION_STATUS_OK : CALIBRATION_STATUS_WAIT: CALIBRATION_STATUS_OFF); #endif return NULL; diff --git a/src/main/fc/core.c b/src/main/fc/core.c index 0251b1048e..b59fdbb533 100644 --- a/src/main/fc/core.c +++ b/src/main/fc/core.c @@ -186,7 +186,7 @@ static bool isCalibrating(void) || (sensors(SENSOR_ACC) && !accIsCalibrationComplete()) #endif #ifdef USE_BARO - || (sensors(SENSOR_BARO) && !baroIsCalibrationComplete()) + || (sensors(SENSOR_BARO) && !baroIsCalibrated()) #endif #ifdef USE_MAG || (sensors(SENSOR_MAG) && !compassIsCalibrationComplete()) diff --git a/src/main/fc/parameter_names.h b/src/main/fc/parameter_names.h index b90ab960b3..77e486c504 100644 --- a/src/main/fc/parameter_names.h +++ b/src/main/fc/parameter_names.h @@ -34,6 +34,8 @@ #define PARAM_NAME_ACC_LPF_HZ "acc_lpf_hz" #define PARAM_NAME_MAG_HARDWARE "mag_hardware" #define PARAM_NAME_BARO_HARDWARE "baro_hardware" +#define PARAM_NAME_BARO_NOISE_LPF "baro_noise_lpf" +#define PARAM_NAME_BARO_VARIO_LPF "baro_vario_lpf" #define PARAM_NAME_RC_SMOOTHING "rc_smoothing" #define PARAM_NAME_RC_SMOOTHING_AUTO_FACTOR "rc_smoothing_auto_factor" #define PARAM_NAME_RC_SMOOTHING_AUTO_FACTOR_THROTTLE "rc_smoothing_auto_factor_throttle" diff --git a/src/main/fc/tasks.c b/src/main/fc/tasks.c index de630702b4..0bb2fed77a 100644 --- a/src/main/fc/tasks.c +++ b/src/main/fc/tasks.c @@ -294,9 +294,25 @@ void taskUpdateRangefinder(timeUs_t currentTimeUs) #endif #if defined(USE_BARO) || defined(USE_GPS) -static void taskCalculateAltitude(timeUs_t currentTimeUs) +static bool taskAltitudeCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs) { - calculateEstimatedAltitude(currentTimeUs); + UNUSED(currentTimeUs); + +#if defined(USE_BARO) + if (isBaroSampleReady()) { + return true; + } +#endif + + if (currentDeltaTimeUs > getTask(TASK_ALTITUDE)->attribute->desiredPeriodUs) { + return true; + } + + return false; + } + static void taskCalculateAltitude() +{ + calculateEstimatedAltitude(); } #endif // USE_BARO || USE_GPS @@ -374,11 +390,11 @@ task_attribute_t task_attributes[TASK_COUNT] = { #endif #ifdef USE_BARO - [TASK_BARO] = DEFINE_TASK("BARO", NULL, NULL, taskUpdateBaro, TASK_PERIOD_HZ(20), TASK_PRIORITY_LOW), + [TASK_BARO] = DEFINE_TASK("BARO", NULL, NULL, taskUpdateBaro, TASK_PERIOD_HZ(TASK_BARO_RATE_HZ), TASK_PRIORITY_LOW), #endif #if defined(USE_BARO) || defined(USE_GPS) - [TASK_ALTITUDE] = DEFINE_TASK("ALTITUDE", NULL, NULL, taskCalculateAltitude, TASK_PERIOD_HZ(40), TASK_PRIORITY_LOW), + [TASK_ALTITUDE] = DEFINE_TASK("ALTITUDE", NULL, taskAltitudeCheck, taskCalculateAltitude, TASK_PERIOD_HZ(TASK_ALTITUDE_RATE_HZ), TASK_PRIORITY_LOW), #endif #ifdef USE_DASHBOARD diff --git a/src/main/flight/gps_rescue.c b/src/main/flight/gps_rescue.c index c2caebfeaf..0ad228b1f2 100644 --- a/src/main/flight/gps_rescue.c +++ b/src/main/flight/gps_rescue.c @@ -146,9 +146,9 @@ PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, .initialAltitudeM = 30, .descentDistanceM = 20, .rescueGroundspeed = 500, - .throttleP = 18, - .throttleI = 40, - .throttleD = 13, + .throttleP = 20, + .throttleI = 20, + .throttleD = 10, .velP = 6, .velI = 20, .velD = 70, diff --git a/src/main/flight/position.c b/src/main/flight/position.c index 1d34ced0ca..dc91dc2972 100644 --- a/src/main/flight/position.c +++ b/src/main/flight/position.c @@ -29,6 +29,7 @@ #include "build/debug.h" #include "common/maths.h" +#include "common/filter.h" #include "fc/runtime_config.h" @@ -46,6 +47,11 @@ #include "pg/pg.h" #include "pg/pg_ids.h" +static int32_t estimatedAltitudeCm = 0; // in cm +#ifdef USE_BARO + static pt2Filter_t baroDerivativeLpf; +#endif + typedef enum { DEFAULT = 0, BARO_ONLY, @@ -60,30 +66,13 @@ PG_RESET_TEMPLATE(positionConfig_t, positionConfig, .altNumSatsBaroFallback = POSITION_DEFAULT_ALT_NUM_SATS_BARO_FALLBACK, ); -static int32_t estimatedAltitudeCm = 0; // in cm - -#define BARO_UPDATE_FREQUENCY_40HZ (1000 * 25) - #ifdef USE_VARIO static int16_t estimatedVario = 0; // in cm/s -int16_t calculateEstimatedVario(int32_t baroAlt, const uint32_t dTime) { - static float vel = 0; - static int32_t lastBaroAlt = 0; - - int32_t baroVel = 0; - - baroVel = (baroAlt - lastBaroAlt) * 1000000.0f / dTime; - lastBaroAlt = baroAlt; - - baroVel = constrain(baroVel, -1500.0f, 1500.0f); - baroVel = applyDeadband(baroVel, 10.0f); - - vel = vel * CONVERT_PARAMETER_TO_FLOAT(barometerConfig()->baro_cf_vel) + baroVel * (1.0f - CONVERT_PARAMETER_TO_FLOAT(barometerConfig()->baro_cf_vel)); - int32_t vel_tmp = lrintf(vel); - vel_tmp = applyDeadband(vel_tmp, 5.0f); - - return constrain(vel_tmp, SHRT_MIN, SHRT_MAX); +int16_t calculateEstimatedVario(float baroAltVelocity) +{ + baroAltVelocity = applyDeadband(baroAltVelocity, 10.0f); // cm/s, so ignore climb rates less than 0.1 m/s + return constrain(lrintf(baroAltVelocity), -1500, 1500); } #endif @@ -91,35 +80,38 @@ int16_t calculateEstimatedVario(int32_t baroAlt, const uint32_t dTime) { static bool altitudeOffsetSetBaro = false; static bool altitudeOffsetSetGPS = false; -void calculateEstimatedAltitude(timeUs_t currentTimeUs) +void calculateEstimatedAltitude() { - static timeUs_t previousTimeUs = 0; - static int32_t baroAltOffset = 0; - static int32_t gpsAltOffset = 0; + static float baroAltOffset = 0; + static float gpsAltOffset = 0; - const uint32_t dTime = currentTimeUs - previousTimeUs; - if (dTime < BARO_UPDATE_FREQUENCY_40HZ) { - schedulerIgnoreTaskExecTime(); - return; - } - previousTimeUs = currentTimeUs; - - int32_t baroAlt = 0; - int32_t gpsAlt = 0; + float baroAlt = 0; + float baroAltVelocity = 0; + float gpsAlt = 0; uint8_t gpsNumSat = 0; #if defined(USE_GPS) && defined(USE_VARIO) - int16_t gpsVertSpeed = 0; + float gpsVertSpeed = 0; #endif float gpsTrust = 0.3; //conservative default bool haveBaroAlt = false; bool haveGpsAlt = false; #ifdef USE_BARO if (sensors(SENSOR_BARO)) { - if (!baroIsCalibrationComplete()) { - performBaroCalibrationCycle(); - } else { - baroAlt = baroCalculateAltitude(); + static float lastBaroAlt = 0; + static bool initBaroFilter = false; + if (!initBaroFilter) { + const float cutoffHz = barometerConfig()->baro_vario_lpf / 100.0f; + const float sampleTimeS = HZ_TO_INTERVAL(TASK_ALTITUDE_RATE_HZ); + const float gain = pt2FilterGain(cutoffHz, sampleTimeS); + pt2FilterInit(&baroDerivativeLpf, gain); + initBaroFilter = true; + } + baroAlt = baroUpsampleAltitude(); + const float baroAltVelocityRaw = (baroAlt - lastBaroAlt) * TASK_ALTITUDE_RATE_HZ; // cm/s + baroAltVelocity = pt2FilterApply(&baroDerivativeLpf, baroAltVelocityRaw); + lastBaroAlt = baroAlt; + if (baroIsCalibrated()) { haveBaroAlt = true; } } @@ -185,7 +177,7 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs) } #ifdef USE_VARIO // baro is a better source for vario, so ignore gpsVertSpeed - estimatedVario = calculateEstimatedVario(baroAlt, dTime); + estimatedVario = calculateEstimatedVario(baroAltVelocity); #endif } else if (haveGpsAlt && (positionConfig()->altSource == GPS_ONLY || positionConfig()->altSource == DEFAULT )) { estimatedAltitudeCm = gpsAlt; @@ -195,12 +187,10 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs) } else if (haveBaroAlt && (positionConfig()->altSource == BARO_ONLY || positionConfig()->altSource == DEFAULT)) { estimatedAltitudeCm = baroAlt; #ifdef USE_VARIO - estimatedVario = calculateEstimatedVario(baroAlt, dTime); + estimatedVario = calculateEstimatedVario(baroAltVelocity); // cm/s #endif } - - DEBUG_SET(DEBUG_ALTITUDE, 0, (int32_t)(100 * gpsTrust)); DEBUG_SET(DEBUG_ALTITUDE, 1, baroAlt); DEBUG_SET(DEBUG_ALTITUDE, 2, gpsAlt); diff --git a/src/main/flight/position.h b/src/main/flight/position.h index 5c2cc5f4b5..bc32d591f1 100644 --- a/src/main/flight/position.h +++ b/src/main/flight/position.h @@ -22,6 +22,7 @@ #include "common/time.h" +#define TASK_ALTITUDE_RATE_HZ 120 #define POSITION_DEFAULT_ALT_NUM_SATS_GPS_USE 10 #define POSITION_DEFAULT_ALT_NUM_SATS_BARO_FALLBACK 7 @@ -34,6 +35,6 @@ typedef struct positionConfig_s { PG_DECLARE(positionConfig_t, positionConfig); bool isAltitudeOffset(void); -void calculateEstimatedAltitude(timeUs_t currentTimeUs); +void calculateEstimatedAltitude(); int32_t getEstimatedAltitudeCm(void); int16_t getEstimatedVario(void); diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c index 802b158610..ac71b6b230 100644 --- a/src/main/sensors/barometer.c +++ b/src/main/sensors/barometer.c @@ -29,6 +29,7 @@ #include "build/debug.h" #include "common/maths.h" +#include "common/filter.h" #include "pg/pg.h" #include "pg/pg_ids.h" @@ -58,13 +59,12 @@ baro_t baro; // barometer access functions -PG_REGISTER_WITH_RESET_FN(barometerConfig_t, barometerConfig, PG_BAROMETER_CONFIG, 1); +PG_REGISTER_WITH_RESET_FN(barometerConfig_t, barometerConfig, PG_BAROMETER_CONFIG, 2); void pgResetFn_barometerConfig(barometerConfig_t *barometerConfig) { - barometerConfig->baro_sample_count = 21; - barometerConfig->baro_noise_lpf = 600; - barometerConfig->baro_cf_vel = 985; + barometerConfig->baro_noise_lpf = 50; + barometerConfig->baro_vario_lpf = 10; barometerConfig->baro_hardware = BARO_DEFAULT; // For backward compatibility; ceate a valid default value for bus parameters @@ -139,18 +139,20 @@ void pgResetFn_barometerConfig(barometerConfig_t *barometerConfig) barometerConfig->baro_xclr_tag = IO_TAG(BARO_XCLR_PIN); } -static uint16_t calibratingB = 0; // baro calibration = get new ground pressure value +static uint16_t calibrationCycles = 0; // baro calibration = get new ground pressure value +static bool baroCalibrated = false; static int32_t baroPressure = 0; static int32_t baroTemperature = 0; +static float baroGroundAltitude = 0.0f; +static float baroAltitudeRaw = 0.0f; +static pt2Filter_t baroUpsampleLpf; -static int32_t baroGroundAltitude = 0; -static int32_t baroGroundPressure = 8*101325; -static uint32_t baroPressureSum = 0; -#define CALIBRATING_BARO_CYCLES 200 // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles +#define CALIBRATING_BARO_CYCLES 100 // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles #define SET_GROUND_LEVEL_BARO_CYCLES 10 // calibrate baro to new ground level (10 * 25 ms = ~250 ms non blocking) static bool baroReady = false; +static bool baroSampleReady = false; void baroPreInit(void) { @@ -161,7 +163,21 @@ void baroPreInit(void) #endif } -bool baroDetect(baroDev_t *baroDev, baroSensor_e baroHardwareToUse) +static bool baroDetect(baroDev_t *baroDev, baroSensor_e baroHardwareToUse); + +void baroInit(void) +{ + baroDetect(&baro.dev, barometerConfig()->baro_hardware); + + const float cutoffHz = barometerConfig()->baro_noise_lpf / 100.0f; + const float sampleTimeS = HZ_TO_INTERVAL(TASK_ALTITUDE_RATE_HZ); + const float gain = pt2FilterGain(cutoffHz, sampleTimeS); + pt2FilterInit(&baroUpsampleLpf, gain); + + baroReady = true; +} + +static bool baroDetect(baroDev_t *baroDev, baroSensor_e baroHardwareToUse) { extDevice_t *dev = &baroDev->dev; @@ -295,72 +311,26 @@ bool baroDetect(baroDev_t *baroDev, baroSensor_e baroHardwareToUse) return true; } -bool baroIsCalibrationComplete(void) +bool baroIsCalibrated(void) { - return calibratingB == 0; + return baroCalibrated; } static void baroSetCalibrationCycles(uint16_t calibrationCyclesRequired) { - calibratingB = calibrationCyclesRequired; + calibrationCycles = calibrationCyclesRequired; } void baroStartCalibration(void) { baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES); + baroCalibrated = false; } void baroSetGroundLevel(void) { baroSetCalibrationCycles(SET_GROUND_LEVEL_BARO_CYCLES); -} - -#define PRESSURE_SAMPLES_MEDIAN 3 - -static int32_t applyBarometerMedianFilter(int32_t newPressureReading) -{ - static int32_t barometerFilterSamples[PRESSURE_SAMPLES_MEDIAN]; - static int currentFilterSampleIndex = 0; - static bool medianFilterReady = false; - int nextSampleIndex; - - nextSampleIndex = (currentFilterSampleIndex + 1); - if (nextSampleIndex == PRESSURE_SAMPLES_MEDIAN) { - nextSampleIndex = 0; - medianFilterReady = true; - } - - barometerFilterSamples[currentFilterSampleIndex] = newPressureReading; - currentFilterSampleIndex = nextSampleIndex; - - if (medianFilterReady) - return quickMedianFilter3(barometerFilterSamples); - else - return newPressureReading; -} - -static uint32_t recalculateBarometerTotal(uint32_t pressureTotal, int32_t newPressureReading) -{ - static int32_t barometerSamples[BARO_SAMPLE_COUNT_MAX + 1]; - static int currentSampleIndex = 0; - int nextSampleIndex; - - // store current pressure in barometerSamples - if (currentSampleIndex >= barometerConfig()->baro_sample_count) { - nextSampleIndex = 0; - baroReady = true; - } else { - nextSampleIndex = (currentSampleIndex + 1); - } - barometerSamples[currentSampleIndex] = applyBarometerMedianFilter(newPressureReading); - - // recalculate pressure total - pressureTotal += barometerSamples[currentSampleIndex]; - pressureTotal -= barometerSamples[nextSampleIndex]; - - currentSampleIndex = nextSampleIndex; - - return pressureTotal; + baroCalibrated = false; } typedef enum { @@ -373,11 +343,24 @@ typedef enum { BARO_STATE_COUNT } barometerState_e; - bool isBaroReady(void) { return baroReady; } +bool isBaroSampleReady(void) +{ + if (baroSampleReady) { + baroSampleReady = false; + return true; + } + return false; + } + +static float pressureToAltitude(const float pressure) +{ + return (1.0f - powf(pressure / 101325.0f, 0.190295f)) * 4433000.0f; +} + uint32_t baroUpdate(timeUs_t currentTimeUs) { static timeUs_t baroStateDurationUs[BARO_STATE_COUNT]; @@ -441,21 +424,24 @@ uint32_t baroUpdate(timeUs_t currentTimeUs) schedulerIgnoreTaskExecTime(); break; } - baro.dev.calculate(&baroPressure, &baroTemperature); baro.baroPressure = baroPressure; baro.baroTemperature = baroTemperature; - baroPressureSum = recalculateBarometerTotal(baroPressureSum, baroPressure); + + baroAltitudeRaw = pressureToAltitude(baroPressure); + + performBaroCalibrationCycle(); + + DEBUG_SET(DEBUG_BARO, 1, baro.baroTemperature); + DEBUG_SET(DEBUG_BARO, 2, baroAltitudeRaw - baroGroundAltitude); + + baroSampleReady = true; + if (baro.dev.combined_read) { state = BARO_STATE_PRESSURE_START; } else { state = BARO_STATE_TEMPERATURE_START; } - - DEBUG_SET(DEBUG_BARO, 1, baroTemperature); - DEBUG_SET(DEBUG_BARO, 2, baroPressure); - DEBUG_SET(DEBUG_BARO, 3, baroPressureSum); - break; } @@ -475,40 +461,30 @@ uint32_t baroUpdate(timeUs_t currentTimeUs) return sleepTime; } -static float pressureToAltitude(const float pressure) +// baroAltitudeRaw will get updated in the BARO task while baroUpsampleAltitude() will run in the ALTITUDE task. +float baroUpsampleAltitude(void) { - return (1.0f - powf(pressure / 101325.0f, 0.190295f)) * 4433000.0f; -} - -int32_t baroCalculateAltitude(void) -{ - int32_t BaroAlt_tmp; - - // calculates height from ground via baro readings - if (baroIsCalibrationComplete()) { - BaroAlt_tmp = lrintf(pressureToAltitude((float)(baroPressureSum / barometerConfig()->baro_sample_count))); - BaroAlt_tmp -= baroGroundAltitude; - baro.BaroAlt = lrintf((float)baro.BaroAlt * CONVERT_PARAMETER_TO_FLOAT(barometerConfig()->baro_noise_lpf) + (float)BaroAlt_tmp * (1.0f - CONVERT_PARAMETER_TO_FLOAT(barometerConfig()->baro_noise_lpf))); // additional LPF to reduce baro noise - } - else { - baro.BaroAlt = 0; + const float baroAltitudeRawFiltered = pt2FilterApply(&baroUpsampleLpf, baroAltitudeRaw); + if (baroIsCalibrated()) { + baro.BaroAlt = baroAltitudeRawFiltered - baroGroundAltitude; + } else { + baro.BaroAlt = 0.0f; } + DEBUG_SET(DEBUG_BARO, 3, baro.BaroAlt); // cm return baro.BaroAlt; } void performBaroCalibrationCycle(void) { - static int32_t savedGroundPressure = 0; - - baroGroundPressure -= baroGroundPressure / 8; - baroGroundPressure += baroPressureSum / barometerConfig()->baro_sample_count; - baroGroundAltitude = (1.0f - pow_approx((baroGroundPressure / 8) / 101325.0f, 0.190259f)) * 4433000.0f; - - if (baroGroundPressure == savedGroundPressure) { - calibratingB = 0; - } else { - calibratingB--; - savedGroundPressure = baroGroundPressure; + static uint16_t cycleCount = 0; + if (!baroCalibrated) { + cycleCount++; + baroGroundAltitude += baroAltitudeRaw; + if (cycleCount == calibrationCycles) { + baroGroundAltitude = baroGroundAltitude / calibrationCycles; // simple average + baroCalibrated = true; + cycleCount = 0; + } } } diff --git a/src/main/sensors/barometer.h b/src/main/sensors/barometer.h index 2d2c6d514d..4cd72ae990 100644 --- a/src/main/sensors/barometer.h +++ b/src/main/sensors/barometer.h @@ -22,6 +22,7 @@ #include "pg/pg.h" #include "drivers/barometer/barometer.h" +#include "flight/position.h" typedef enum { BARO_DEFAULT = 0, @@ -35,8 +36,6 @@ typedef enum { BARO_DPS310 = 8, } baroSensor_e; -#define BARO_SAMPLE_COUNT_MAX 48 - typedef struct barometerConfig_s { uint8_t baro_busType; uint8_t baro_spi_device; @@ -44,18 +43,21 @@ typedef struct barometerConfig_s { uint8_t baro_i2c_device; uint8_t baro_i2c_address; uint8_t baro_hardware; // Barometer hardware to use - uint8_t baro_sample_count; // size of baro filter array - uint16_t baro_noise_lpf; // additional LPF to reduce baro noise - uint16_t baro_cf_vel; // apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity) + uint16_t baro_noise_lpf; // lowpass cutoff (value / 100) Hz for baro smoothing + uint16_t baro_vario_lpf; // lowpass for (value / 100) Hz baro derivative smoothing ioTag_t baro_eoc_tag; ioTag_t baro_xclr_tag; } barometerConfig_t; PG_DECLARE(barometerConfig_t, barometerConfig); +// #define TASK_BARO_DENOM 3 +// #define TASK_BARO_RATE_HZ (TASK_ALTITUDE_RATE_HZ / TASK_BARO_DENOM) +#define TASK_BARO_RATE_HZ 40 + typedef struct baro_s { baroDev_t dev; - int32_t BaroAlt; + float BaroAlt; int32_t baroTemperature; // Use temperature for telemetry int32_t baroPressure; // Use pressure for telemetry } baro_t; @@ -63,11 +65,12 @@ typedef struct baro_s { extern baro_t baro; void baroPreInit(void); -bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse); -bool baroIsCalibrationComplete(void); +void baroInit(void); +bool baroIsCalibrated(void); void baroStartCalibration(void); void baroSetGroundLevel(void); uint32_t baroUpdate(timeUs_t currentTimeUs); bool isBaroReady(void); -int32_t baroCalculateAltitude(void); +bool isBaroSampleReady(void); +float baroUpsampleAltitude(void); void performBaroCalibrationCycle(void); diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index deafc1fc16..c4a97e0efa 100644 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -81,7 +81,7 @@ bool sensorsAutodetect(void) #endif #ifdef USE_BARO - baroDetect(&baro.dev, barometerConfig()->baro_hardware); + baroInit(); #endif #ifdef USE_RANGEFINDER diff --git a/src/test/unit/arming_prevention_unittest.cc b/src/test/unit/arming_prevention_unittest.cc index dbbe6f93fe..af2029986f 100644 --- a/src/test/unit/arming_prevention_unittest.cc +++ b/src/test/unit/arming_prevention_unittest.cc @@ -1042,7 +1042,7 @@ extern "C" { void saveConfigAndNotify(void) {} void blackboxFinish(void) {} bool accIsCalibrationComplete(void) { return true; } - bool baroIsCalibrationComplete(void) { return true; } + bool baroIsCalibrated(void) { return true; } bool gyroIsCalibrationComplete(void) { return gyroCalibDone; } void gyroStartCalibration(bool) {} bool isFirstArmingGyroCalibrationRunning(void) { return false; } diff --git a/src/test/unit/flight_imu_unittest.cc b/src/test/unit/flight_imu_unittest.cc index ba7164a80d..534d883df6 100644 --- a/src/test/unit/flight_imu_unittest.cc +++ b/src/test/unit/flight_imu_unittest.cc @@ -43,6 +43,7 @@ extern "C" { #include "flight/mixer.h" #include "flight/pid.h" #include "flight/imu.h" + #include "flight/position.h" #include "io/gps.h" @@ -202,57 +203,64 @@ TEST(FlightImuTest, TestSmallAngle) // STUBS extern "C" { -boxBitmask_t rcModeActivationMask; -float rcCommand[4]; -float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; + boxBitmask_t rcModeActivationMask; + float rcCommand[4]; + float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; -gyro_t gyro; -acc_t acc; -mag_t mag; + gyro_t gyro; + acc_t acc; + mag_t mag; -gpsSolutionData_t gpsSol; -int16_t GPS_verticalSpeedInCmS; + gpsSolutionData_t gpsSol; + int16_t GPS_verticalSpeedInCmS; -uint8_t debugMode; -int16_t debug[DEBUG16_VALUE_COUNT]; + uint8_t debugMode; + int16_t debug[DEBUG16_VALUE_COUNT]; -uint8_t stateFlags; -uint16_t flightModeFlags; -uint8_t armingFlags; + uint8_t stateFlags; + uint16_t flightModeFlags; + uint8_t armingFlags; -pidProfile_t *currentPidProfile; + pidProfile_t *currentPidProfile; -uint16_t enableFlightMode(flightModeFlags_e mask) -{ - return flightModeFlags |= (mask); -} - -uint16_t disableFlightMode(flightModeFlags_e mask) -{ - return flightModeFlags &= ~(mask); -} - -bool sensors(uint32_t mask) -{ - return mask & SENSOR_ACC; -}; - -uint32_t millis(void) { return 0; } -uint32_t micros(void) { return 0; } - -bool compassIsHealthy(void) { return true; } -bool baroIsCalibrationComplete(void) { return true; } -void performBaroCalibrationCycle(void) {} -int32_t baroCalculateAltitude(void) { return 0; } -bool gyroGetAccumulationAverage(float *) { return false; } -bool accGetAccumulationAverage(float *) { return false; } -void mixerSetThrottleAngleCorrection(int) {}; -bool gpsRescueIsRunning(void) { return false; } -bool isFixedWing(void) { return false; } -void pinioBoxTaskControl(void) {} -void schedulerIgnoreTaskExecTime(void) {} -void schedulerIgnoreTaskStateTime(void) {} -void schedulerSetNextStateTime(timeDelta_t) {} -bool schedulerGetIgnoreTaskExecTime() { return false; } -float gyroGetFilteredDownsampled(int) { return 0.0f; } + uint16_t enableFlightMode(flightModeFlags_e mask) { + return flightModeFlags |= (mask); + } + + uint16_t disableFlightMode(flightModeFlags_e mask) { + return flightModeFlags &= ~(mask); + } + + bool sensors(uint32_t mask) { + return mask & SENSOR_ACC; + }; + + uint32_t millis(void) { return 0; } + uint32_t micros(void) { return 0; } + + bool compassIsHealthy(void) { return true; } + bool baroIsCalibrated(void) { return true; } + void performBaroCalibrationCycle(void) {} + float baroCalculateAltitude(void) { return 0; } + bool gyroGetAccumulationAverage(float *) { return false; } + bool accGetAccumulationAverage(float *) { return false; } + void mixerSetThrottleAngleCorrection(int) {}; + bool gpsRescueIsRunning(void) { return false; } + bool isFixedWing(void) { return false; } + void pinioBoxTaskControl(void) {} + void schedulerIgnoreTaskExecTime(void) {} + void schedulerIgnoreTaskStateTime(void) {} + void schedulerSetNextStateTime(timeDelta_t) {} + bool schedulerGetIgnoreTaskExecTime() { return false; } + float gyroGetFilteredDownsampled(int) { return 0.0f; } + float baroUpsampleAltitude() { return 0.0f; } + float pt2FilterGain(float, float) { return 0.0f; } + + void pt2FilterInit(pt2Filter_t *baroDerivativeLpf, float) { + UNUSED(baroDerivativeLpf); + } + float pt2FilterApply(pt2Filter_t *baroDerivativeLpf, float) { + UNUSED(baroDerivativeLpf); + return 0.0f; + } } diff --git a/src/test/unit/telemetry_ibus_unittest.cc b/src/test/unit/telemetry_ibus_unittest.cc index b4340b73f3..c291cafed3 100644 --- a/src/test/unit/telemetry_ibus_unittest.cc +++ b/src/test/unit/telemetry_ibus_unittest.cc @@ -82,7 +82,7 @@ int32_t getAmperage(void) return amperage; } -int32_t getEstimatedVario(void) +int16_t getEstimatedVario(void) { return estimatedVario; } diff --git a/src/test/unit/vtx_unittest.cc b/src/test/unit/vtx_unittest.cc index ad457450ec..f9f3311f89 100644 --- a/src/test/unit/vtx_unittest.cc +++ b/src/test/unit/vtx_unittest.cc @@ -136,7 +136,7 @@ extern "C" { void blackboxFinish(void) {} bool accIsCalibrationComplete(void) { return true; } bool accHasBeenCalibrated(void) { return true; } - bool baroIsCalibrationComplete(void) { return true; } + bool baroIsCalibrated(void) { return true; } bool gyroIsCalibrationComplete(void) { return gyroCalibDone; } void gyroStartCalibration(bool) {} bool isFirstArmingGyroCalibrationRunning(void) { return false; }