kings / Mbed 2 deprecated Bewegungen_Encoder

Encoder

Dependencies:   mbed

Fork of Bewegungen by kings

LowpassFilter.cpp@1:e454e6f5d81a, 2017-05-10 (annotated)

Committer:
EHess
Date:
Wed May 10 09:15:42 2017 +0000
Revision:
1:e454e6f5d81a
wd

Who changed what in which revision?

UserRevisionLine numberNew contents of line
EHess 1:e454e6f5d81a 1 #include "LowpassFilter.h"
EHess 1:e454e6f5d81a 2
EHess 1:e454e6f5d81a 3 //E. Hess
EHess 1:e454e6f5d81a 4 //LowpassFilter.cpp
EHess 1:e454e6f5d81a 5
EHess 1:e454e6f5d81a 6 using namespace std;
EHess 1:e454e6f5d81a 7
EHess 1:e454e6f5d81a 8 /**
EHess 1:e454e6f5d81a 9 * Creates a LowpassFilter object with a default corner frequency of 1000 [rad/s].
EHess 1:e454e6f5d81a 10 */
EHess 1:e454e6f5d81a 11 LowpassFilter::LowpassFilter() {
EHess 1:e454e6f5d81a 12
EHess 1:e454e6f5d81a 13 period = 1.0f;
EHess 1:e454e6f5d81a 14 frequency = 1000.0f;
EHess 1:e454e6f5d81a 15
EHess 1:e454e6f5d81a 16 a11 = (1.0f+frequency*period)*exp(-frequency*period);
EHess 1:e454e6f5d81a 17 a12 = period*exp(-frequency*period);
EHess 1:e454e6f5d81a 18 a21 = -frequency*frequency*period*exp(-frequency*period);
EHess 1:e454e6f5d81a 19 a22 = (1.0f-frequency*period)*exp(-frequency*period);
EHess 1:e454e6f5d81a 20 b1 = (1.0f-(1.0f+frequency*period)*exp(-frequency*period))/frequency/frequency;
EHess 1:e454e6f5d81a 21 b2 = period*exp(-frequency*period);
EHess 1:e454e6f5d81a 22
EHess 1:e454e6f5d81a 23 x1 = 0.0f;
EHess 1:e454e6f5d81a 24 x2 = 0.0f;
EHess 1:e454e6f5d81a 25 }
EHess 1:e454e6f5d81a 26
EHess 1:e454e6f5d81a 27 /**
EHess 1:e454e6f5d81a 28 * Deletes the LowpassFilter object.
EHess 1:e454e6f5d81a 29 */
EHess 1:e454e6f5d81a 30 LowpassFilter::~LowpassFilter() {}
EHess 1:e454e6f5d81a 31
EHess 1:e454e6f5d81a 32 /**
EHess 1:e454e6f5d81a 33 * Resets the filtered value to zero.
EHess 1:e454e6f5d81a 34 */
EHess 1:e454e6f5d81a 35 void LowpassFilter::reset() {
EHess 1:e454e6f5d81a 36
EHess 1:e454e6f5d81a 37 x1 = 0.0f;
EHess 1:e454e6f5d81a 38 x2 = 0.0f;
EHess 1:e454e6f5d81a 39 }
EHess 1:e454e6f5d81a 40
EHess 1:e454e6f5d81a 41 /**
EHess 1:e454e6f5d81a 42 * Resets the filtered value to a given value.
EHess 1:e454e6f5d81a 43 * @param value the value to reset the filter to.
EHess 1:e454e6f5d81a 44 */
EHess 1:e454e6f5d81a 45 void LowpassFilter::reset(float value) {
EHess 1:e454e6f5d81a 46
EHess 1:e454e6f5d81a 47 x1 = value/frequency/frequency;
EHess 1:e454e6f5d81a 48 x2 = (x1-a11*x1-b1*value)/a12;
EHess 1:e454e6f5d81a 49 }
EHess 1:e454e6f5d81a 50
EHess 1:e454e6f5d81a 51 /**
EHess 1:e454e6f5d81a 52 * Sets the sampling period of the filter.
EHess 1:e454e6f5d81a 53 * This is typically the sampling period of the realtime thread of a controller that uses this filter.
EHess 1:e454e6f5d81a 54 * @param the sampling period, given in [s].
EHess 1:e454e6f5d81a 55 */
EHess 1:e454e6f5d81a 56 void LowpassFilter::setPeriod(float period) {
EHess 1:e454e6f5d81a 57
EHess 1:e454e6f5d81a 58 this->period = period;
EHess 1:e454e6f5d81a 59
EHess 1:e454e6f5d81a 60 a11 = (1.0f+frequency*period)*exp(-frequency*period);
EHess 1:e454e6f5d81a 61 a12 = period*exp(-frequency*period);
EHess 1:e454e6f5d81a 62 a21 = -frequency*frequency*period*exp(-frequency*period);
EHess 1:e454e6f5d81a 63 a22 = (1.0f-frequency*period)*exp(-frequency*period);
EHess 1:e454e6f5d81a 64 b1 = (1.0f-(1.0f+frequency*period)*exp(-frequency*period))/frequency/frequency;
EHess 1:e454e6f5d81a 65 b2 = period*exp(-frequency*period);
EHess 1:e454e6f5d81a 66 }
EHess 1:e454e6f5d81a 67
EHess 1:e454e6f5d81a 68 /**
EHess 1:e454e6f5d81a 69 * Sets the corner frequency of this filter.
EHess 1:e454e6f5d81a 70 * @param frequency the corner frequency of the filter in [rad/s].
EHess 1:e454e6f5d81a 71 */
EHess 1:e454e6f5d81a 72 void LowpassFilter::setFrequency(float frequency) {
EHess 1:e454e6f5d81a 73
EHess 1:e454e6f5d81a 74 this->frequency = frequency;
EHess 1:e454e6f5d81a 75
EHess 1:e454e6f5d81a 76 a11 = (1.0f+frequency*period)*exp(-frequency*period);
EHess 1:e454e6f5d81a 77 a12 = period*exp(-frequency*period);
EHess 1:e454e6f5d81a 78 a21 = -frequency*frequency*period*exp(-frequency*period);
EHess 1:e454e6f5d81a 79 a22 = (1.0f-frequency*period)*exp(-frequency*period);
EHess 1:e454e6f5d81a 80 b1 = (1.0f-(1.0f+frequency*period)*exp(-frequency*period))/frequency/frequency;
EHess 1:e454e6f5d81a 81 b2 = period*exp(-frequency*period);
EHess 1:e454e6f5d81a 82 }
EHess 1:e454e6f5d81a 83
EHess 1:e454e6f5d81a 84 /**
EHess 1:e454e6f5d81a 85 * Gets the current corner frequency of this filter.
EHess 1:e454e6f5d81a 86 * @return the current corner frequency in [rad/s].
EHess 1:e454e6f5d81a 87 */
EHess 1:e454e6f5d81a 88 float LowpassFilter::getFrequency() {
EHess 1:e454e6f5d81a 89
EHess 1:e454e6f5d81a 90 return frequency;
EHess 1:e454e6f5d81a 91 }
EHess 1:e454e6f5d81a 92
EHess 1:e454e6f5d81a 93 /**
EHess 1:e454e6f5d81a 94 * Filters a value.
EHess 1:e454e6f5d81a 95 * @param value the original unfiltered value.
EHess 1:e454e6f5d81a 96 * @return the filtered value.
EHess 1:e454e6f5d81a 97 */
EHess 1:e454e6f5d81a 98 float LowpassFilter::filter(float value) {
EHess 1:e454e6f5d81a 99
EHess 1:e454e6f5d81a 100 float x1old = x1;
EHess 1:e454e6f5d81a 101 float x2old = x2;
EHess 1:e454e6f5d81a 102
EHess 1:e454e6f5d81a 103 x1 = a11*x1old+a12*x2old+b1*value;
EHess 1:e454e6f5d81a 104 x2 = a21*x1old+a22*x2old+b2*value;
EHess 1:e454e6f5d81a 105
EHess 1:e454e6f5d81a 106 return frequency*frequency*x1;
EHess 1:e454e6f5d81a 107 }