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