Allan Green
/
touchpad
..
filters.cpp@0:ff5187998c84, 2013-02-01 (annotated)
- Committer:
- allanalpha
- Date:
- Fri Feb 01 15:24:15 2013 +0000
- Revision:
- 0:ff5187998c84
what ev
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
allanalpha | 0:ff5187998c84 | 1 | /* |
allanalpha | 0:ff5187998c84 | 2 | mbed touchpad & accelerometer experiments. |
allanalpha | 0:ff5187998c84 | 3 | |
allanalpha | 0:ff5187998c84 | 4 | CPV, 14/09/2009 |
allanalpha | 0:ff5187998c84 | 5 | */ |
allanalpha | 0:ff5187998c84 | 6 | |
allanalpha | 0:ff5187998c84 | 7 | |
allanalpha | 0:ff5187998c84 | 8 | #include <string.h> |
allanalpha | 0:ff5187998c84 | 9 | #include "filters.h" |
allanalpha | 0:ff5187998c84 | 10 | |
allanalpha | 0:ff5187998c84 | 11 | |
allanalpha | 0:ff5187998c84 | 12 | void FilterBoxF::initialise(unsigned int size) |
allanalpha | 0:ff5187998c84 | 13 | { |
allanalpha | 0:ff5187998c84 | 14 | m_size = size; |
allanalpha | 0:ff5187998c84 | 15 | if (m_size>FILTER_MAX_SIZE) m_size = FILTER_MAX_SIZE; |
allanalpha | 0:ff5187998c84 | 16 | memset(m_history,0,sizeof(float)*FILTER_MAX_SIZE); |
allanalpha | 0:ff5187998c84 | 17 | m_index = 0; |
allanalpha | 0:ff5187998c84 | 18 | m_out = 0; |
allanalpha | 0:ff5187998c84 | 19 | } |
allanalpha | 0:ff5187998c84 | 20 | |
allanalpha | 0:ff5187998c84 | 21 | |
allanalpha | 0:ff5187998c84 | 22 | float FilterBoxF::tick(float in) |
allanalpha | 0:ff5187998c84 | 23 | { |
allanalpha | 0:ff5187998c84 | 24 | m_history[m_index] = in; |
allanalpha | 0:ff5187998c84 | 25 | m_index += 1; |
allanalpha | 0:ff5187998c84 | 26 | if (m_index>=m_size) m_index = 0; |
allanalpha | 0:ff5187998c84 | 27 | |
allanalpha | 0:ff5187998c84 | 28 | float sum = 0; |
allanalpha | 0:ff5187998c84 | 29 | for (int i=0; i<m_size; i++) |
allanalpha | 0:ff5187998c84 | 30 | { |
allanalpha | 0:ff5187998c84 | 31 | sum += m_history[i]; |
allanalpha | 0:ff5187998c84 | 32 | } |
allanalpha | 0:ff5187998c84 | 33 | m_out = sum/m_size; |
allanalpha | 0:ff5187998c84 | 34 | return m_out; |
allanalpha | 0:ff5187998c84 | 35 | } |
allanalpha | 0:ff5187998c84 | 36 | |
allanalpha | 0:ff5187998c84 | 37 | //================================= |
allanalpha | 0:ff5187998c84 | 38 | |
allanalpha | 0:ff5187998c84 | 39 | void FilterBoxI::initialise(unsigned int size) |
allanalpha | 0:ff5187998c84 | 40 | { |
allanalpha | 0:ff5187998c84 | 41 | m_size = size; |
allanalpha | 0:ff5187998c84 | 42 | if (m_size>FILTER_MAX_SIZE) m_size = FILTER_MAX_SIZE; |
allanalpha | 0:ff5187998c84 | 43 | memset(m_history,0,sizeof(int)*FILTER_MAX_SIZE); |
allanalpha | 0:ff5187998c84 | 44 | m_index = 0; |
allanalpha | 0:ff5187998c84 | 45 | m_out = 0; |
allanalpha | 0:ff5187998c84 | 46 | } |
allanalpha | 0:ff5187998c84 | 47 | |
allanalpha | 0:ff5187998c84 | 48 | |
allanalpha | 0:ff5187998c84 | 49 | int FilterBoxI::tick(int in) |
allanalpha | 0:ff5187998c84 | 50 | { |
allanalpha | 0:ff5187998c84 | 51 | m_history[m_index] = in; |
allanalpha | 0:ff5187998c84 | 52 | m_index += 1; |
allanalpha | 0:ff5187998c84 | 53 | if (m_index>=m_size) m_index = 0; |
allanalpha | 0:ff5187998c84 | 54 | |
allanalpha | 0:ff5187998c84 | 55 | int sum = 0; |
allanalpha | 0:ff5187998c84 | 56 | for (int i=0; i<m_size; i++) |
allanalpha | 0:ff5187998c84 | 57 | { |
allanalpha | 0:ff5187998c84 | 58 | sum += m_history[i]; |
allanalpha | 0:ff5187998c84 | 59 | } |
allanalpha | 0:ff5187998c84 | 60 | m_out = sum/m_size; |
allanalpha | 0:ff5187998c84 | 61 | return m_out; |
allanalpha | 0:ff5187998c84 | 62 | } |
allanalpha | 0:ff5187998c84 | 63 | |
allanalpha | 0:ff5187998c84 | 64 | //================================= |
allanalpha | 0:ff5187998c84 | 65 | |
allanalpha | 0:ff5187998c84 | 66 | void FilterIirI::initialise(int tau, int limit) |
allanalpha | 0:ff5187998c84 | 67 | { |
allanalpha | 0:ff5187998c84 | 68 | m_tau = tau; |
allanalpha | 0:ff5187998c84 | 69 | m_limit = limit; |
allanalpha | 0:ff5187998c84 | 70 | m_remainder = 0; |
allanalpha | 0:ff5187998c84 | 71 | m_out = 0; |
allanalpha | 0:ff5187998c84 | 72 | } |
allanalpha | 0:ff5187998c84 | 73 | |
allanalpha | 0:ff5187998c84 | 74 | |
allanalpha | 0:ff5187998c84 | 75 | int FilterIirI::tick(int in) |
allanalpha | 0:ff5187998c84 | 76 | { |
allanalpha | 0:ff5187998c84 | 77 | int temp = in - m_out + m_remainder; |
allanalpha | 0:ff5187998c84 | 78 | m_remainder = temp % m_tau; |
allanalpha | 0:ff5187998c84 | 79 | m_out += temp / m_tau; |
allanalpha | 0:ff5187998c84 | 80 | if (m_out>m_limit) m_out = m_limit; |
allanalpha | 0:ff5187998c84 | 81 | if (m_out<-m_limit) m_out = -m_limit; |
allanalpha | 0:ff5187998c84 | 82 | return m_out; |
allanalpha | 0:ff5187998c84 | 83 | } |
allanalpha | 0:ff5187998c84 | 84 |