Roboshark / Mbed 2 deprecated MyClass

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

LowpassFilter.cpp@0:af3f2e5c9cd4, 2018-04-19 (annotated)

Committer:
fluckmi1
Date:
Thu Apr 19 11:53:52 2018 +0000
Revision:
0:af3f2e5c9cd4
peace

Who changed what in which revision?

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