mirror of https://github.com/ArduPilot/ardupilot
uncrustify libraries/Filter/FilterWithBuffer.h
This commit is contained in:
parent
b743ed39b0
commit
51346f4790
|
@ -22,22 +22,24 @@
|
||||||
template <class T, uint8_t FILTER_SIZE>
|
template <class T, uint8_t FILTER_SIZE>
|
||||||
class FilterWithBuffer : public Filter<T>
|
class FilterWithBuffer : public Filter<T>
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
// constructor
|
// constructor
|
||||||
FilterWithBuffer();
|
FilterWithBuffer();
|
||||||
|
|
||||||
// apply - Add a new raw value to the filter, retrieve the filtered result
|
// apply - Add a new raw value to the filter, retrieve the filtered result
|
||||||
virtual T apply(T sample);
|
virtual T apply(T sample);
|
||||||
|
|
||||||
// reset - clear the filter
|
// reset - clear the filter
|
||||||
virtual void reset();
|
virtual void reset();
|
||||||
|
|
||||||
// get filter size
|
// get filter size
|
||||||
virtual uint8_t get_filter_size() { return FILTER_SIZE; };
|
virtual uint8_t get_filter_size() {
|
||||||
|
return FILTER_SIZE;
|
||||||
|
};
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
T samples[FILTER_SIZE]; // buffer of samples
|
T samples[FILTER_SIZE]; // buffer of samples
|
||||||
uint8_t sample_index; // pointer to the next empty slot in the buffer
|
uint8_t sample_index; // pointer to the next empty slot in the buffer
|
||||||
};
|
};
|
||||||
|
|
||||||
// Typedef for convenience
|
// Typedef for convenience
|
||||||
|
@ -70,42 +72,42 @@ typedef FilterWithBuffer<uint32_t,7> FilterWithBufferUInt32_Size7;
|
||||||
// Constructor
|
// Constructor
|
||||||
template <class T, uint8_t FILTER_SIZE>
|
template <class T, uint8_t FILTER_SIZE>
|
||||||
FilterWithBuffer<T,FILTER_SIZE>::FilterWithBuffer() :
|
FilterWithBuffer<T,FILTER_SIZE>::FilterWithBuffer() :
|
||||||
Filter<T>(),
|
Filter<T>(),
|
||||||
sample_index(0)
|
sample_index(0)
|
||||||
{
|
{
|
||||||
// clear sample buffer
|
// clear sample buffer
|
||||||
reset();
|
reset();
|
||||||
}
|
}
|
||||||
|
|
||||||
// reset - clear all samples from the buffer
|
// reset - clear all samples from the buffer
|
||||||
template <class T, uint8_t FILTER_SIZE>
|
template <class T, uint8_t FILTER_SIZE>
|
||||||
void FilterWithBuffer<T,FILTER_SIZE>::reset()
|
void FilterWithBuffer<T,FILTER_SIZE>:: reset()
|
||||||
{
|
{
|
||||||
// call base class reset
|
// call base class reset
|
||||||
Filter<T>::reset();
|
Filter<T>::reset();
|
||||||
|
|
||||||
// clear samples buffer
|
// clear samples buffer
|
||||||
for( int8_t i=0; i<FILTER_SIZE; i++ ) {
|
for( int8_t i=0; i<FILTER_SIZE; i++ ) {
|
||||||
samples[i] = 0;
|
samples[i] = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
// reset index back to beginning of the array
|
// reset index back to beginning of the array
|
||||||
sample_index = 0;
|
sample_index = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
// apply - take in a new raw sample, and return the filtered results
|
// apply - take in a new raw sample, and return the filtered results
|
||||||
template <class T, uint8_t FILTER_SIZE>
|
template <class T, uint8_t FILTER_SIZE>
|
||||||
T FilterWithBuffer<T,FILTER_SIZE>::apply(T sample)
|
T FilterWithBuffer<T,FILTER_SIZE>:: apply(T sample)
|
||||||
{
|
{
|
||||||
// add sample to array
|
// add sample to array
|
||||||
samples[sample_index++] = sample;
|
samples[sample_index++] = sample;
|
||||||
|
|
||||||
// wrap index if necessary
|
// wrap index if necessary
|
||||||
if( sample_index >= FILTER_SIZE )
|
if( sample_index >= FILTER_SIZE )
|
||||||
sample_index = 0;
|
sample_index = 0;
|
||||||
|
|
||||||
// base class doesn't know what filtering to do so we just return the raw sample
|
// base class doesn't know what filtering to do so we just return the raw sample
|
||||||
return sample;
|
return sample;
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
Loading…
Reference in New Issue