TeamSurface / Libs

Nim leo niiiim

LowpassFilter.cpp@0:da791f233257, 2018-05-11 (annotated)

Committer:
Kiwicjam
Date:
Fri May 11 12:21:19 2018 +0000
Revision:
0:da791f233257
start of rome2 p5;

Who changed what in which revision?

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