Koen Boulogne / Mbed 2 deprecated Project

Basis aansturing projectgroep 3

Dependencies:   Biquad HIDScope MODSERIAL QEI mbed

BiQuad.cpp@9:e764cb50d343, 2016-11-07 (annotated)

Committer:
s1588141
Date:
Mon Nov 07 13:30:49 2016 +0000
Revision:
9:e764cb50d343
Parent:
5:4b2ff2a4664a 
Final program;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
s1588141 5:4b2ff2a4664a 1 #include "BiQuad.h"
s1588141 5:4b2ff2a4664a 2
s1588141 5:4b2ff2a4664a 3 BiQuad::BiQuad() {
s1588141 5:4b2ff2a4664a 4 resetStateOnGainChange = true;
s1588141 5:4b2ff2a4664a 5 set( 1.0, 0.0, 0.0, 0.0, 0.0 );
s1588141 5:4b2ff2a4664a 6 }
s1588141 5:4b2ff2a4664a 7
s1588141 5:4b2ff2a4664a 8 BiQuad::BiQuad(double b0, double b1, double b2, double a1, double a2) {
s1588141 5:4b2ff2a4664a 9 resetStateOnGainChange = true;
s1588141 5:4b2ff2a4664a 10 set( b0, b1, b2, a1, a2 );
s1588141 5:4b2ff2a4664a 11 }
s1588141 5:4b2ff2a4664a 12
s1588141 5:4b2ff2a4664a 13 BiQuad::BiQuad(double b0, double b1, double b2, double a0, double a1, double a2) {
s1588141 5:4b2ff2a4664a 14 resetStateOnGainChange = true;
s1588141 5:4b2ff2a4664a 15 set( b0/a0, b1/a0, b2/a0, a1/a0, a2/a0 );
s1588141 5:4b2ff2a4664a 16 }
s1588141 5:4b2ff2a4664a 17
s1588141 5:4b2ff2a4664a 18 void BiQuad::PIDF( double Kp, double Ki, double Kd, double N, double Ts ) {
s1588141 5:4b2ff2a4664a 19
s1588141 5:4b2ff2a4664a 20 double b0, b1, b2, bd, a1, a2;
s1588141 5:4b2ff2a4664a 21
s1588141 5:4b2ff2a4664a 22 a1 = -4.0/(N*Ts+2.0);
s1588141 5:4b2ff2a4664a 23 a2 = -(N*Ts-2.0)/(N*Ts+2.0);
s1588141 5:4b2ff2a4664a 24
s1588141 5:4b2ff2a4664a 25 bd = ( N*Ts+2.0 );
s1588141 5:4b2ff2a4664a 26
s1588141 5:4b2ff2a4664a 27 b0 = ( 4.0*Kp + 4.0*Kd*N + 2.0*Ki*Ts + 2.0*Kp*N*Ts + Ki*N*Ts*Ts )/(2.0*bd);
s1588141 5:4b2ff2a4664a 28 b1 = ( Ki*N*Ts*Ts - 4.0*Kp - 4.0*Kd*N )/bd;
s1588141 5:4b2ff2a4664a 29 b2 = ( 4.0*Kp + 4.0*Kd*N - 2*Ki*Ts - 2*Kp*N*Ts + Ki*N*Ts*Ts )/(2.0*bd);
s1588141 5:4b2ff2a4664a 30
s1588141 5:4b2ff2a4664a 31 set( b0, b1, b2, a1, a2 );
s1588141 5:4b2ff2a4664a 32
s1588141 5:4b2ff2a4664a 33 };
s1588141 5:4b2ff2a4664a 34
s1588141 5:4b2ff2a4664a 35 void BiQuad::set(double b0, double b1, double b2, double a1, double a2) {
s1588141 5:4b2ff2a4664a 36
s1588141 5:4b2ff2a4664a 37 B[0] = b0; B[1] = b1; B[2] = b2;
s1588141 5:4b2ff2a4664a 38 A[0] = a1; A[1] = a2;
s1588141 5:4b2ff2a4664a 39
s1588141 5:4b2ff2a4664a 40 if( resetStateOnGainChange )
s1588141 5:4b2ff2a4664a 41 wz[0] = 0; wz[1] = 0;
s1588141 5:4b2ff2a4664a 42
s1588141 5:4b2ff2a4664a 43 }
s1588141 5:4b2ff2a4664a 44
s1588141 5:4b2ff2a4664a 45 double BiQuad::step(double x) {
s1588141 5:4b2ff2a4664a 46
s1588141 5:4b2ff2a4664a 47 double y,w;
s1588141 5:4b2ff2a4664a 48
s1588141 5:4b2ff2a4664a 49 /* Direct form II */
s1588141 5:4b2ff2a4664a 50 w = x - A[0]*wz[0] - A[1]*wz[1];
s1588141 5:4b2ff2a4664a 51 y = B[0]*w + B[1]*wz[0] + B[2]*wz[1];
s1588141 5:4b2ff2a4664a 52
s1588141 5:4b2ff2a4664a 53 /* Shift */
s1588141 5:4b2ff2a4664a 54 wz[1] = wz[0];
s1588141 5:4b2ff2a4664a 55 wz[0] = w;
s1588141 5:4b2ff2a4664a 56
s1588141 5:4b2ff2a4664a 57 return y;
s1588141 5:4b2ff2a4664a 58
s1588141 5:4b2ff2a4664a 59 }
s1588141 5:4b2ff2a4664a 60
s1588141 5:4b2ff2a4664a 61 std::vector< std::complex<double> > BiQuad::poles() {
s1588141 5:4b2ff2a4664a 62
s1588141 5:4b2ff2a4664a 63 std::vector< std::complex<double> > poles;
s1588141 5:4b2ff2a4664a 64
s1588141 5:4b2ff2a4664a 65 std::complex<double> b2(A[0]*A[0],0);
s1588141 5:4b2ff2a4664a 66 std::complex<double> ds = std::sqrt( b2-4*A[1] );
s1588141 5:4b2ff2a4664a 67
s1588141 5:4b2ff2a4664a 68 poles.push_back( 0.5*(-A[0]+ds) );
s1588141 5:4b2ff2a4664a 69 poles.push_back( 0.5*(-A[0]-ds) );
s1588141 5:4b2ff2a4664a 70
s1588141 5:4b2ff2a4664a 71 return poles;
s1588141 5:4b2ff2a4664a 72
s1588141 5:4b2ff2a4664a 73 }
s1588141 5:4b2ff2a4664a 74
s1588141 5:4b2ff2a4664a 75 std::vector< std::complex<double> > BiQuad::zeros() {
s1588141 5:4b2ff2a4664a 76
s1588141 5:4b2ff2a4664a 77 std::vector< std::complex<double> > zeros;
s1588141 5:4b2ff2a4664a 78
s1588141 5:4b2ff2a4664a 79 std::complex<double> b2(B[1]*B[1],0);
s1588141 5:4b2ff2a4664a 80 std::complex<double> ds = std::sqrt( b2-4*B[0]*B[2] );
s1588141 5:4b2ff2a4664a 81
s1588141 5:4b2ff2a4664a 82 zeros.push_back( 0.5*(-B[1]+ds)/B[0] );
s1588141 5:4b2ff2a4664a 83 zeros.push_back( 0.5*(-B[1]-ds)/B[0] );
s1588141 5:4b2ff2a4664a 84
s1588141 5:4b2ff2a4664a 85 return zeros;
s1588141 5:4b2ff2a4664a 86
s1588141 5:4b2ff2a4664a 87 }
s1588141 5:4b2ff2a4664a 88
s1588141 5:4b2ff2a4664a 89 bool BiQuad::stable() {
s1588141 5:4b2ff2a4664a 90 bool stable = true;
s1588141 5:4b2ff2a4664a 91 std::vector< std::complex<double> > ps = poles();
s1588141 5:4b2ff2a4664a 92 for( size_t i = 0; i < ps.size(); i++ )
s1588141 5:4b2ff2a4664a 93 stable = stable & ( std::abs( ps[i] ) < 1 );
s1588141 5:4b2ff2a4664a 94 return stable;
s1588141 5:4b2ff2a4664a 95 }
s1588141 5:4b2ff2a4664a 96
s1588141 5:4b2ff2a4664a 97 void BiQuad::setResetStateOnGainChange( bool v ){
s1588141 5:4b2ff2a4664a 98 resetStateOnGainChange = v;
s1588141 5:4b2ff2a4664a 99 }
s1588141 5:4b2ff2a4664a 100
s1588141 5:4b2ff2a4664a 101 BiQuadChain &BiQuadChain::add(BiQuad *bq) {
s1588141 5:4b2ff2a4664a 102 biquads.push_back( bq );
s1588141 5:4b2ff2a4664a 103 return *this;
s1588141 5:4b2ff2a4664a 104 }
s1588141 5:4b2ff2a4664a 105
s1588141 5:4b2ff2a4664a 106 BiQuadChain operator*( BiQuad &bq1, BiQuad &bq2 ) {
s1588141 5:4b2ff2a4664a 107 BiQuadChain bqc;
s1588141 5:4b2ff2a4664a 108 bqc.add( &bq1 ).add( &bq2 );
s1588141 5:4b2ff2a4664a 109 return bqc;
s1588141 5:4b2ff2a4664a 110 }
s1588141 5:4b2ff2a4664a 111
s1588141 5:4b2ff2a4664a 112 double BiQuadChain::step(double x) {
s1588141 5:4b2ff2a4664a 113
s1588141 5:4b2ff2a4664a 114 int i;
s1588141 5:4b2ff2a4664a 115 size_t bqs;
s1588141 5:4b2ff2a4664a 116
s1588141 5:4b2ff2a4664a 117 bqs = biquads.size();
s1588141 5:4b2ff2a4664a 118
s1588141 5:4b2ff2a4664a 119 for( i = 0; i < bqs; i++ )
s1588141 5:4b2ff2a4664a 120 x = biquads[i]->step( x );
s1588141 5:4b2ff2a4664a 121
s1588141 5:4b2ff2a4664a 122 return x;
s1588141 5:4b2ff2a4664a 123 }
s1588141 5:4b2ff2a4664a 124
s1588141 5:4b2ff2a4664a 125 std::vector< std::complex<double> > BiQuadChain::poles_zeros( bool zeros ) {
s1588141 5:4b2ff2a4664a 126
s1588141 5:4b2ff2a4664a 127 std::vector< std::complex<double> > chain, bq;
s1588141 5:4b2ff2a4664a 128 int i;
s1588141 5:4b2ff2a4664a 129 size_t bqs;
s1588141 5:4b2ff2a4664a 130
s1588141 5:4b2ff2a4664a 131 bqs = biquads.size();
s1588141 5:4b2ff2a4664a 132
s1588141 5:4b2ff2a4664a 133 for( i = 0; i < bqs; i++ ){
s1588141 5:4b2ff2a4664a 134 bq = ( zeros ) ? biquads[ i ]->zeros() : biquads[ i ]->poles();
s1588141 5:4b2ff2a4664a 135 chain.insert( chain.end(), bq.begin(), bq.end() );
s1588141 5:4b2ff2a4664a 136 }
s1588141 5:4b2ff2a4664a 137
s1588141 5:4b2ff2a4664a 138 return chain;
s1588141 5:4b2ff2a4664a 139
s1588141 5:4b2ff2a4664a 140 }
s1588141 5:4b2ff2a4664a 141
s1588141 5:4b2ff2a4664a 142 std::vector< std::complex<double> > BiQuadChain::poles() {
s1588141 5:4b2ff2a4664a 143 return poles_zeros( false );
s1588141 5:4b2ff2a4664a 144 }
s1588141 5:4b2ff2a4664a 145
s1588141 5:4b2ff2a4664a 146 std::vector< std::complex<double> > BiQuadChain::zeros() {
s1588141 5:4b2ff2a4664a 147 return poles_zeros( true );
s1588141 5:4b2ff2a4664a 148 }
s1588141 5:4b2ff2a4664a 149
s1588141 5:4b2ff2a4664a 150 bool BiQuadChain::stable() {
s1588141 5:4b2ff2a4664a 151 bool stable = true;
s1588141 5:4b2ff2a4664a 152 for( size_t i = 0; i < biquads.size(); i++ )
s1588141 5:4b2ff2a4664a 153 stable = stable & biquads[i]->stable();
s1588141 5:4b2ff2a4664a 154 return stable;
s1588141 5:4b2ff2a4664a 155 }
s1588141 5:4b2ff2a4664a 156
s1588141 5:4b2ff2a4664a 157 BiQuadChain& BiQuadChain::operator*( BiQuad& bq ) {
s1588141 5:4b2ff2a4664a 158 add( &bq );
s1588141 5:4b2ff2a4664a 159 return *this;
s1588141 5:4b2ff2a4664a 160 }