Skip to content

Commit

Permalink
BARO: Remove averaging and extra LPF filtering. Changed defaults for …
Browse files Browse the repository at this point in the history 
…sensor fusion for INAV
  • Loading branch information
@digitalentity
digitalentity committed Mar 29, 2016
1 parent 517c958 commit c180856
Show file tree
Hide file tree
Showing 6 changed files with 19 additions and 51 deletions.
15 changes: 7 additions & 8 deletions src/main/config/config.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -134,7 +134,7 @@ static uint32_t activeFeaturesLatch = 0;
static uint8_t currentControlRateProfileIndex = 0;
controlRateConfig_t *currentControlRateProfile;

static const uint8_t EEPROM_CONF_VERSION = 114;
static const uint8_t EEPROM_CONF_VERSION = 115;

static void resetAccelerometerTrims(flightDynamicsTrims_t * accZero, flightDynamicsTrims_t * accGain)
{
Expand Down Expand Up @@ -231,10 +231,10 @@ void resetNavConfig(navConfig_t * navConfig)
navConfig->inav.accz_unarmed_cal = 1;
navConfig->inav.use_gps_velned = 0; // "Disabled" is mandatory with gps_nav_model = LOW_G

navConfig->inav.w_z_baro_p = 1.0f;
navConfig->inav.w_z_baro_p = 0.35f;

navConfig->inav.w_z_gps_p = 0.3f;
navConfig->inav.w_z_gps_v = 0.3f;
navConfig->inav.w_z_gps_p = 0.2f;
navConfig->inav.w_z_gps_v = 0.2f;

navConfig->inav.w_xy_gps_p = 1.0f;
navConfig->inav.w_xy_gps_v = 2.0f;
Expand Down Expand Up @@ -277,8 +277,7 @@ void resetNavConfig(navConfig_t * navConfig)

void resetBarometerConfig(barometerConfig_t *barometerConfig)
{
barometerConfig->baro_sample_count = 7;
barometerConfig->baro_noise_lpf = 0.35f;
barometerConfig->use_median_filtering = 1;
}

void resetSensorAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig)
Expand Down Expand Up @@ -546,7 +545,7 @@ static void resetConf(void)

currentProfile->mag_declination = 0;

resetBarometerConfig(&currentProfile->barometerConfig);
resetBarometerConfig(&masterConfig.barometerConfig);

// Radio
parseRcChannels("AETR1234", &masterConfig.rxConfig);
Expand Down Expand Up @@ -808,7 +807,7 @@ void activateConfig(void)
#endif

#ifdef BARO
useBarometerConfig(&currentProfile->barometerConfig);
useBarometerConfig(&masterConfig.barometerConfig);
#endif
}

