forked from ExpressLRS/ExpressLRS
-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathLowPassFilter.h
More file actions
65 lines (54 loc) · 1.36 KB
/
LowPassFilter.h
File metadata and controls
65 lines (54 loc) · 1.36 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
#pragma once
#include "stdint.h"
/////////// Super Simple Fixed Point Lowpass ////////////////
class LPF
{
public:
int32_t SmoothDataINT;
int32_t SmoothDataFP;
int32_t Beta = 3; // Length = 16
int32_t FP_Shift = 5; //Number of fractional bits
bool NeedReset = true; // wait for the first data to upcoming.
LPF(int Beta_, int FP_Shift_)
{
Beta = Beta_;
FP_Shift = FP_Shift_;
}
LPF(int Beta_)
{
Beta = Beta_;
}
LPF()
{
Beta = 3;
FP_Shift = 5;
}
int32_t ICACHE_RAM_ATTR update(int32_t Indata)
{
if (NeedReset)
{
init(Indata);
return SmoothDataINT;
}
int RawData;
RawData = Indata;
RawData <<= FP_Shift; // Shift to fixed point
SmoothDataFP = (SmoothDataFP << Beta) - SmoothDataFP;
SmoothDataFP += RawData;
SmoothDataFP >>= Beta;
// Don't do the following shift if you want to do further
// calculations in fixed-point using SmoothData
SmoothDataINT = SmoothDataFP >> FP_Shift;
return SmoothDataINT;
}
void ICACHE_RAM_ATTR reset()
{
NeedReset = true;
}
void ICACHE_RAM_ATTR init(int32_t Indata)
{
NeedReset = false;
SmoothDataINT = Indata;
SmoothDataFP = SmoothDataINT << FP_Shift;
}
};