diff --git a/src/main/common/maths.c b/src/main/common/maths.c index 911fa84162..71dc6f40ef 100644 --- a/src/main/common/maths.c +++ b/src/main/common/maths.c @@ -304,96 +304,6 @@ void arraySubInt32(int32_t *dest, int32_t *array1, int32_t *array2, int count) } } -// Base functions for FIR filters -void filterWithBufferReset(filterWithBufferState_t * state) -{ - int i; - state->sample_index = 0; - for (i = 0; i < state->filter_size; i++) { - state->samples[i].value = 0.0f; - state->samples[i].timestamp = 0; - } -} - -void filterWithBufferInit(filterWithBufferState_t * state, filterWithBufferSample_t * buffer, uint16_t size) -{ - state->samples = buffer; - state->filter_size = size; - filterWithBufferReset(state); -} - -void filterWithBufferUpdate(filterWithBufferState_t * state, float sample, uint32_t timestamp) -{ - uint16_t old_sample_index; - - if (state->sample_index == 0) - old_sample_index = state->filter_size - 1; - else - old_sample_index = state->sample_index - 1; - - if (state->samples[old_sample_index].timestamp == timestamp) { - return; - } - - state->samples[state->sample_index].timestamp = timestamp; - state->samples[state->sample_index].value = sample; - state->sample_index++; - - if (state->sample_index >= state->filter_size) - state->sample_index = 0; -} - -// An implementation of a derivative (slope) filter -// Code mostly taken from ArduPilot's DerivativeFilter.cpp -// http://www.holoborodko.com/pavel/numerical-methods/numerical-derivative/smooth-low-noise-differentiators/ -float filterWithBufferApply_Derivative(filterWithBufferState_t * state) -{ - float result = 0; - - // use f() to make the code match the maths a bit better. Note - // that unlike an average filter, we care about the order of the elements - #define f(i) (state->samples[(((state->sample_index - 1) + i + 1) + 3 * state->filter_size / 2) % state->filter_size].value) - #define x(i) (state->samples[(((state->sample_index - 1) + i + 1) + 3 * state->filter_size / 2) % state->filter_size].timestamp) - - if (state->samples[state->filter_size - 1].timestamp == state->samples[state->filter_size - 2].timestamp) { - return 0; - } - - switch (state->filter_size) { - case 5: - result = 2*2*(f(1) - f(-1)) / (x(1) - x(-1)) - + 4*1*(f(2) - f(-2)) / (x(2) - x(-2)); - result /= 8; - break; - case 7: - result = 2*5*(f(1) - f(-1)) / (x(1) - x(-1)) - + 4*4*(f(2) - f(-2)) / (x(2) - x(-2)) - + 6*1*(f(3) - f(-3)) / (x(3) - x(-3)); - result /= 32; - break; - case 9: - result = 2*14*(f(1) - f(-1)) / (x(1) - x(-1)) - + 4*14*(f(2) - f(-2)) / (x(2) - x(-2)) - + 6* 6*(f(3) - f(-3)) / (x(3) - x(-3)) - + 8* 1*(f(4) - f(-4)) / (x(4) - x(-4)); - result /= 128; - break; - case 11: - result = 2*42*(f(1) - f(-1)) / (x(1) - x(-1)) - + 4*48*(f(2) - f(-2)) / (x(2) - x(-2)) - + 6*27*(f(3) - f(-3)) / (x(3) - x(-3)) - + 8* 8*(f(4) - f(-4)) / (x(4) - x(-4)) - + 10* 1*(f(5) - f(-5)) / (x(5) - x(-5)); - result /= 512; - break; - default: - result = 0; - break; - } - - return result; -} - /** * Sensor offset calculation code based on Freescale's AN4246 * Initial implementation by @HaukeRa diff --git a/src/main/common/maths.h b/src/main/common/maths.h index e2bcf4df7c..4390242c51 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -131,11 +131,6 @@ int32_t quickMedianFilter5(int32_t * v); int32_t quickMedianFilter7(int32_t * v); int32_t quickMedianFilter9(int32_t * v); -void filterWithBufferReset(filterWithBufferState_t * state); -void filterWithBufferInit(filterWithBufferState_t * state, filterWithBufferSample_t * buffer, uint16_t size); -void filterWithBufferUpdate(filterWithBufferState_t * state, float sample, uint32_t timestamp); -float filterWithBufferApply_Derivative(filterWithBufferState_t * state); - #if defined(FAST_MATH) || defined(VERY_FAST_MATH) float sin_approx(float x); float cos_approx(float x); diff --git a/src/main/flight/navigation_rewrite_pos_estimator.c b/src/main/flight/navigation_rewrite_pos_estimator.c index 36f2fc037b..f6716a0d56 100755 --- a/src/main/flight/navigation_rewrite_pos_estimator.c +++ b/src/main/flight/navigation_rewrite_pos_estimator.c @@ -62,13 +62,11 @@ #define INAV_POSITION_PUBLISH_RATE_HZ 50 // Publish position updates at this rate #define INAV_BARO_UPDATE_RATE 20 -#define INAV_SONAR_UPDATE_RATE 10 // Sonar is limited to 1/60ms update rate, go lower that that +#define INAV_SONAR_UPDATE_RATE 15 // Sonar is limited to 1/60ms update rate, go lower that that #define INAV_GPS_TIMEOUT_MS 1500 // GPS timeout #define INAV_BARO_TIMEOUT_MS 200 // Baro timeout -#define INAV_SONAR_TIMEOUT_MS 250 // Sonar timeout (missed two readings in a row - -#define INAV_BARO_CLIMB_RATE_FILTER_SIZE 7 +#define INAV_SONAR_TIMEOUT_MS 200 // Sonar timeout (missed 3 readings in a row) #define INAV_SONAR_W1 0.8461f // Sonar predictive filter gain for altitude #define INAV_SONAR_W2 6.2034f // Sonar predictive filter gain for velocity @@ -97,7 +95,6 @@ typedef struct { typedef struct { uint32_t lastUpdateTime; // Last update time (us) float alt; // Raw barometric altitude (cm) - float vel; // Altitude derivative - velocity (cms) float epv; } navPositionEstimatorBARO_t; @@ -345,39 +342,16 @@ void onNewGPSData(int32_t newLat, int32_t newLon, int32_t newAlt, int16_t velN, static void updateBaroTopic(uint32_t currentTime) { static navigationTimer_t baroUpdateTimer; - /* - static filterWithBufferSample_t baroClimbRateFilterBuffer[INAV_BARO_CLIMB_RATE_FILTER_SIZE]; - static filterWithBufferState_t baroClimbRateFilter; - static bool climbRateFiltersInitialized = false; - */ if (updateTimer(&baroUpdateTimer, HZ2US(INAV_BARO_UPDATE_RATE), currentTime)) { - /* - if (!climbRateFiltersInitialized) { - filterWithBufferInit(&baroClimbRateFilter, &baroClimbRateFilterBuffer[0], INAV_BARO_CLIMB_RATE_FILTER_SIZE); - climbRateFiltersInitialized = true; - } - */ - float newBaroAlt = baroCalculateAltitude(); if (sensors(SENSOR_BARO) && isBaroCalibrationComplete()) { - /* - filterWithBufferUpdate(&baroClimbRateFilter, newBaroAlt, currentTime); - float baroClimbRate = filterWithBufferApply_Derivative(&baroClimbRateFilter) * 1e6f; - - // FIXME: do we need this? - baroClimbRate = constrainf(baroClimbRate, -1500, 1500); // constrain baro velocity +/- 1500cm/s - baroClimbRate = applyDeadband(baroClimbRate, 10); // to reduce noise near zero - */ - posEstimator.baro.alt = newBaroAlt; - posEstimator.baro.vel = 0; //baroClimbRate; posEstimator.baro.epv = posControl.navConfig->inav.baro_epv; posEstimator.baro.lastUpdateTime = currentTime; } else { posEstimator.baro.alt = 0; - posEstimator.baro.vel = 0; posEstimator.baro.lastUpdateTime = 0; } } @@ -507,7 +481,7 @@ static void updateEstimatedTopic(uint32_t currentTime) /* Apply GPS altitude corrections only on fixed wing aircrafts */ bool useGpsZ = STATE(FIXED_WING) && isGPSValid; - /* Pre-calculate history index for GPS & baro delay compensation */ + /* Pre-calculate history index for GPS delay compensation */ int gpsHistoryIndex = (posEstimator.history.index - 1) - constrain(((int)posControl.navConfig->inav.gps_delay_ms / (1000 / INAV_POSITION_PUBLISH_RATE_HZ)), 0, INAV_HISTORY_BUF_SIZE - 1); if (gpsHistoryIndex < 0) { gpsHistoryIndex += INAV_HISTORY_BUF_SIZE; @@ -535,7 +509,6 @@ static void updateEstimatedTopic(uint32_t currentTime) /* accelerometer bias correction for baro */ if (isBaroValid) { accelBiasCorr.V.Z -= (posEstimator.baro.alt - posEstimator.est.pos.V.Z) * sq(posControl.navConfig->inav.w_z_baro_p); - //accelBiasCorr.V.Z -= (posEstimator.baro.vel - posEstimator.est.vel.V.Z) * posControl.navConfig->inav.w_z_baro_v; } /* transform error vector from NEU frame to body frame */ @@ -548,7 +521,6 @@ static void updateEstimatedTopic(uint32_t currentTime) } /* Estimate Z-axis */ - /* FIXME: baro & sonar delay compensation (~100ms for sonar, about 500ms for baro) */ if ((posEstimator.est.epv < posControl.navConfig->inav.max_eph_epv) || useGpsZ || isBaroValid) { /* Predict position/velocity based on acceleration */ inavFilterPredict(Z, dt, posEstimator.imu.accelNEU.V.Z); @@ -557,7 +529,6 @@ static void updateEstimatedTopic(uint32_t currentTime) if (isBaroValid) { /* Apply only baro correction, no sonar */ inavFilterCorrectPos(Z, dt, posEstimator.baro.alt - posEstimator.est.pos.V.Z, posControl.navConfig->inav.w_z_baro_p); - //inavFilterCorrectVel(Z, dt, posEstimator.baro.vel - posEstimator.est.vel.V.Z, posControl.navConfig->inav.w_z_baro_v); /* Adjust EPV */ posEstimator.est.epv = MIN(posEstimator.est.epv, posEstimator.baro.epv);