Nicolas Borla / Mbed OS PES2_mbed_os

PES2_mbed_os_6

Dependencies:   Servo

LowpassFilter.cpp@0:5d4d21d56334, 2021-03-12 (annotated)

Committer:
boro
Date:
Fri Mar 12 13:04:33 2021 +0000
Revision:
0:5d4d21d56334
controller added;

Who changed what in which revision?

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