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
00360
00361 #ifdef Q_AXIS_REAL
00362
00363 #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)
00364 #define DQ2PH(fqd,fa,fb,fc) fa=fqd.q;fb=_IQmpy(_IQ(-0.5),fqd.q)-_IQmpy(SQRT3DIV2,fqd.d);fc=-(fa+fb)
00365
00366 #define PH2QD(fa,fb,fqd) fqd.q=fa;fqd.d=-(_IQmpy(SQRT3DIV3,fa)+_IQmpy(TWOSQRT3DIV3,fb));
00367 #define PH2QD_QDDATA(fa,fb,fqd) fqd.q[N]=fa;fqd.d[N]=-(_IQmpy(SQRT3DIV3,fa)+_IQmpy(TWOSQRT3DIV3,fb));
00368
00369 #define CROSS(in,saliency,out) out=_IQmpy(-in.d,_IQcos(saliency))-_IQmpy(in.q,_IQsin(saliency))
00370 #define CROSS_QDDATA(in,saliency,out) out=_IQmpy(-in.d[N],_IQcos(saliency))-_IQmpy(in.q[N],_IQsin(saliency))
00371
00372 #define SCALAR2SYNCVEC(in,out,t) out.q[N]=_IQmpy(in , _IQcos(t));out.d[N]=-_IQmpy(in , _IQsin(t));
00373 #define SSCALAR2SYNCVEC(in,out,t) out.q=_IQmpy(in , _IQcos(t));out.d=-_IQmpy(in , _IQsin(t));
00374
00375 #define SCALAR2PULSATING(in,out,t) out.q[N]=_IQ(0); out.d[N]=-_IQmpy(in , _IQcos(t));
00376 #define SSCALAR2PULSATING(in,out,t) out.q=_IQ(0); out.d=-_IQmpy(in , _IQcos(t));
00377
00378 #else
00379 #ifdef D_AXIS_REAL
00380
00381
00387 #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)
00388
00394 #define DQ2PH(fqd,fa,fb,fc) fa=fqd.d;fb=_IQmpy(_IQ(-0.5),fqd.d)+_IQmpy(SQRT3DIV2,fqd.q);fc=-(fa+fb)
00395
00397
00403 #define PH2QD(fa,fb,fqd) fqd.d=fa;fqd.q=(_IQmpy(SQRT3DIV3,fa)+_IQmpy(TWOSQRT3DIV3,fb));
00404
00410 #define PH2QD_QDDATA(fa,fb,fqd) fqd.d[N]=fa;fqd.q[N]=(_IQmpy(SQRT3DIV3,fa)+_IQmpy(TWOSQRT3DIV3,fb));
00411
00413
00419 #define CROSS(in,saliency,out) out=_IQmpy(in.d,_IQsin(saliency))-_IQmpy(in.q,_IQcos(saliency))
00420
00426 #define CROSS_QDDATA(in,saliency,out) out=_IQmpy(in.d[N],_IQsin(saliency))-_IQmpy(in.q[N],_IQcos(saliency))
00427
00429
00435 #define SCALAR2SYNCVEC(in,out,t) out.d[N]=_IQmpy(in , _IQcos(t));out.q[N]=_IQmpy(in , _IQsin(t));
00436
00442 #define SSCALAR2SYNCVEC(in,out,t) out.d=_IQmpy(in , _IQcos(t));out.q=_IQmpy(in , _IQsin(t));
00443
00445
00451 #define SCALAR2PULSATING(in,out,t) out.d[N]=_IQmpy(in , _IQcos(t));out.q[N]=_IQ(0);
00452
00458 #define SSCALAR2PULSATING(in,out,t) out.d=_IQmpy(in , _IQcos(t));out.q=_IQ(0);
00459 #else
00460 #error Q_AXIS_REAL or D_AXIS_REAL must be defined to use this header
00461 #endif
00462 #endif
00463
00464
00465 #endif
00466
00467
00468