Patrick Ruesi
/
Micromouse_alpha_copy_copy
Version12.04.18
Fork of Micromouse_beta by
Diff: LowpassFilter.cpp
- Revision:
- 1:d9e840c48b1e
- Parent:
- 0:a9fe4ef404bf
- Child:
- 2:592f01278db4
--- a/LowpassFilter.cpp Wed Mar 07 14:06:19 2018 +0000 +++ b/LowpassFilter.cpp Sat Mar 31 16:45:57 2018 +0000 @@ -1,8 +1,8 @@ -/* - * LowpassFilter.cpp - * Copyright (c) 2018, ZHAW - * All rights reserved. - */ + +// LowpassFilter.cpp +// Copyright (c) 2018, ZHAW +// All rights reserved. + #include <cmath> #include "LowpassFilter.h" @@ -12,18 +12,19 @@ /** * Creates a LowpassFilter object with a default cutoff frequency of 1000 [rad/s]. */ -LowpassFilter::LowpassFilter() { - +LowpassFilter::LowpassFilter() +{ + period = 1.0f; frequency = 1000.0f; - + a11 = (1.0f+frequency*period)*exp(-frequency*period); a12 = period*exp(-frequency*period); a21 = -frequency*frequency*period*exp(-frequency*period); a22 = (1.0f-frequency*period)*exp(-frequency*period); b1 = (1.0f-(1.0f+frequency*period)*exp(-frequency*period))/frequency/frequency; b2 = period*exp(-frequency*period); - + x1 = 0.0f; x2 = 0.0f; } @@ -36,8 +37,9 @@ /** * Resets the filtered value to zero. */ -void LowpassFilter::reset() { - +void LowpassFilter::reset() +{ + x1 = 0.0f; x2 = 0.0f; } @@ -46,8 +48,9 @@ * Resets the filtered value to a given value. * @param value the value to reset the filter to. */ -void LowpassFilter::reset(float value) { - +void LowpassFilter::reset(float value) +{ + x1 = value/frequency/frequency; x2 = (x1-a11*x1-b1*value)/a12; } @@ -57,10 +60,11 @@ * This is typically the sampling period of the periodic task of a controller that uses this filter. * @param the sampling period, given in [s]. */ -void LowpassFilter::setPeriod(float period) { - +void LowpassFilter::setPeriod(float period) +{ + this->period = period; - + a11 = (1.0f+frequency*period)*exp(-frequency*period); a12 = period*exp(-frequency*period); a21 = -frequency*frequency*period*exp(-frequency*period); @@ -73,10 +77,11 @@ * Sets the cutoff frequency of this filter. * @param frequency the cutoff frequency of the filter in [rad/s]. */ -void LowpassFilter::setFrequency(float frequency) { - +void LowpassFilter::setFrequency(float frequency) +{ + this->frequency = frequency; - + a11 = (1.0f+frequency*period)*exp(-frequency*period); a12 = period*exp(-frequency*period); a21 = -frequency*frequency*period*exp(-frequency*period); @@ -89,8 +94,9 @@ * Gets the current cutoff frequency of this filter. * @return the current cutoff frequency in [rad/s]. */ -float LowpassFilter::getFrequency() { - +float LowpassFilter::getFrequency() +{ + return frequency; } @@ -99,13 +105,14 @@ * @param value the original unfiltered value. * @return the filtered value. */ -float LowpassFilter::filter(float value) { +float LowpassFilter::filter(float value) +{ float x1old = x1; float x2old = x2; - + x1 = a11*x1old+a12*x2old+b1*value; x2 = a21*x1old+a22*x2old+b2*value; - + return frequency*frequency*x1; }