Marco Oehler / Mbed 2 deprecated ROME2

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

LowpassFilter.cpp@0:7ee4c6416e08, 2020-02-24 (annotated)

Committer:
oehlemar
Date:
Mon Feb 24 16:05:50 2020 +0000
Revision:
0:7ee4c6416e08
ROME2 P1

Who changed what in which revision?

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