00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00028 #ifndef FILTERANDROTATIONS_H
00029 #define FILTERANDROTATIONS_H
00030
00032 #ifndef LIBRARY_BUILD
00033 #ifndef DTDIV2
00034 #error DTDIV2 (sampling rate divide by 2) MUST be defined to use this header
00035 #endif
00036 #endif
00037
00038 #include "Constants.h"
00039
00041
00052 #define ROTATE_POS(in,out,t) out.q[N]=_IQmpy(in.q[N],_IQcos(t))-_IQmpy(in.d[N],_IQsin(t));out.d[N]=_IQmpy(in.q[N],_IQsin(t))+_IQmpy(in.d[N],_IQcos(t));
00053
00064 #define ROTATE_NEG(in,out,t) out.q[N]=_IQmpy(in.q[N],_IQcos(t))+_IQmpy(in.d[N],_IQsin(t));out.d[N]=-_IQmpy(in.q[N],_IQsin(t))+_IQmpy(in.d[N],_IQcos(t));
00065
00066
00068
00079 #define SROTATE_POS(in,out,t) out.q=_IQmpy(in.q,_IQcos(t))-_IQmpy(in.d,_IQsin(t));out.d=_IQmpy(in.q,_IQsin(t))+_IQmpy(in.d,_IQcos(t));
00080
00091 #define SROTATE_NEG(in,out,t) out.q=_IQmpy(in.q,_IQcos(t))+_IQmpy(in.d,_IQsin(t));out.d=-_IQmpy(in.q,_IQsin(t))+_IQmpy(in.d,_IQcos(t));
00092
00093
00095
00107 #define ROTATE_POS_ESC(in,out,t) out.q[N]=_IQmpy(in,_IQcos(t));out.d[N]=_IQmpy(in,_IQsin(t));
00108
00120 #define ROTATE_NEG_ESC(in,out,t) out.q[N]=_IQmpy(in,_IQcos(t));out.d[N]=-_IQmpy(in,_IQsin(t));
00121
00126 #define SROTATE_POS_ESC(in,out,t) out.q=_IQmpy(in,_IQcos(t));out.d=_IQmpy(in,_IQsin(t));
00127
00132 #define SROTATE_NEG_ESC(in,out,t) out.q=_IQmpy(in,_IQcos(t));out.d=-_IQmpy(in,_IQsin(t));
00133
00134
00136
00141 #define PI_REG(in,out,Kp,Ki) out[N]=out[N1]+_IQmpy(Kp,_IQmpy(in[N],(_IQ(1)+_IQmpy(DTDIV2,Ki))) + _IQmpy(in[N1],(_IQmpy(DTDIV2,Ki)-_IQ(1))))
00142
00147 #define PI_QD_REG(in,out,Kp,Ki) PI_REG(in.q,out.q,Kp,Ki); PI_REG(in.d,out.d,Kp,Ki)
00148
00153 #define PI_QD_REG_NOSYM(in,out,Kp_q,Ki_q,Kp_d,Ki_d) PI_REG(in.q,out.q,Kp_q,Ki_q); PI_REG(in.d,out.d,Kp_d,Ki_d)
00154
00159 #define PI_REG_DT(in,out,Kp,Ki,dTDiv2) out[N]=out[N1]+_IQmpy(Kp,_IQmpy(in[N],(_IQ(1)+_IQmpy(dTDiv2,Ki))) + _IQmpy(in[N1],(_IQmpy(dTDiv2,Ki)-_IQ(1))))
00160
00162
00178 #define PID_COEFFS_LPF(kp,ki,kd,lpf,b0,b1,b2,a1,a2) {\
00179 b0 = _IQ(lpf*(kp*2*DTF+ki*DTF*DTF+4*kd)/(4+2*lpf*DTF));\
00180 b1 = _IQ(lpf*(8*ki*DTF*DTF-8*kd)/(4+2*lpf*DTF));\
00181 b2 = _IQ(lpf*(-kp*2*DTF+ki*DTF*DTF+4*kd)/(4+2*lpf*DTF));\
00182 a1 = _IQ((-8)/(4+2*lpf*DTF));\
00183 a2 = _IQ((4-2*lpf*DTF)/(4+2*lpf*DTF));\
00184 }
00185
00190 #define PID_REG_LPF(in,out,b0,b1,b2,a1,a2) out[N] = - _IQmpy(a1,out[N1]) - _IQmpy(a2,out[N2]) + _IQmpy(b0,in[N]) + _IQmpy(b1,in[N1]) + _IQmpy(b2,in[N2])
00191
00196 #define PID_QD_REG_LPF(in,out,b0,b1,b2,a1,a2) PID_REG(in.q,out.q,b0,b1,b2,a1,a2); PID_REG(in.d,out.d,b0,b1,b2,a1,a2)
00197
00203 #define PID_COEFFS(kp,ki,kd,b0,b1,b2) {\
00204 b0 = _IQmpy(ki,DTDIV2)+_IQdiv(kd,DTDIV2)+kp;\
00205 b1 = _IQmpy(ki,DT) -_IQdiv(kd,DTDIV4);\
00206 b2 = _IQmpy(ki,DTDIV2)+_IQdiv(kd,DTDIV2)-kp;\
00207 }
00208
00209
00214 #define PID_REG(in,out,k0,k1,k2) out[N] = _IQmpy(k0,in[N]) + _IQmpy(k1,in[N1]) + _IQmpy(k2,in[N2]) + out[N2]
00215
00220 #define INV_PID_REG(in,out,k0,k1,k2) in[N] = _IQdiv((out[N]-out[N2]- _IQmpy(k1,in[N1])- _IQmpy(k2,in[N2])),k0)
00221
00226 #define PID_QD_REG(in,out,k0,k1,k2) PID_REG(in.d,out.d,k0,k1,k2); PID_REG(in.q,out.q,k0,k1,k2)
00227
00232 #define INV_PID_QD_REG(in,out,k0,k1,k2) INV_PID_REG(in.d,out.d,k0,k1,k2); INV_PID_REG(in.q,out.q,k0,k1,k2)
00233
00234
00240 #define INV_PI_REG(in,out,Kp,Ki) {\
00241 in[N] = _IQdiv(out[N] - out[N1] -\
00242 _IQmpy(Kp,_IQmpy(_IQmpy(DTDIV2,Ki)-_IQ(1), in[N1])),\
00243 _IQmpy(Kp, _IQ(1)+_IQmpy(DTDIV2,Ki) ) );\
00244 }
00245
00251 #define INV_PI_QD_REG(in,out,Kp,Ki) INV_PI_REG(in.q,out.q,Kp,Ki); INV_PI_REG(in.d,out.d,Kp,Ki)
00252
00258 #define INV_PI_QD_REG_NOSYM(in,out,Kp_q,Ki_q,Kp_d,Ki_d) INV_PI_REG(in.q,out.q,Kp_q,Ki_q); INV_PI_REG(in.d,out.d,Kp_d,Ki_d)
00259
00260
00262 #define PI_REG_WR(in,out,Kp,Ki) out[N]=out[N1]+_IQmpy(Kp,_IQmpy(in[N],(_IQ(1)+_IQmpy(DTWDIV2,Ki))) + _IQmpy(in[N1],(_IQmpy(DTWDIV2,Ki)-_IQ(1))))
00263
00265
00271 #define INT(in,out) out[N]=out[N1]+_IQmpy(DTDIV2,(in[N]+in[N1]))
00272
00278 #define INT_QD(in, out) INT(in.q,out.q); INT(in.d,out.d);
00279
00285 #define INT_DT(in,out,dTDiv2) out[N]=out[N1]+_IQmpy(dTDiv2,(in[N]+in[N1]))
00286
00292 #define INT_QD_DT(in, out,dTDiv2) INT_DT(in.q,out.q,dTDiv2); INT_DT(in.d,out.d,dTDiv2);
00293
00294
00295
00297
00298
00304 #define FILTER_LPF(in,out,B0_LPF,A1_LPF) out[N]=_IQmpy(B0_LPF,(in[N]+in[N1]))+_IQmpy(A1_LPF,out[N1])
00305
00311 #define FILTER_Q_LPF(in,out,B0_LPF,A1_LPF) out.q[N]=_IQmpy(B0_LPF,(in.q[N]+in.q[N1]))+_IQmpy(A1_LPF,out.q[N1])
00312
00318 #define FILTER_D_LPF(in,out,B0_LPF,A1_LPF) out.d[N]=_IQmpy(B0_LPF,(in.d[N]+in.d[N1]))+_IQmpy(A1_LPF,out.d[N1])
00319
00325 #define FILTER_QD_LPF(in,out,B0_LPF,A1_LPF) FILTER_Q_LPF(in,out,B0_LPF,A1_LPF); FILTER_D_LPF(in,out,B0_LPF,A1_LPF)
00326
00327
00329
00330
00336 #define FILTER_HPF(in,out,B0_HPF,A1_HPF) out[N]=_IQmpy(B0_HPF,(in[N]-in[N1]))+_IQmpy(A1_HPF,out[N1])
00337
00343 #define FILTER_Q_HPF(in,out,B0_HPF,A1_HPF) out.q[N]=_IQmpy(B0_HPF,(in.q[N]-in.q[N1]))+_IQmpy(A1_HPF,out.q[N1])
00344
00350 #define FILTER_D_HPF(in,out,B0_HPF,A1_HPF) out.d[N]=_IQmpy(B0_HPF,(in.d[N]-in.d[N1]))+_IQmpy(A1_HPF,out.d[N1])
00351
00357 #define FILTER_QD_HPF(in,out,B0_HPF,A1_HPF) FILTER_Q_HPF(in,out,B0_HPF,A1_HPF); FILTER_D_HPF(in,out,B0_HPF,A1_HPF)
00358
00359
00361
00362
00370 static inline _iq FirFilter(volatile _iq* in, _iq* b, unsigned int n)
00371 {
00372 unsigned int cc;
00373 _iq out = 0;
00374 for(cc=0;cc<n;cc++)
00375 out += _IQmpy(in[cc],b[cc]);
00376 return out;
00377 }
00378
00379
00380
00389 static inline _iq FirFilterV2(volatile _iq* in, _iq* b, unsigned int n)
00390 {
00391 unsigned int cc;
00392 register _iq temp;
00393 temp = _IQmpy(*in++,*b++);
00394 for(cc=1;cc<n;cc++)
00395 temp += _IQmpy(*in++,*b++);
00396 return temp;
00397 }
00398
00399
00400
00411 static inline _iq IIRFilter(volatile _iq* in, volatile _iq* out,
00412 _iq* b, unsigned int nb, _iq* a, unsigned int na)
00413 {
00414 return FirFilter(in,b,nb) - FirFilter(out,a,na);
00415 }
00416
00417
00418
00419
00426 static inline void ShiftRight(volatile _iq* in, unsigned int n)
00427 {
00428 unsigned int cc;
00429 for(cc=n-1;cc>0;cc--)
00430 in[cc] = in[cc-1];
00431 }
00432
00433
00434
00442 static inline void ShiftRightV2(volatile _iq* in, unsigned int n)
00443 {
00444 unsigned int cc;
00445 volatile _iq* yend = &(in[n-1]);
00446 for(cc=0;cc<n;cc++)
00447 *yend = *(--yend);
00448 }
00449
00450
00451
00458 static inline void Zeros(volatile _iq* v, unsigned int n)
00459 {
00460 unsigned int cc;
00461 for(cc=0;cc<n;cc++)
00462 v[cc] = 0;
00463 }
00464
00465
00466
00473 static inline void MAvrgCoeffs(_iq* b, unsigned int n)
00474 {
00475 unsigned int cc;
00476 _iq b0 = _IQdiv(_IQ(1.0),_IQ(n));
00477 for(cc=0;cc<n;cc++)
00478 b[cc] = b0;
00479 }
00480
00481
00482
00483
00484 #ifdef Q_AXIS_REAL
00485
00486 #define DQ2PH_QDDATA(fqd,fa,fb,fc) fa=fqd.q[N];fb=_IQmpy(_IQ(-0.5),fqd.q[N])-_IQmpy(SQRT3DIV2,fqd.d[N]);fc=-(fa+fb)
00487 #define DQ2PH(fqd,fa,fb,fc) fa=fqd.q;fb=_IQmpy(_IQ(-0.5),fqd.q)-_IQmpy(SQRT3DIV2,fqd.d);fc=-(fa+fb)
00488
00489 #define PH2QD(fa,fb,fqd) fqd.q=fa;fqd.d=-(_IQmpy(SQRT3DIV3,fa)+_IQmpy(TWOSQRT3DIV3,fb));
00490 #define PH2QD_QDDATA(fa,fb,fqd) fqd.q[N]=fa;fqd.d[N]=-(_IQmpy(SQRT3DIV3,fa)+_IQmpy(TWOSQRT3DIV3,fb));
00491
00492 #define CROSS(in,saliency,out) out=_IQmpy(-in.d,_IQcos(saliency))-_IQmpy(in.q,_IQsin(saliency))
00493 #define CROSS_QDDATA(in,saliency,out) out=_IQmpy(-in.d[N],_IQcos(saliency))-_IQmpy(in.q[N],_IQsin(saliency))
00494
00495 #define SCALAR2SYNCVEC(in,out,t) out.q[N]=_IQmpy(in , _IQcos(t));out.d[N]=-_IQmpy(in , _IQsin(t));
00496 #define SSCALAR2SYNCVEC(in,out,t) out.q=_IQmpy(in , _IQcos(t));out.d=-_IQmpy(in , _IQsin(t));
00497
00498 #define SCALAR2PULSATING(in,out,t) out.q[N]=_IQ(0); out.d[N]=-_IQmpy(in , _IQcos(t));
00499 #define SSCALAR2PULSATING(in,out,t) out.q=_IQ(0); out.d=-_IQmpy(in , _IQcos(t));
00500
00501 #else
00502 #ifdef D_AXIS_REAL
00503
00504
00510 #define DQ2PH_QDDATA(fqd,fa,fb,fc) fa=fqd.d[N];fb=_IQmpy(_IQ(-0.5),fqd.d[N])+_IQmpy(SQRT3DIV2,fqd.q[N]);fc=-(fa+fb)
00511
00517 #define DQ2PH(fqd,fa,fb,fc) fa=fqd.d;fb=_IQmpy(_IQ(-0.5),fqd.d)+_IQmpy(SQRT3DIV2,fqd.q);fc=-(fa+fb)
00518
00520
00526 #define PH2QD(fa,fb,fqd) fqd.d=fa;fqd.q=(_IQmpy(SQRT3DIV3,fa)+_IQmpy(TWOSQRT3DIV3,fb));
00527
00533 #define PH2QD_QDDATA(fa,fb,fqd) fqd.d[N]=fa;fqd.q[N]=(_IQmpy(SQRT3DIV3,fa)+_IQmpy(TWOSQRT3DIV3,fb));
00534
00536
00542 #define CROSS(in,saliency,out) out=_IQmpy(in.d,_IQsin(saliency))-_IQmpy(in.q,_IQcos(saliency))
00543
00549 #define CROSS_QDDATA(in,saliency,out) out=_IQmpy(in.d[N],_IQsin(saliency))-_IQmpy(in.q[N],_IQcos(saliency))
00550
00552
00558 #define SCALAR2SYNCVEC(in,out,t) out.d[N]=_IQmpy(in , _IQcos(t));out.q[N]=_IQmpy(in , _IQsin(t));
00559
00565 #define SSCALAR2SYNCVEC(in,out,t) out.d=_IQmpy(in , _IQcos(t));out.q=_IQmpy(in , _IQsin(t));
00566
00568
00574 #define SCALAR2PULSATING(in,out,t) out.d[N]=_IQmpy(in , _IQcos(t));out.q[N]=_IQ(0);
00575
00581 #define SSCALAR2PULSATING(in,out,t) out.d=_IQmpy(in , _IQcos(t));out.q=_IQ(0);
00582 #else
00583 #error Q_AXIS_REAL or D_AXIS_REAL must be defined to use this header
00584 #endif
00585 #endif
00586
00587
00588 #endif
00589
00590
00591