ROME2 - TI / Mbed 2 deprecated ROME2 - Praktikum

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:08ca9b208045, 2017-03-31 (annotated)

Committer:
solcager
Date:
Fri Mar 31 11:00:19 2017 +0000
Revision:
1:08ca9b208045
P3

Who changed what in which revision?

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