mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-24 16:55:36 +03:00
Servo filtering test
This commit is contained in:
parent
a9020d7b51
commit
ceb4690bef
3 changed files with 61 additions and 0 deletions
|
@ -653,3 +653,50 @@ bool isMixerUsingServos(void)
|
|||
{
|
||||
return useServo;
|
||||
}
|
||||
|
||||
#include "notch_table.h"
|
||||
#define NOTCH_FILTER_COEFS 3
|
||||
static notchFilter_t notchFilters[MAX_SUPPORTED_SERVOS];
|
||||
|
||||
static float notchFilter(notchFilter_t *filter, float in)
|
||||
{
|
||||
int16_t coefIdx;
|
||||
float out;
|
||||
|
||||
if (!filter->init) {
|
||||
filter->freqIdx = 9;
|
||||
filter->freq = notch_table[filter->freqIdx][0];
|
||||
filter->pCoef = ¬ch_table[filter->freqIdx][1];
|
||||
for (coefIdx = 0; coefIdx < NOTCH_FILTER_COEFS; coefIdx++) {
|
||||
filter->in[coefIdx] = in;
|
||||
filter->out[coefIdx] = in;
|
||||
}
|
||||
filter->init = true;
|
||||
}
|
||||
|
||||
filter->in[2] = filter->in[1];
|
||||
filter->in[1] = filter->in[0];
|
||||
filter->in[0] = in;
|
||||
filter->out[2] = filter->out[1];
|
||||
filter->out[1] = filter->out[0];
|
||||
|
||||
out = filter->in[0] * filter->pCoef[0] +
|
||||
filter->in[1] * filter->pCoef[1] +
|
||||
filter->in[2] * filter->pCoef[2] -
|
||||
(filter->out[1] * filter->pCoef[3] +
|
||||
filter->out[2] * filter->pCoef[4]);
|
||||
|
||||
filter->out[0] = out;
|
||||
|
||||
return out;
|
||||
}
|
||||
|
||||
void filterServos(void)
|
||||
{
|
||||
int16_t servoIdx;
|
||||
|
||||
for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
|
||||
notchFilter(¬chFilters[servoIdx], servo[servoIdx]);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue