forked from achillhasler/TFT_eTouch
-
Notifications
You must be signed in to change notification settings - Fork 0
/
TFT_eFirFilter.h
139 lines (127 loc) · 2.94 KB
/
TFT_eFirFilter.h
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
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
#ifndef TFT_E_FIR_FILTER_H
#define TFT_E_FIR_FILTER_H
//
// TFT_eFirFilter.h
//
// (C) Copyright Achill Hasler 2019.
//
// Distributed under the Boost Software License, Version 1.0.
// See accompanying file at https://www.boost.org/LICENSE_1_0.txt
//
//
// See TFT_eTouch/docs/html/index.html for documentation.
//
/**
* @brief fir filter
* @param N number of coefficients, must be even
* @param T used data type
*/
template <uint16_t N = 12, class T = uint16_t>
class FirFilter
{
public:
FirFilter(uint8_t filter = 1)
: divisor_(0)
, filled_(0)
{
calc_q(filter);
}
void calc_q(uint8_t filter)
{
// calculate q for Hamming window
// w[n] = 0.54 - 0.46·cos(2·pi·n/N)
// w[n] = 0.5·[1-cos(2·pi·n/N)] Hanning window
// w[n] = 0.42 - 0.5·cos(2·pi·n/N) + 0.08·cos(4·pi·n/N) Blackmann window
for (uint16_t i = 0; i < N/2+1; i++) {
uint8_t qi;
switch (filter) {
case 1: // Hamming window
qi = (uint8_t)((0.54 - 0.46 * cos(2 * PI * i / N)) * 255);
break;
case 2: // Hanning window
qi = (uint8_t)((0.5 * (1 - cos(2 * PI * i / N))) * 255);
break;
case 3: // Blackmann window
qi = (uint8_t)((0.42 - 0.5 * cos(2 * PI * i / N) + 0.08 * cos(4 * PI * i / N)) * 255);
break;
default:
qi = 0;
}
divisor_ += 2*qi;
q_[i] = qi;
#ifdef TOUCH_SERIAL_DEBUG
if (Serial) {
Serial.printf("q[%i]: %i\n", i, q_[i]);
}
#endif
}
divisor_ -= 255; // q[N/2+1] only once
if (N <= 8)
divisor_ = divisor_ * 1000/1024; // some adjustment N=6
else if (N <= 14)
divisor_ = divisor_ * 1011/1024; // some adjustment N=12
else if (N <= 26)
divisor_ = divisor_ * 1017/1024; // some adjustment N=20
else
divisor_ = divisor_ * 1020/1024; // some adjustment N=40
#ifdef TOUCH_SERIAL_DEBUG
if (Serial) {
Serial.printf("divisor: %i\n", divisor_);
}
#endif
reset();
}
T next(T val)
{
T ret = 0;
buffer_[act_] = val;
if (filled_ < N) {
filled_++;
if (filled_ < N) {
act_++;
return ret;
}
#ifdef TOUCH_SERIAL_DEBUG
else {
if (Serial) {
Serial.print("q: ");
for (uint16_t i = 0; i < N/2; i++) {
Serial.printf("%i, ", q_[i]);
}
Serial.println(q_[N/2]);
}
}
#endif
}
uint32_t sum = 0;
for (uint16_t i = 0; i < N; i++) {
uint16_t pos_q = i < N/2+1 ? i : N - i;
uint16_t pos_b = (N + act_ - i) % N;
sum += buffer_[pos_b] * q_[pos_q];
}
ret = sum / divisor_;
act_++;
if (act_ >= N) act_ = 0;
return ret;
}
inline void reset()
{
filled_ = 0;
act_ = 0;
}
inline uint16_t size() const
{
return N;
}
inline uint16_t filled() const
{
return filled_;
}
private:
T buffer_[N];
uint8_t q_[N/2+1];
uint32_t divisor_;
uint16_t filled_;
uint16_t act_;
};
#endif // TFT_E_FIR_FILTER_H