Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-25 01:05:27 +03:00
Code Issues Wiki Activity

Merge pull request #6071 from codecae/reintroduce-lma

Browse source 
Lagged Moving Average smoothing/filter
This commit is contained in:
Michael Keller 2018-06-10 01:52:31 +12:00 • committed by GitHub
commit ce61f3b99d
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
2 changed files with 35 additions and 0 deletions
Download patch file Download diff file Expand all files Collapse all files

23
src/main/common/filter.c
View file

@ -192,3 +192,26 @@ FAST_CODE float biquadFilterApply(biquadFilter_t *filter, float input)
filter->x2 = filter->b2 * input - filter->a2 * result;
return result;
}
void laggedMovingAverageInit(laggedMovingAverage_t *filter, uint16_t windowSize, float *buf)
{
filter->movingWindowIndex = 0;
filter->windowSize = windowSize;
filter->buf = buf;
filter->primed = false;
}
FAST_CODE float laggedMovingAverageUpdate(laggedMovingAverage_t *filter, float input)
{
filter->movingSum -= filter->buf[filter->movingWindowIndex];
filter->buf[filter->movingWindowIndex] = input;
filter->movingSum += input;
if (++filter->movingWindowIndex == filter->windowSize) {
filter->movingWindowIndex = 0;
filter->primed = true;
}
const uint16_t denom = filter->primed ? filter->windowSize : filter->movingWindowIndex;
return filter->movingSum / denom;
}

12
src/main/common/filter.h
View file

@ -19,6 +19,7 @@
*/
#pragma once
#include <stdbool.h>
struct filter_s;
typedef struct filter_s filter_t;
@ -40,6 +41,14 @@ typedef struct biquadFilter_s {
float x1, x2, y1, y2;
} biquadFilter_t;
typedef struct laggedMovingAverage_s {
uint16_t movingWindowIndex;
uint16_t windowSize;
float movingSum;
float *buf;
bool primed;
} laggedMovingAverage_t;
typedef enum {
FILTER_PT1 = 0,
FILTER_BIQUAD,
@ -63,6 +72,9 @@ float biquadFilterApplyDF1(biquadFilter_t *filter, float input);
float biquadFilterApply(biquadFilter_t *filter, float input);
float filterGetNotchQ(float centerFreq, float cutoffFreq);
void laggedMovingAverageInit(laggedMovingAverage_t *filter, uint16_t windowSize, float *buf);
float laggedMovingAverageUpdate(laggedMovingAverage_t *filter, float input);
float pt1FilterGain(uint16_t f_cut, float dT);
void pt1FilterInit(pt1Filter_t *filter, float k);
float pt1FilterApply(pt1Filter_t *filter, float input);