Expand Down
2 changes: 2 additions & 0 deletions src/main/config/config_master.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,6 +59,8 @@ typedef struct master_t {

gyroConfig_t gyroConfig;

barometerConfig_t barometerConfig;

uint8_t mag_hardware; // Which mag hardware to use on boards with more than one device
uint8_t baro_hardware; // Barometer hardware to use

Expand Down
2 changes: 0 additions & 2 deletions src/main/config/config_profile.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,8 +25,6 @@ typedef struct profile_s {
int16_t mag_declination; // Get your magnetic decliniation from here : http://magnetic-declination.com/
// For example, -6deg 37min, = -637 Japan, format is [sign]dddmm (degreesminutes) default is zero.

barometerConfig_t barometerConfig;

modeActivationCondition_t modeActivationConditions[MAX_MODE_ACTIVATION_CONDITION_COUNT];

adjustmentRange_t adjustmentRanges[MAX_ADJUSTMENT_RANGE_COUNT];
Expand Down
3 changes: 1 addition & 2 deletions src/main/io/serial_cli.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -701,8 +701,7 @@ const clivalue_t valueTable[] = {

{ "acc_hardware", VAR_UINT8 | MASTER_VALUE, &masterConfig.acc_hardware, .config.minmax = { 0, ACC_MAX }, 0 },

{ "baro_tab_size", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].barometerConfig.baro_sample_count, .config.minmax = { 0, BARO_SAMPLE_COUNT_MAX }, 0 },
{ "baro_noise_lpf", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].barometerConfig.baro_noise_lpf, .config.minmax = { 0, 1 }, 0 },
{ "baro_use_median_filter", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.barometerConfig.use_median_filtering, .config.lookup = { TABLE_OFF_ON }, 0 },
{ "baro_hardware", VAR_UINT8 | MASTER_VALUE, &masterConfig.baro_hardware, .config.minmax = { 0, BARO_MAX }, 0 },

{ "mag_hardware", VAR_UINT8 | MASTER_VALUE, &masterConfig.mag_hardware, .config.minmax = { 0, MAG_MAX }, 0 },
Expand Down
45 changes: 8 additions & 37 deletions src/main/sensors/barometer.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,6 @@ int32_t BaroAlt = 0;

static int32_t baroGroundAltitude = 0;
static int32_t baroGroundPressure = 0;
static uint32_t baroPressureSum = 0;

static barometerConfig_t *barometerConfig;

Expand Down Expand Up @@ -72,7 +71,7 @@ static int32_t applyBarometerMedianFilter(int32_t newPressureReading)
static int currentFilterSampleIndex = 0;
static bool medianFilterReady = false;
int nextSampleIndex;

nextSampleIndex = (currentFilterSampleIndex + 1);
if (nextSampleIndex == PRESSURE_SAMPLES_MEDIAN) {
nextSampleIndex = 0;
Expand All @@ -81,45 +80,18 @@ static int32_t applyBarometerMedianFilter(int32_t newPressureReading)

barometerFilterSamples[currentFilterSampleIndex] = newPressureReading;
currentFilterSampleIndex = nextSampleIndex;

if (medianFilterReady)
return quickMedianFilter3(barometerFilterSamples);
else
return newPressureReading;
}

#define PRESSURE_SAMPLE_COUNT (barometerConfig->baro_sample_count - 1)

static uint32_t recalculateBarometerTotal(uint8_t baroSampleCount, uint32_t pressureTotal, int32_t newPressureReading)
{
static int32_t barometerSamples[BARO_SAMPLE_COUNT_MAX];
static int currentSampleIndex = 0;
int nextSampleIndex;

// store current pressure in barometerSamples
nextSampleIndex = (currentSampleIndex + 1);
if (nextSampleIndex == baroSampleCount) {
nextSampleIndex = 0;
baroReady = true;
}
barometerSamples[currentSampleIndex] = applyBarometerMedianFilter(newPressureReading);

// recalculate pressure total
// Note, the pressure total is made up of baroSampleCount - 1 samples - See PRESSURE_SAMPLE_COUNT
pressureTotal += barometerSamples[currentSampleIndex];
pressureTotal -= barometerSamples[nextSampleIndex];

currentSampleIndex = nextSampleIndex;

return pressureTotal;
}

typedef enum {
BAROMETER_NEEDS_SAMPLES = 0,
BAROMETER_NEEDS_CALCULATION
} barometerState_e;


bool isBaroReady(void) {
return baroReady;
}
Expand All @@ -141,7 +113,9 @@ uint32_t baroUpdate(void)
baro.get_up();
baro.start_ut();
baro.calculate(&baroPressure, &baroTemperature);
baroPressureSum = recalculateBarometerTotal(barometerConfig->baro_sample_count, baroPressureSum, baroPressure);
if (barometerConfig->use_median_filtering) {
baroPressure = applyBarometerMedianFilter(baroPressure);
}
state = BAROMETER_NEEDS_SAMPLES;
return baro.ut_delay;
break;
Expand All @@ -151,7 +125,7 @@ uint32_t baroUpdate(void)
static void performBaroCalibrationCycle(void)
{
baroGroundPressure -= baroGroundPressure / 8;
baroGroundPressure += baroPressureSum / PRESSURE_SAMPLE_COUNT;
baroGroundPressure += baroPressure;
baroGroundAltitude = (1.0f - powf((baroGroundPressure / 8) / 101325.0f, 0.190295f)) * 4433000.0f;

calibratingB--;
Expand All @@ -167,13 +141,10 @@ int32_t baroCalculateAltitude(void)
#ifdef HIL
if (!hilActive) {
#endif
int32_t BaroAlt_tmp;

// calculates height from ground via baro readings
// see: https://github.com/diydrones/ardupilot/blob/master/libraries/AP_Baro/AP_Baro.cpp#L140
BaroAlt_tmp = lrintf((1.0f - powf((float)(baroPressureSum / PRESSURE_SAMPLE_COUNT) / 101325.0f, 0.190295f)) * 4433000.0f); // in cm
BaroAlt_tmp -= baroGroundAltitude;
BaroAlt = lrintf((float)BaroAlt * barometerConfig->baro_noise_lpf + (float)BaroAlt_tmp * (1.0f - barometerConfig->baro_noise_lpf)); // additional LPF to reduce baro noise
BaroAlt = lrintf((1.0f - powf((float)(baroPressure) / 101325.0f, 0.190295f)) * 4433000.0f); // in cm
BaroAlt -= baroGroundAltitude;
#ifdef HIL
} else {
BaroAlt = hilToFC.baroAlt;
Expand Down
3 changes: 1 addition & 2 deletions src/main/sensors/barometer.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,8 +30,7 @@ typedef enum {
#define BARO_MAX BARO_FAKE

typedef struct barometerConfig_s {
uint8_t baro_sample_count; // size of baro filter array
float baro_noise_lpf; // additional LPF to reduce baro noise
uint8_t use_median_filtering; // Use 3-point median filtering
} barometerConfig_t;

extern int32_t BaroAlt;
Expand Down

3 comments on commit c180856

@Linjieqiang
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why need remove averaging and extra LPF filtering code?

@digitalentity
Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@Linjieqiang because that introduces delay without adding much benefit.

@Linjieqiang
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Oh,I see.

Please sign in to comment.