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; }