Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-23 16:25:26 +03:00
Code Issues Wiki Activity

BARO: Remove averaging and extra LPF filtering. Changed defaults for sensor fusion for INAV

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2016-03-29 11:13:15 +10:00
parent 517c958d17
commit c180856b4f
6 changed files with 19 additions and 51 deletions
Download patch file Download diff file Expand all files Collapse all files

15
src/main/config/config.c
View file

@ -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)
{
@ -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;
@ -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)
@ -546,7 +545,7 @@ static void resetConf(void)
currentProfile->mag_declination = 0;
resetBarometerConfig(&currentProfile->barometerConfig);
resetBarometerConfig(&masterConfig.barometerConfig);
// Radio
parseRcChannels("AETR1234", &masterConfig.rxConfig);
@ -808,7 +807,7 @@ void activateConfig(void)
#endif
#ifdef BARO
useBarometerConfig(&currentProfile->barometerConfig);
useBarometerConfig(&masterConfig.barometerConfig);
#endif
}

2
src/main/config/config_master.h
View file

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

2
src/main/config/config_profile.h
View file

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

3
src/main/io/serial_cli.c
View file

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

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

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

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

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