Marco Oehler / Mbed 2 deprecated Lab2

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:1a972ed770da, 2020-03-09 (annotated)

Committer:
oehlemar
Date:
Mon Mar 09 16:23:04 2020 +0000
Revision:
0:1a972ed770da
LAB2

Who changed what in which revision?

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