AlbaniGang / Mbed 2 deprecated Rome_P3

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@1:bba0ec7e075a, 2020-04-08 (annotated)

Committer:
stollpa1
Date:
Wed Apr 08 09:13:33 2020 +0000
Revision:
1:bba0ec7e075a
Parent:
0:0a667cdbf4c1 
P4 init;

Who changed what in which revision?

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