Michael Ernst Peter / PM2_Libary

Libary for PM2.

Dependencies:   LSM9DS1 RangeFinder FastPWM

Dependents:   PM2_Example_PES_board PM2_Example_PES_board PM2_Example_PES_board PM2_Example_PES_board ... more

AvgFilter.cpp@32:bb074bb17d39, 2022-06-22 (annotated)

Committer:
pmic
Date:
Wed Jun 22 14:05:13 2022 +0000
Revision:
32:bb074bb17d39
Parent:
25:51e92f2c89e2 
Updated SensorBar so it is not blocking the program if it is not connected

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pmic 24:f2614d8577a1 1 #include "AvgFilter.h"
pmic 24:f2614d8577a1 2
pmic 24:f2614d8577a1 3 AvgFilter::AvgFilter(uint8_t _N)
pmic 24:f2614d8577a1 4 {
pmic 24:f2614d8577a1 5 setup(_N);
pmic 24:f2614d8577a1 6 }
pmic 24:f2614d8577a1 7
pmic 24:f2614d8577a1 8 AvgFilter::~AvgFilter()
pmic 24:f2614d8577a1 9 {
pmic 24:f2614d8577a1 10 }
pmic 24:f2614d8577a1 11
pmic 24:f2614d8577a1 12 void AvgFilter::setup(uint8_t _N)
pmic 24:f2614d8577a1 13 {
pmic 24:f2614d8577a1 14 m_N = _N;
pmic 24:f2614d8577a1 15 m_ring_buffer = (float*)malloc(m_N*sizeof(float));
pmic 24:f2614d8577a1 16 reset();
pmic 24:f2614d8577a1 17 }
pmic 24:f2614d8577a1 18
pmic 25:51e92f2c89e2 19 void AvgFilter::reset(float _avg)
pmic 25:51e92f2c89e2 20 {
pmic 25:51e92f2c89e2 21 m_avg = _avg;
pmic 25:51e92f2c89e2 22 float scaled_inp = m_avg / (float)m_N;
pmic 25:51e92f2c89e2 23 m_idx = 0;
pmic 25:51e92f2c89e2 24 for(uint8_t i = 0; i < m_N; i++) {
pmic 25:51e92f2c89e2 25 m_ring_buffer[i] = scaled_inp;
pmic 25:51e92f2c89e2 26 }
pmic 25:51e92f2c89e2 27 }
pmic 25:51e92f2c89e2 28
pmic 24:f2614d8577a1 29 void AvgFilter::reset()
pmic 24:f2614d8577a1 30 {
pmic 25:51e92f2c89e2 31 reset(0.0f);
pmic 24:f2614d8577a1 32 }
pmic 24:f2614d8577a1 33
pmic 24:f2614d8577a1 34 float AvgFilter::update(float _inp)
pmic 24:f2614d8577a1 35 {
pmic 24:f2614d8577a1 36 // remove last value from ringbuffer
pmic 24:f2614d8577a1 37 m_avg -= m_ring_buffer[m_idx];
pmic 24:f2614d8577a1 38
pmic 24:f2614d8577a1 39 // add new value and update ringbuffer, rinbuffer stores the scaled value
pmic 24:f2614d8577a1 40 float scaled_inp = _inp / (float)m_N;
pmic 24:f2614d8577a1 41 m_avg += scaled_inp;
pmic 24:f2614d8577a1 42 m_ring_buffer[m_idx] = scaled_inp;
pmic 24:f2614d8577a1 43
pmic 24:f2614d8577a1 44 // check if we are at the end of the ringbuffer
pmic 24:f2614d8577a1 45 m_idx++;
pmic 24:f2614d8577a1 46 if (m_idx == m_N)
pmic 24:f2614d8577a1 47 {
pmic 24:f2614d8577a1 48 m_idx = 0;
pmic 24:f2614d8577a1 49 }
pmic 24:f2614d8577a1 50
pmic 24:f2614d8577a1 51 return m_avg;
pmic 24:f2614d8577a1 52 }