Nicolas Borla / Mbed 2 deprecated Example_for_Joel

IMU measurement + Speed controller

Dependencies:   mbed

LowpassFilter.cpp@1:17fdd812cb8d, 2019-05-30 (annotated)

Committer:
boro
Date:
Thu May 30 13:21:44 2019 +0000
Revision:
1:17fdd812cb8d
Parent:
0:5a93e4916fb1 
a

Who changed what in which revision?

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