Zürcher Eliteeinheit / Mbed 2 deprecated ROME5

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:497d7f448d3e, 2018-05-11 (annotated)

Committer:
Appalco
Date:
Fri May 11 12:38:21 2018 +0000
Revision:
0:497d7f448d3e
Importet P5 ROME2; LIDAR

Who changed what in which revision?

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