Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-15 12:25:20 +03:00
Code Issues Wiki Activity

Refactor Baro to floats, filter at position rate

Browse source 
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 <post@stud.tu-darmstadt.de>
Co-Authored-By: Steve Evans <SteveCEvans@users.noreply.github.com>
This commit is contained in:
ctzsnooze 2022-08-02 15:21:38 +10:00
parent 21594c62e1
commit b2241b32c3
16 changed files with 209 additions and 212 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/blackbox/blackbox.c
View file

@ -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);

5
src/main/cli/settings.c
View file

@ -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

2
src/main/cms/cms_menu_firmware.c
View file

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

2
src/main/fc/core.c
View file

@ -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())

2
src/main/fc/parameter_names.h
View file

@ -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"

24
src/main/fc/tasks.c
View file

@ -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

6
src/main/flight/gps_rescue.c
View file

@ -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,

76
src/main/flight/position.c
View file

@ -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);

3
src/main/flight/position.h
View file

@ -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);

168
src/main/sensors/barometer.c
View file

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

21
src/main/sensors/barometer.h
View file

@ -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);

2
src/main/sensors/initialisation.c
View file

@ -81,7 +81,7 @@ bool sensorsAutodetect(void)
#endif
#ifdef USE_BARO
baroDetect(&baro.dev, barometerConfig()->baro_hardware);
baroInit();
#endif
#ifdef USE_RANGEFINDER

2
src/test/unit/arming_prevention_unittest.cc
View file

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

102
src/test/unit/flight_imu_unittest.cc
View file

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

2
src/test/unit/telemetry_ibus_unittest.cc
View file

@ -82,7 +82,7 @@ int32_t getAmperage(void)
return amperage;
}
int32_t getEstimatedVario(void)
int16_t getEstimatedVario(void)
{
return estimatedVario;
}

2
src/test/unit/vtx_unittest.cc
View file

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