zhaw_st16b_pes2_10 / Mbed 2 deprecated SpeedCtrl

a

Dependencies:   mbed

LowpassFilter.cpp@0:dfea4e0e064b, 2017-05-22 (annotated)

Committer:
beacon
Date:
Mon May 22 10:45:28 2017 +0000
Revision:
0:dfea4e0e064b
k

Who changed what in which revision?

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