Zürcher Eliteeinheit / Mbed 2 deprecated ROME2_P6

rome2_p6 imported

Dependencies:   mbed

LowpassFilter.cpp@5:957580f33e52, 2018-05-18 (annotated)

Committer:
Appalco
Date:
Fri May 18 13:54:25 2018 +0000
Revision:
5:957580f33e52
Parent:
0:351a2fb21235 
fixed tolerance and wayponts

Who changed what in which revision?

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