mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-22 07:45:24 +03:00
Remove unused filterWithBuffer code. Not useful anymore as inertial filter alone calculate velocity with sufficient accuracy
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity)
parent
4dbd985046
commit
03bfb70a15
3 changed files with 3 additions and 127 deletions
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue