BA / Mbed OS BaBoRo1

Erste version der Software für der Prototyp

LowpassFilter.cpp@4:75df35ef4fb6, 2018-03-30 (annotated)

Committer:
borlanic
Date:
Fri Mar 30 14:07:05 2018 +0000
Revision:
4:75df35ef4fb6
Parent:
0:380207fcb5c1 
commentar

Who changed what in which revision?

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