Roboshark / Mbed 2 deprecated Roboshark_V3

inkl Line Sensor

Dependencies:   mbed

Fork of Roboshark_V2 by Roboshark

LowpassFilter.cpp@5:e715d157ced5, 2018-04-26 (annotated)

Committer:
ahlervin
Date:
Thu Apr 26 05:58:07 2018 +0000
Revision:
5:e715d157ced5
Parent:
0:6d0671ae4648 
bug fix

Who changed what in which revision?

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