Furkan Cinar / PM2_Libary

Workshop 2

Dependencies:   FastPWM

LowpassFilter.cpp@4:9c003c402033, 2021-04-06 (annotated)

Committer:
pmic
Date:
Tue Apr 06 11:21:54 2021 +0000
Revision:
4:9c003c402033
Parent:
0:86129f1b4a93 
Update Libary via copy+past, no idea why it is not working via mbed studio.

Who changed what in which revision?

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