Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-19 22:35:19 +03:00
Code Issues Wiki Activity

Remove unused filterWithBuffer code. Not useful anymore as inertial filter alone calculate velocity with sufficient accuracy

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2015-11-25 22:56:57 +10:00
parent 4dbd985046
commit 03bfb70a15
3 changed files with 3 additions and 127 deletions
Download patch file Download diff file Expand all files Collapse all files

90
src/main/common/maths.c
View file

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