Quentin Mettraux / Mbed OS MAVErIC_TEST

Working Maveric

LowpassFilter.cpp@10:36a2131f636c, 2017-08-22 (annotated)

Committer:
mettrque
Date:
Tue Aug 22 10:49:39 2017 +0000
Revision:
10:36a2131f636c
Parent:
0:bdca5e4773dd 
last military version;

Who changed what in which revision?

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