Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 :
16 : //
17 : /// @file AverageFilter.h
18 : /// @brief A class to provide the average of a number of samples
19 : #pragma once
20 :
21 : #include "FilterClass.h"
22 : #include "FilterWithBuffer.h"
23 :
24 : // 1st parameter <T> is the type of data being filtered.
25 : // 2nd parameter <U> is a larger data type used during summation to prevent overflows
26 : // 3rd parameter <FILTER_SIZE> is the number of elements in the filter
27 : template <class T, class U, uint8_t FILTER_SIZE>
28 : class AverageFilter : public FilterWithBuffer<T,FILTER_SIZE>
29 : {
30 : public:
31 : // constructor
32 2082 : AverageFilter() : FilterWithBuffer<T,FILTER_SIZE>(), _num_samples(0) {
33 2082 : };
34 :
35 : // apply - Add a new raw value to the filter, retrieve the filtered result
36 : virtual T apply(T sample) override;
37 :
38 : // reset - clear the filter
39 : virtual void reset() override;
40 :
41 : protected:
42 : // the number of samples in the filter, maxes out at size of the filter
43 : uint8_t _num_samples;
44 : };
45 :
46 : // Typedef for convenience (1st argument is the data type, 2nd is a larger datatype to handle overflows, 3rd is buffer size)
47 : typedef AverageFilter<int8_t,int16_t,2> AverageFilterInt8_Size2;
48 : typedef AverageFilter<int8_t,int16_t,3> AverageFilterInt8_Size3;
49 : typedef AverageFilter<int8_t,int16_t,4> AverageFilterInt8_Size4;
50 : typedef AverageFilter<int8_t,int16_t,5> AverageFilterInt8_Size5;
51 : typedef AverageFilter<uint8_t,uint16_t,2> AverageFilterUInt8_Size2;
52 : typedef AverageFilter<uint8_t,uint16_t,3> AverageFilterUInt8_Size3;
53 : typedef AverageFilter<uint8_t,uint16_t,4> AverageFilterUInt8_Size4;
54 : typedef AverageFilter<uint8_t,uint16_t,5> AverageFilterUInt8_Size5;
55 :
56 : typedef AverageFilter<int16_t,int32_t,2> AverageFilterInt16_Size2;
57 : typedef AverageFilter<int16_t,int32_t,3> AverageFilterInt16_Size3;
58 : typedef AverageFilter<int16_t,int32_t,4> AverageFilterInt16_Size4;
59 : typedef AverageFilter<int16_t,int32_t,5> AverageFilterInt16_Size5;
60 : typedef AverageFilter<uint16_t,uint32_t,2> AverageFilterUInt16_Size2;
61 : typedef AverageFilter<uint16_t,uint32_t,3> AverageFilterUInt16_Size3;
62 : typedef AverageFilter<uint16_t,uint32_t,4> AverageFilterUInt16_Size4;
63 : typedef AverageFilter<uint16_t,uint32_t,5> AverageFilterUInt16_Size5;
64 :
65 : typedef AverageFilter<int32_t,float,2> AverageFilterInt32_Size2;
66 : typedef AverageFilter<int32_t,float,3> AverageFilterInt32_Size3;
67 : typedef AverageFilter<int32_t,float,4> AverageFilterInt32_Size4;
68 : typedef AverageFilter<int32_t,float,5> AverageFilterInt32_Size5;
69 : typedef AverageFilter<uint32_t,float,2> AverageFilterUInt32_Size2;
70 : typedef AverageFilter<uint32_t,float,3> AverageFilterUInt32_Size3;
71 : typedef AverageFilter<uint32_t,float,4> AverageFilterUInt32_Size4;
72 : typedef AverageFilter<uint32_t,float,5> AverageFilterUInt32_Size5;
73 :
74 : typedef AverageFilter<float,float,5> AverageFilterFloat_Size5;
75 :
76 : // Public Methods //////////////////////////////////////////////////////////////
77 :
78 : template <class T, class U, uint8_t FILTER_SIZE>
79 1794492 : T AverageFilter<T,U,FILTER_SIZE>:: apply(T sample)
80 : {
81 1794492 : U result = 0;
82 :
83 : // call parent's apply function to get the sample into the array
84 1794492 : FilterWithBuffer<T,FILTER_SIZE>::apply(sample);
85 :
86 : // increment the number of samples so far
87 1794492 : _num_samples++;
88 1794492 : if( _num_samples > FILTER_SIZE || _num_samples == 0 )
89 1792574 : _num_samples = FILTER_SIZE;
90 :
91 : // get sum of all values - there is a risk of overflow here that we ignore
92 10766936 : for(uint8_t i=0; i<FILTER_SIZE; i++)
93 8972444 : result += FilterWithBuffer<T,FILTER_SIZE>::samples[i];
94 :
95 1794492 : return (T)(result / _num_samples);
96 : }
97 :
98 : // reset - clear all samples
99 : template <class T, class U, uint8_t FILTER_SIZE>
100 740 : void AverageFilter<T,U,FILTER_SIZE>:: reset()
101 : {
102 : // call parent's apply function to get the sample into the array
103 740 : FilterWithBuffer<T,FILTER_SIZE>::reset();
104 :
105 : // clear our variable
106 740 : _num_samples = 0;
107 740 : }
108 :
109 : /*
110 : * This filter is intended to be used with integral types to be faster and
111 : * avoid loss of precision on floating point arithmetic. The integral type
112 : * chosen must be one that fits FILTER_SIZE values you are filtering.
113 : *
114 : * Differently from other average filters, the result is only returned when
115 : * getf()/getd() is called
116 : */
117 : template <class T, class U, uint8_t FILTER_SIZE>
118 : class AverageIntegralFilter : public AverageFilter<T,U,FILTER_SIZE>
119 : {
120 : public:
121 : /*
122 : * Add a new raw value to the filter: method signature is maintained from
123 : * AverageFilter, but it doesn't retrieve the filtered value: return value
124 : * is always 0. Call getf()/getd() in order to get the filtered value.
125 : */
126 : virtual T apply(T sample) override;
127 :
128 : // get the current value as a float
129 : virtual float getf();
130 :
131 : // get the current value as a double
132 : virtual double getd();
133 : protected:
134 : // the current sum of samples
135 : U _sum = 0;
136 : };
137 :
138 : template <class T, class U, uint8_t FILTER_SIZE>
139 12 : T AverageIntegralFilter<T,U,FILTER_SIZE>::apply(T sample)
140 : {
141 12 : T curr = this->samples[this->sample_index];
142 :
143 : // call parent's parent apply function to get the sample into the array
144 12 : FilterWithBuffer<T,FILTER_SIZE>::apply(sample);
145 :
146 : // increment the number of samples so far
147 12 : this->_num_samples++;
148 12 : if (this->_num_samples > FILTER_SIZE || this->_num_samples == 0) {
149 2 : this->_num_samples = FILTER_SIZE;
150 : }
151 :
152 12 : _sum -= curr;
153 12 : _sum += sample;
154 :
155 : // don't return the value: caller is forced to call getf() or getd()
156 12 : return 0;
157 : }
158 :
159 : template <class T, class U, uint8_t FILTER_SIZE>
160 14 : float AverageIntegralFilter<T,U,FILTER_SIZE>::getf()
161 : {
162 14 : if (this->_num_samples == 0) {
163 2 : return 0.f;
164 : }
165 :
166 12 : return (float)_sum / this->_num_samples;
167 : }
168 :
169 : template <class T, class U, uint8_t FILTER_SIZE>
170 4 : double AverageIntegralFilter<T,U,FILTER_SIZE>::getd()
171 : {
172 4 : if (this->_num_samples == 0) {
173 2 : return 0.f;
174 : }
175 :
176 2 : return (double)_sum / this->_num_samples;
177 : }
|
