Code Snippets Submitted by radiorama
Clarke and Park direct and inverse transforms
/* -------------- CPTrans.h begins -------------- */
#ifndef __CPTRANS_H__
#define __CPTRANS_H__
#include "types.h"
#define Minus1Over2 0xC000
#define Root3Over2 0x6EDA
#define TwoOver3 0x5555
#define Minus1Over3 0xD555
#define OneOnRoot3 0x49E7
/*---------------------------------------------------------------------------;
; Clarke transform (Vuvw -> Vab) with constant amplitudes. ;
; For optimal results it is advised to call this function with saturation ;
; disabled. ;
; ;
; Input: (R2) = Vu ;
; (R2+1) = Vv ;
; (R2+2) = Vw ;
; (R3) = Destination memory location for Va ;
; (R3+1) = Destination memory location for Vb ;
; ;
; Output: None ;
; ;
; Registers modified: A, X0, Y ;
;---------------------------------------------------------------------------*/
asm void Clarke(Frac16 *Vuvw, Frac16 *Vab);
/*---------------------------------------------------------------------------;
; Inverse Clarke transform (Vab -> Vuvw) with constant amplitudes. ;
; For optimal results it is advised to call this function with saturation ;
; disabled. ;
; ;
; Input: (R2) = Va ;
; (R2+1) = Vb ;
; (R3) = Destination memory location for Vu ;
; (R3+1) = Destination memory location for Vv ;
; (R3+2) = Destination memory location for Vw ;
; ;
; Output: None ;
; ;
; Registers modified: A, B, X0, Y ;
;---------------------------------------------------------------------------*/
asm void InvClarke(Frac16 *Vab, Frac16 *Vuvw);
/*---------------------------------------------------------------------------;
; Park transform (Vab -> Vdq). ;
; For optimal results it is advised to call this function with saturation ;
; disabled. ;
; ;
; Input: Y0 = cos(phi) ;
; Y1 = sin(phi) ;
; (R2) = Va ;
; (R2+1) = Vb ;
; (R3) = Destination memory location for Vd ;
; (R3+1) = Destination memory location for Vq ;
; ;
; Output: None ;
; ;
; Registers modified: A, B, X0 ;
;---------------------------------------------------------------------------*/
asm void Park(Frac16 cosphi, Frac16 sinphi, Frac16 *Vab, Frac16 *Vdq);
/*---------------------------------------------------------------------------;
; Inverse Park transform (Vdq -> Vab). ;
; For optimal results it is advised to call this function with saturation ;
; disabled. ;
; ;
; Input: Y0 = cos(phi) ;
; Y1 = sin(phi) ;
; (R2) = Vd ;
; (R2+1) = Vq ;
; (R3) = Destination memory location for Va ;
; (R3+1) = Destination memory location for Vb ;
; ;
; Output: None ;
; ;
; Registers modified: A, B, X0 ;
;---------------------------------------------------------------------------*/
asm void InvPark(Frac16 cosphi, Frac16 sinphi, Frac16 *Vdq, Frac16 *Vab);
#endif //ifndef __CPTRANS_H__
/* -------------- CPTrans.h ends -------------- */
/* -------------- CPTrans.c begins -------------- */
/*---------------------------------------------------------------------------;
; Clarke transform (Vuvw -> Vab) with constant amplitudes. ;
; For optimal results it is advised to call this function with saturation ;
; disabled. ;
; ;
; Input: (R2) = Vu ;
; (R2+1) = Vv ;
; (R2+2) = Vw ;
; (R3) = Destination memory location for Va ;
; (R3+1) = Destination memory location for Vb ;
; ;
; Output: None ;
; ;
; Registers modified: A, X0, Y ;
;---------------------------------------------------------------------------*/
asm void Clarke(Frac16 *Vuvw, Frac16 *Vab){
MOVE.W X:(R2),X0 //X0 = Vu
MOVE.W #TwoOver3,Y0 //Y0 = 2/3
MPY X0,Y0,A //A = 2/3*Vu
MOVE.W #Minus1Over3,X0 //X0 = -1/3
MOVE.W X:(R2+1),Y0 //Y0 = Vv
MOVE.W X:(R2+2),Y1 //Y1 = Vw
MAC X0,Y0,A //A = 2/3*Vu-1/3*Vv
MAC X0,Y1,A //A = 2/3*Vu-1/3*Vv-1/3*Vw
MOVE.W A,X:(R3) //Va = 2/3*Vu-1/3*Vv-1/3*Vw
MOVE.W #OneOnRoot3,X0 //X0 = 1/sqrt(3)
MPY X0,Y0,A //A = 1/sqrt(3)*Vv
MACR -X0,Y1,A //A = 1/sqrt(3)*Vv-1/sqrt(3)*Vw
MOVE.W A,X:(R3+1) //Vb = 1/sqrt(3)*Vv-1/sqrt(3)*Vw
RTS //Return from subroutine
}
/*---------------------------------------------------------------------------;
; Inverse Clarke transform (Vab -> Vuvw) with constant amplitudes. ;
; For optimal results it is advised to call this function with saturation ;
; disabled. ;
; ;
; Input: (R2) = Va ;
; (R2+1) = Vb ;
; (R3) = Destination memory location for Vu ;
; (R3+1) = Destination memory location for Vv ;
; (R3+2) = Destination memory location for Vw ;
; ;
; Output: None ;
; ;
; Registers modified: A, B, X0, Y ;
;---------------------------------------------------------------------------*/
asm void InvClarke(Frac16 *Vab, Frac16 *Vuvw){
MOVE.W X:(R2),Y0 //Y0 = Va
MOVE.W X:(R2+1),Y1 //Y1 = Vb
MOVE.W Y0,X:(R3) //Vu = Va
MOVE.W #Minus1Over2,X0 //X0 = -1/2
MPY X0,Y0,A //A = -1/2*Va
TFR A,B //B = -1/2*Va
MOVE.W #Root3Over2,X0 //X0 = sqrt(3)/2
MAC X0,Y1,A //A = -1/2*Va+sqrt(3)/2*Vb
MAC -X0,Y1,B //B = -1/2*Va-sqrt(3)/2*Vb
MOVE.W A,X:(R3+1) //Vv = -1/2*Va+sqrt(3)/2*Vb
MOVE.W B,X:(R3+2) //Vw = -1/2*Va-sqrt(3)/2*Vb
RTS //Return from subroutine
}
/*---------------------------------------------------------------------------;
; Park transform (Vab -> Vdq). ;
; For optimal results it is advised to call this function with saturation ;
; disabled. ;
; ;
; Input: Y0 = cos(phi) ;
; Y1 = sin(phi) ;
; (R2) = Va ;
; (R2+1) = Vb ;
; (R3) = Destination memory location for Vd ;
; (R3+1) = Destination memory location for Vq ;
; ;
; Output: None ;
; ;
; Registers modified: A, B, X0 ;
;---------------------------------------------------------------------------*/
asm void Park(Frac16 cosphi, Frac16 sinphi, Frac16 *Vab, Frac16 *Vdq){
MOVE.W X:(R2),X0 //X0 = Va
MPY X0,Y0,A //A = Va*cos(phi)
MPY -X0,Y1,B //B = -Va*sin(phi)
MOVE.W X:(R2+1),X0 //X0 = Vb
MAC X0,Y1,A //A = Va*cos(phi)+Vb*sin(phi)
MAC X0,Y0,B //B = -Va*sin(phi)+Vb*cos(phi)
MOVE.W A,X:(R3) //Vd = Va*cos(phi)+Vb*sin(phi)
MOVE.W B,X:(R3+1) //Vq = -Va*sin(phi)+Vb*cos(phi)
RTS //Return from subroutine
}
/*---------------------------------------------------------------------------;
; Inverse Park transform (Vdq -> Vab). ;
; For optimal results it is advised to call this function with saturation ;
; disabled. ;
; ;
; Input: Y0 = cos(phi) ;
; Y1 = sin(phi) ;
; (R2) = Vd ;
; (R2+1) = Vq ;
; (R3) = Destination memory location for Va ;
; (R3+1) = Destination memory location for Vb ;
; ;
; Output: None ;
; ;
; Registers modified: A, B, X0 ;
;---------------------------------------------------------------------------*/
asm void InvPark(Frac16 cosphi, Frac16 sinphi, Frac16 *Vdq, Frac16 *Vab){
MOVE.W X:(R2),X0 //X0 = Vd
MPY X0,Y0,A //A = Vd*cos(phi)
MPY X0,Y1,B //B = Vd*sin(phi)
MOVE.W X:(R2+1),X0 //X0 = Vq
MAC -X0,Y1,A //A = Vd*cos(phi)-Vq*sin(phi)
MAC X0,Y0,B //B = Vd*sin(phi)+Vq*cos(phi)
MOVE.W A,X:(R3) //Vd = Vd*cos(phi)-Vq*sin(phi)
MOVE.W B,X:(R3+1) //Vq = Vd*sin(phi)+Vq*cos(phi)
RTS //Return from subroutine
}
/* -------------- CPTrans.c ends -------------- */
/* -------------- Usage example begins ------------- */
/* application specific includes */
#include "CPTrans.h"
/* global variables */
Frac16 cosphi,sinphi;
Frac16 Vuvw[3];
Frac16 Vab[2];
Frac16 Vdq[2];
Frac16 Vab1[2];
Frac16 Vuvw1[3];
/* Transform function calls */
//This example performs the full processing chain from three-phase fixed to
//two-phase rotating reference and back. In the end Vuvw and Vuvw1 should be
//equal save for rounding errors.
Clarke((Frac16*)&Vuvw,(Frac16*)&Vab);
Park(cosphi,sinphi,(Frac16*)&Vab,(Frac16*)&Vdq);
InvPark(cosphi,sinphi,(Frac16*)&Vdq,(Frac16*)&Vab1);
InvClarke((Frac16*)&Vab1,(Frac16*)&Vuvw1);
/* -------------- Usage example ends ------------- */Second order IIR filter with decimation
/* -------------- IIR2.h begins -------------- */
#ifndef __IIR2_H__
#define __IIR2_H__
#include "types.h"
typedef struct
{
unsigned int decimation;
Frac16 b0over2;
Frac16 b1over2;
Frac16 b2over2;
Frac16 ma1over2;
Frac16 ma2over2;
}IIR2params;
typedef struct
{
unsigned int deccnt;
Frac16 xkm1;
Frac16 xkm2;
Frac16 ykm1;
Frac16 ykm2;
}IIR2data;
/*---------------------------------------------------------------------------;
; Initialize decimation divider and past data of filter to zero. ;
; ;
; Input: A10 = Integral memory initialization value ;
; Y0 = Past input initialization value ;
; (R2) = Integral memory storage destination ;
; (R2+2) = Past input storage destination ;
; ;
; Output: None ;
; ;
; Registers modified: R2 ;
;---------------------------------------------------------------------------*/
asm void IIR2Init(IIR2data *data);
/*---------------------------------------------------------------------------;
; This function implements a second order IIR Butteworth filter. ;
; The result yk is computed as a function of current (xk) and past (xk-1, ;
; xk-2) inputs and past (yk-1, yk-2) outputs as follows: ;
; yk = b0*xk + b1*xk-1 + b2*xk-2 - a1*yk-1 - a2*yk-2. ;
; Filter coefficients bi and aj are stored halved to ensure their magnitude ;
; does not exceed 1 (coefficients magnitude can be at most equal to 2). ;
; For optimal results it is advised to call this function with saturation ;
; disabled. ;
; ;
; Input: Y0 = xk (input sample) ;
; (R2) = decimation (1 = no decimation, 2 = discard every other ;
; sample, and so on) ;
; (R2+1) = b0/2 ;
; (R2+2) = b1/2 ;
; (R2+3) = b2/2 ;
; (R2+4) = -a1/2 ;
; (R2+5) = -a2/2 ;
; (R3) = decimation counter (used internally for data decimation) ;
; (R3+1) = xk-1 ;
; (R3+2) = xk-2 ;
; (R3+3) = yk-1 ;
; (R3+4) = yk-2 ;
; ;
; Output: Y0 = yk ;
; ;
; Registers modified: A, X0, Y, R0, R3 ;
;---------------------------------------------------------------------------*/
asm Frac16 IIR2(Frac16 xk, IIR2params *params, IIR2data *data);
#endif //ifndef __IIR2_H__
/* -------------- IIR2.h ends -------------- */
/* -------------- IIR2.c begins -------------- */
/*---------------------------------------------------------------------------;
; Initialize decimation divider and past data of filter to zero. ;
; ;
; Input: A10 = Integral memory initialization value ;
; Y0 = Past input initialization value ;
; (R2) = Integral memory storage destination ;
; (R2+2) = Past input storage destination ;
; ;
; Output: None ;
; ;
; Registers modified: R2 ;
;---------------------------------------------------------------------------*/
asm void IIR2Init(IIR2data *data){
MOVE.W #0001,X:(R2) //Initialize decimation divider
ADDA #1,R2 //to execute the first call
REP #4 //Initialize past data to zero
CLR.W X:(R2)+
RTS //Return from subroutine
}
/*---------------------------------------------------------------------------;
; This function implements a second order IIR Butteworth filter. ;
; The result yk is computed as a function of current (xk) and past (xk-1, ;
; xk-2) inputs and past (yk-1, yk-2) outputs as follows: ;
; yk = b0*xk + b1*xk-1 + b2*xk-2 - a1*yk-1 - a2*yk-2. ;
; Filter coefficients bi and aj are stored halved to ensure their magnitude ;
; does not exceed 1 (coefficients magnitude can be at most equal to 2). ;
; For optimal results it is advised to call this function with saturation ;
; disabled. ;
; ;
; Input: Y0 = xk (input sample) ;
; (R2) = decimation (1 = no decimation, 2 = discard every other ;
; sample, and so on) ;
; (R2+1) = b0/2 ;
; (R2+2) = b1/2 ;
; (R2+3) = b2/2 ;
; (R2+4) = -a1/2 ;
; (R2+5) = -a2/2 ;
; (R3) = decimation counter (used internally for data decimation) ;
; (R3+1) = xk-1 ;
; (R3+2) = xk-2 ;
; (R3+3) = yk-1 ;
; (R3+4) = yk-2 ;
; ;
; Output: Y0 = yk ;
; ;
; Registers modified: A, X0, Y, R0, R3 ;
;---------------------------------------------------------------------------*/
asm Frac16 IIR2(Frac16 xk, IIR2params *params, IIR2data *data){
DEC.W X:(R3) //Decrement decimation counter
BEQ DoFilter //Decremented to zero?
MOVE.W X:(R3+3),Y0 //No, output old value and exit
BRA FilterDone
DoFilter: MOVE.W X:(R2),X0 //Yes, reload decimation counter
MOVE.W X0,X:(R3)+
ADDA #1,R2,R0 //Use R0 for parallel moves
MOVE.W Y0,Y1 //Save xk
MOVE.W X:(R0)+,X0 //b0/2
MPY Y0,X0,A X:(R0)+,Y0 X:(R3)+,X0 //b0/2*xk, b1/2, xk-1
MAC Y0,X0,A X:(R0)+,Y0 X:(R3)+,X0 //b1/2*xk-1, b2/2, xk-2
MAC Y0,X0,A X:(R0)+,Y0 X:(R3)+,X0 //b2/2*xk-2, -a2/2, yk-1
MAC Y0,X0,A X:(R0)+,Y0 X:(R3)-,X0 //-a2/2*yk-1, -a3/2, yk-2
MAC Y0,X0,A X:(R0)+,Y0 X:(R3)+,X0 //-a3/2*yk-2, n/a, yk-1
ASL A //Compensate for halved coeffs
MOVE.W A,Y0 //Saturate result if necessary
MOVE.W X0,X:(R3)- //yk-2=yk-1
MOVE.W Y0,X:(R3)- //yk-1=yk
ADDA #-1,R3 //Point to xk-1
MOVE.W X:(R3)+,X0 //xk-1
MOVE.W X0,X:(R3)- //xk-2=xk-1
MOVE.W Y1,X:(R3)+ //xk-1=xk
FilterDone: RTS //Return from subroutine
}
/* -------------- IIR2.c ends -------------- */
/* -------------- Usage example begins ------------- */
/* application specific constants */
#define IIRDec 160 //IIR filter decimation factor
#define IIRb0 0x0019 //IIR filter parameters: 128 Hz
#define IIRb1 0x0032 //lowpass (with 10 kHz sampling
#define IIRb2 0x0019 //frequency)
#define IIRa1 0x78BA
#define IIRa2 0xC6E1
/* application specific includes */
#include "IIR2.h"
/* global variables */
IIR2params IIRparams;
IIR2data IIRdata;
int FiltIn,IIROut;
/* initializations */
IIRparams.decimation=IIRDec; //Initialize filter decimation
IIRparams.b0over2=IIRb0; //Initialize filter parameters
IIRparams.b1over2=IIRb1;
IIRparams.b2over2=IIRb2;
IIRparams.ma1over2=IIRa1;
IIRparams.ma2over2=IIRa2;
IIR2Init(&IIRdata); //Initialize IIR filter past data
/* IIR filter function call */
IIROut=IIR2(FiltIn,&IIRparams,&IIRdata); //Call IIR filter
/* -------------- Usage example ends ------------- */32 bit Proportional-Integral (PI) Regulator
/* -------------- PI32.h begins -------------- */
#ifndef __PI32_H__
#define __PI32_H__
#include "types.h"
typedef struct
{
Frac16 c0;
Frac16 c1;
Frac16 IntSatH;
Frac16 IntSatL;
Frac16 kp;
UInt16 nkp;
Frac16 OutSatH;
Frac16 OutSatL;
}PI32params;
typedef struct
{
Frac32 Skm1;
Frac16 xkm1;
}PI32data;
/*---------------------------------------------------------------------------;
; Initialize integral memory and past input of PI regulator. ;
; ;
; Input: A10 = Integral memory initialization value ;
; Y0 = Past input initialization value ;
; (R2) = Integral memory storage destination ;
; (R2+2) = Past input storage destination ;
; ;
; Output: None ;
; ;
; Registers modified: R2 ;
;---------------------------------------------------------------------------*/
asm void PI32Init(Frac32 S, Frac16 xp, PI32data *data);
/*---------------------------------------------------------------------------;
; This function implements a 32 bit precision PI regulator with separate ;
; saturations on the integral portion and on the whole output. ;
; The routine first computes the integral portion at current sampling ;
; instant k (Sk) as Sk = Sk-1 + c0*xk + c1*xk-1. Sk is subsequently ;
; saturated between IntSatH and IntSatL and saved for later use at the next ;
; sampling instant. ;
; Next, proportional portion Pk is computed as kp*2^nkp*xk and saturated ;
; between -1 and +1. The two portions are added together and the result ;
; yk=Sk+Pk is saturated between OutSatH and OutSatL. ;
; Coefficients c0 and c1 are equal and amount to ki*Ts/2, where Ts is the ;
; sampling period and ki is the integral multiplier. The proportional ;
; multiplier kp*2^nkp is split into a fractional part and an exponent in ;
; order to allow values greater than unity. ;
; For optimal results it is advised to call this function with saturation ;
; disabled. ;
; ;
; Input: Y0 = xk (input sample of error signal) ;
; (R2) = c0 ;
; (R2+1) = c1 ;
; (R2+2) = IntSatH ;
; (R2+3) = IntSatL ;
; (R2+4) = kp ;
; (R2+5) = nkp ;
; (R2+6) = OutSatH ;
; (R2+7) = OutSatL ;
; (R3) = Sk-1 (32 bits) ;
; (R3+2) = xk-1 ;
; ;
; Output: A = yk (32 significant bits) ;
; ;
; Registers modified: A, B, X0, Y, R0 ;
;---------------------------------------------------------------------------*/
asm Frac32 PI32(Frac16 xk, PI32params *params, PI32data *data);
#endif //ifndef __PI32_H__
/* -------------- PI32.h ends -------------- */
/* -------------- PI32.c begins -------------- */
/*---------------------------------------------------------------------------;
; Initialize integral memory and past input of PI regulator. ;
; ;
; Input: A10 = Integral memory initialization value ;
; Y0 = Past input initialization value ;
; (R2) = Integral memory storage destination ;
; (R2+2) = Past input storage destination ;
; ;
; Output: None ;
; ;
; Registers modified: R2 ;
;---------------------------------------------------------------------------*/
asm void PI32Init(Frac32 S, Frac16 xp, PI32data *data){
MOVE.L A10,X:(R2)+ //Initialize integral memory
MOVE.W Y0,X:(R2) //Initialize past input
RTS //Return from subroutine
}
/*---------------------------------------------------------------------------;
; This function implements a 32 bit precision PI regulator with separate ;
; saturations on the integral portion and on the whole output. ;
; The routine first computes the integral portion at current sampling ;
; instant k (Sk) as Sk = Sk-1 + c0*xk + c1*xk-1. Sk is subsequently ;
; saturated between IntSatH and IntSatL and saved for later use at the next ;
; sampling instant. ;
; Next, proportional portion Pk is computed as kp*2^nkp*xk and saturated ;
; between -1 and +1. The two portions are added together and the result ;
; yk=Sk+Pk is saturated between OutSatH and OutSatL. ;
; Coefficients c0 and c1 are equal and amount to ki*Ts/2, where Ts is the ;
; sampling period and ki is the integral multiplier. The proportional ;
; multiplier kp*2^nkp is split into a fractional part and an exponent in ;
; order to allow values greater than unity. ;
; For optimal results it is advised to call this function with saturation ;
; disabled. ;
; ;
; Input: Y0 = xk (input sample of error signal) ;
; (R2) = c0 ;
; (R2+1) = c1 ;
; (R2+2) = IntSatH ;
; (R2+3) = IntSatL ;
; (R2+4) = kp ;
; (R2+5) = nkp ;
; (R2+6) = OutSatH ;
; (R2+7) = OutSatL ;
; (R3) = Sk-1 (32 bits) ;
; (R3+2) = xk-1 ;
; ;
; Output: A = yk (32 significant bits) ;
; ;
; Registers modified: A, B, X0, Y, R0 ;
;---------------------------------------------------------------------------*/
asm Frac32 PI32(Frac16 xk, PI32params *params, PI32data *data){
MOVE.W R2,X0
MOVE.W X0,R0
MOVE.L X:(R3)+,A //Load Sk-1
MOVE.W X:(R0)+,X0 //Load c0
MOVE.W Y0,Y1 //Save xk
MAC Y0,X0,A X:(R0)+,Y0 X:(R3)+,X0 //c0*xk, c1, xk-1
MAC Y0,X0,A X:(R0)+,Y0 X:(R3)-,X0 //c1*xk-1, IntSatH, decr. R3
MOVE.W Y0,B //Load IntSatH into B
CMP A,B Y1,X:(R3)+ //Check upper satur. and save xk
BGT CheckSatLo //If Sk<IntSatH no need for sat.
MOVE.W Y0,A //else saturate and increment R0
ADDA #1,R0 //to skip IntSatL in the
BRA IntSatOK //parameter structure
CheckSatLo: MOVE.W X:(R0)+,B //Load IntSatL
CMP A,B //Check lower saturation
BLE IntSatOK //If Sk>IntSatL no need for sat.
MOVE.W B1,A //else saturate
IntSatOK: ADDA #-3,R3,R3 //Point to Sk storage location
MOVE.L A10,X:(R3) //Save Sk
MOVE.W X:(R0)+,Y0 //Load kp
MPY Y0,Y1,B X:(R0)+,Y0 //kp*xk, nkp
CLB B,X0 //Number of leading bits of
CMP Y0,X0 //result greater or equal than
NOP //desired shift count?
BLT PropSat //No, saturate
ASLL.L Y0,B //Yes, shift
BRA PropSatOK
PropSat: TST B //Saturate proportional portion
BLT PropSatNeg //according to sign
MOVE.W #$7FFF,B
BRA PropSatOK
PropSatNeg: MOVE.W #$8000,B
PropSatOK: ADD B,A X:(R0)+,B //Compute yk, load OutSatH
CMP A,B X:(R0)+,X0 //Check upper sat., load OutSatL
BGT CheckSatOutLo //If yk<OutSatH no need for sat.
MOVE.W B1,A //else saturate
BRA OutSatOK
CheckSatOutLo: CMP X0,A //Check lower saturation
BGT OutSatOK //If yk>OutSatL no need for sat.
MOVE.W X0,A //else saturate
OutSatOK: RTS //Return from subroutine
}
/* -------------- PI32.c ends -------------- */
/* -------------- Usage example begins ------------- */
/* application specific constants */
#define PIc0 100
#define PIc1 100
#define PIIntSatH 15000
#define PIIntSatL -15000
#define PIkp 0x4000
#define PInkp 1
#define PIOutSatH 30000
#define PIOutSatL -30000
/* application specific includes */
#include "PI32.h"
/* global variables */
PI32params PIparams;
PI32data PIdata;
int PIIn,PIOut;
/* initializations */
PIparams.c0=PIc0; //Initialize PI regulator
PIparams.c1=PIc1; //parameters and past data
PIparams.IntSatH=PIIntSatH;
PIparams.IntSatL=PIIntSatL;
PIparams.kp=PIkp;
PIparams.nkp=PInkp;
PIparams.OutSatH=PIOutSatH;
PIparams.OutSatL=PIOutSatL;
PI32Init(0,0, &PIdata);
/* PI32 regulator function call */
PIOut=(int)(PI32(PIIn,&PIparams,&PIdata)>>16); //Call PI regulator
/* -------------- Usage example ends ------------- */Sine and cosine functions through MacLaurin expansion
/* -------------- SinCos.h begins -------------- */
#ifndef __SINCOS_H__
#define __SINCOS_H__
#include "types.h"
/*---------------------------------------------------------------------------;
; This function computes the sine of an angle using the Maclaurin series ;
; expansion method. Input angle is signed fractional where -1 corresponds to ;
; -pi and +1 to +pi; output is signed fractional and the minimum negative ;
; value is limited to -1+2^(-15) (0x8001 hexadeximal) to avoid overflow in ;
; subsequent calculations. ;
; ;
; Input: Y0 = angle (signed fractional; -1 = -pi, +1 = +pi) ;
; ;
; Output: Y0 = sine (signed fractional) ;
; ;
; Registers modified: A, B, X0, Y ;
;---------------------------------------------------------------------------*/
asm Frac16 Sin(Frac16 angle);
/*---------------------------------------------------------------------------;
; This function computes the cosine of an angle using the Maclaurin series ;
; expansion method. Input angle is signed fractional where -1 corresponds to ;
; -pi and +1 to +pi; output is signed fractional and the minimum negative ;
; value is limited to -1+2^(-15) (0x8001 hexadeximal) to avoid overflow in ;
; subsequent calculations. ;
; ;
; Input: Y0 = angle (signed fractional; -1 = -pi, +1 = +pi) ;
; ;
; Output: Y0 = cosine (signed fractional) ;
; ;
; Registers modified: A, B, X0, Y ;
;---------------------------------------------------------------------------*/
asm Frac16 Cos(Frac16 angle);
#endif //ifndef __SINCOS_H__
/* -------------- SinCos.h ends -------------- */
/* -------------- SinCos.c begins -------------- */
/*---------------------------------------------------------------------------;
; This function computes the sine of an angle using the Maclaurin series ;
; expansion method. Input angle is signed fractional where -1 corresponds to ;
; -pi and +1 to +pi; output is signed fractional and the minimum negative ;
; value is limited to -1+2^(-15) (0x8001 hexadeximal) to avoid overflow in ;
; subsequent calculations. ;
; ;
; Input: Y0 = angle (signed fractional; -1 = -pi, +1 = +pi) ;
; ;
; Output: Y0 = sine (signed fractional) ;
; ;
; Registers modified: A, B, X0, Y ;
;---------------------------------------------------------------------------*/
asm Frac16 Sin(Frac16 angle){
CLR.W Y1 //Clear Y1
MOVE.W Y0,B //Compute absolute value of angle
ABS B
CMP.W #$4000,B //|angle| > pi/2?
BLT SinAngleOK //No, proceed
ADD.W #$8000,Y0 //Yes, add pi and set flag for
BFSET #$0001,Y1 //final negation of result
SinAngleOK: MPYR Y0,Y0,A //Compute angle squared
MOVE.W A,X0 //X0 = angle squared
MOVE.W #$0002,B //Compute Maclaurin expansion
MOVE.W #$FFE2,A
MACR X0,B1,A //A1 = first partial result
MOVE.W #$0150,B
MACR X0,A1,B //B1 = second partial result
MOVE.W #$F669,A
MACR X0,B1,A //A1 = third partial result
MOVE.W #$28CD,B
MACR X0,A1,B //B1 = fourth partial result
MOVE.W #$AD52,A
MACR X0,B1,A //A1 = fifth partial result
MOVE.W #$3244,B
MACR X0,A1,B //B1 = sixth partial result
MOVE.W #$0003,X0 //Required shift amount
MPYR B1,Y0,A //A = result / 8
CMP #$1000,A //If magnitude is maximum
BNE SinCheckSatL //saturate the result
MOVE.W #$7FFF,A
BRA SinValOK
SinCheckSatL: CMP.W #$F000,A
BNE SinNoSat
MOVE.W #$8001,A
BRA SinValOK
SinNoSat: ASLL.L X0,A //Shift to get correct result
//(Maclaurin coefficients are
//divided by 8 to avoid overflow)
SinValOK: BRCLR #$0001,Y1,Update_Sin //Negate result if |angle| was
NEG A //greater than pi/2
Update_Sin: MOVE.W A,Y0 //Save result
RTS //Return from subroutine
}
/*---------------------------------------------------------------------------;
; This function computes the cosine of an angle using the Maclaurin series ;
; expansion method. Input angle is signed fractional where -1 corresponds to ;
; -pi and +1 to +pi; output is signed fractional and the minimum negative ;
; value is limited to -1+2^(-15) (0x8001 hexadeximal) to avoid overflow in ;
; subsequent calculations. ;
; ;
; Input: Y0 = angle (signed fractional; -1 = -pi, +1 = +pi) ;
; ;
; Output: Y0 = cosine (signed fractional) ;
; ;
; Registers modified: A, B, X0, Y ;
;---------------------------------------------------------------------------*/
asm Frac16 Cos(Frac16 angle){
ADD.W #$4000,Y0 //Add pi/2 to angle
JSR Sin //Call sine function
RTS //Return from subroutine
}
/* -------------- SinCos.c ends -------------- */
/* -------------- Usage example begins ------------- */
/* application specific includes */
#include "SinCos.h"
/* global variables */
Frac16 Angle,cosphi,sinphi;
/* Function calls */
cosphi=Cos(Angle); //Compute sin and cos of angle
sinphi=Sin(Angle); //(for use in Park transforms)
/* -------------- Usage example ends ------------- */Moving average filter with decimation
/* -------------- MovAvg.h begins -------------- */
#ifndef __MOVAVG_H__
#define __MOVAVG_H__
#include "types.h"
typedef struct
{
unsigned int decimation;
unsigned int deccnt;
unsigned int BufHeadPtr;
unsigned int BufLength;
Frac16 multiplier;
Frac16 ykm1;
}MovAvgparams;
/*---------------------------------------------------------------------------;
; Initialize buffer length, pointer and decimation divider of moving average ;
; filter; initialize memory buffer of moving average filter to all zeroes. ;
; ;
; Input: Y0 = buffer length ;
; (R2+1) = decimation counter (used internally for data decimation) ;
; (R2+2) = index of buffer head ;
; (R2+3) = buffer length (filter order) ;
; R3 = pointer to storage buffer for the past input samples ;
; ;
; Output: None ;
; ;
; Registers modified: R2 ;
;---------------------------------------------------------------------------*/
asm void MovAvgInit(unsigned int length, MovAvgparams *params, Frac16 *data);
/*---------------------------------------------------------------------------;
; This function implements a moving average Nth order FIR filter. ;
; The result yk is computed as the sum of the current and the past N input ;
; samples multiplied by the multiplier value. Canonical moving average ;
; filtering is obtained if multiplier = 1/(N+1). ;
; For optimal results it is advised to call this function with saturation ;
; disabled. ;
; ;
; Input: Y0 = xk (input sample) ;
; (R2) = decimation (1 = no decimation, 2 = discard every other ;
; sample, and so on) ;
; (R2+1) = decimation counter (used internally for data decimation) ;
; (R2+2) = index of buffer head ;
; (R2+3) = N (length of buffer and filter order) ;
; (R2+4) = fractional multiplier ;
; (R2+5) = yk-1 (past output) ;
; R3 = Pointer to storage buffer for the N past input samples ;
; ;
; Output: Y0 = yk ;
; ;
; Registers modified: A, X0, Y, R0, R3 ;
;---------------------------------------------------------------------------*/
asm Frac16 MovAvg(Frac16 xk, MovAvgparams *params, Frac16 *data);
#endif //ifndef __MOVAVG_H__
/* -------------- MovAvg.h ends -------------- */
/* -------------- MovAvg.c begins -------------- */
/*---------------------------------------------------------------------------;
; Initialize buffer length, pointer and decimation divider of moving average ;
; filter; initialize memory buffer of moving average filter to all zeroes. ;
; ;
; Input: Y0 = buffer length ;
; (R2+1) = decimation counter (used internally for data decimation) ;
; (R2+2) = index of buffer head ;
; (R2+3) = buffer length (filter order) ;
; R3 = pointer to storage buffer for the past input samples ;
; ;
; Output: None ;
; ;
; Registers modified: R2 ;
;---------------------------------------------------------------------------*/
asm void MovAvgInit(unsigned int length, MovAvgparams *params, Frac16 *data){
MOVE.W #0001,X:(R2+1) //Initialize decimation divider
CLR.W X:(R2+2) //Initialize buffer head pointer
MOVE.W Y0,X:(R2+3) //Initialize buffer length
REP Y0 //Initialize past data to zero
CLR.W X:(R3)+
RTS //Return from subroutine
}
/*---------------------------------------------------------------------------;
; This function implements a moving average Nth order FIR filter. ;
; The result yk is computed as the sum of the current and the past N input ;
; samples multiplied by the multiplier value. Canonical moving average ;
; filtering is obtained if multiplier = 1/(N+1). ;
; For optimal results it is advised to call this function with saturation ;
; disabled. ;
; ;
; Input: Y0 = xk (input sample) ;
; (R2) = decimation (1 = no decimation, 2 = discard every other ;
; sample, and so on) ;
; (R2+1) = decimation counter (used internally for data decimation) ;
; (R2+2) = index of buffer head ;
; (R2+3) = N (length of buffer and filter order) ;
; (R2+4) = fractional multiplier ;
; (R2+5) = yk-1 (past output) ;
; R3 = Pointer to storage buffer for the N past input samples ;
; ;
; Output: Y0 = yk ;
; ;
; Registers modified: A, X0, Y, R2, R3 ;
;---------------------------------------------------------------------------*/
asm Frac16 MovAvg(Frac16 xk, MovAvgparams *params, Frac16 *data){
DEC.W X:(R2+1) //Decrement decimation counter
BEQ DoMovAvg //Decremented to zero?
MOVE.W X:(R2+5),Y0 //No, output old value and exit
BRA MovAvgDone
DoMovAvg: MOVE.W X:(R2)+,X0 //Yes, reload decimation counter
MOVE.W X0,X:(R2)+
ADDA #1,SP //Preserve N (needed if called
MOVE.W N,X:(SP) //from an ISR)
MOVE.W X:(R2)+,A //Load index of buffer head into
MOVE.W A1,N //address modifier register
INC.W A X:(R2)+,Y1 //Point to next buffer location
//and load buffer length
CMP.W Y1,A //If buffer head index = buffer
BNE MAIndexOK //length, then reset buffer head
CLR.W A1 //index
MAIndexOK: MOVE.W A1,X:(R2-2) //Save buffer head index
MOVE.W X:(R3+N),A1 //Load oldest buffer sample
MOVE.W Y0,X:(R3+N) //Save new input sample
MOVE.W X:(R2)+,Y0 //Load multiplier
MPY A1,Y0,A X:(R3)+,X0 //Accumulate oldest sample
REP Y1 //Accumulate the latest N samples
MAC X0,Y0,A X:(R3)+,X0
MOVE.W A,Y0 //Return accumulation result
MOVE.W Y0,X:(R2) //Save new output sample
MOVE.W X:(SP)-,X0 //Restore N
MOVE.W X0,N
MovAvgDone: RTS //Return from subroutine
}
/* -------------- MovAvg.c ends -------------- */
/* -------------- Usage example begins ------------- */
/* application specific constants */
#define MovAvgOrder 40 //Order of moving average filter
#define MovAvgDecimation 100 //Decimation factor for m.a.
/* application specific includes */
#include "MovAvg.h"
/* global variables */
MovAvgparams MAparams;
Frac16 MAdata[MovAvgOrder];
int FiltIn,MAOut;
/* initializations */
MAparams.decimation=MovAvgDecimation;
MAparams.multiplier=FRAC16((float)1/(MovAvgOrder+1));
MovAvgInit(MovAvgOrder,&MAparams,(Frac16*)&MAdata); //Initialize m.a. filter
/* Moving average filter function call */
MAOut=MovAvg(FiltIn,&MAparams,(Frac16*)&MAdata); //Call moving av. filter
/* -------------- Usage example ends ------------- */Sine and cosine functions through MacLaurin expansion
/* -------------- SinCos.h begins -------------- */
#ifndef __SINCOS_H__
#define __SINCOS_H__
#include "types.h"
/*---------------------------------------------------------------------------;
; This function computes the sine of an angle using the Maclaurin series ;
; expansion method. Input angle is signed fractional where -1 corresponds to ;
; -pi and +1 to +pi; output is signed fractional and the minimum negative ;
; value is limited to -1+2^(-15) (0x8001 hexadeximal) to avoid overflow in ;
; subsequent calculations. ;
; ;
; Input: Y0 = angle (signed fractional; -1 = -pi, +1 = +pi) ;
; ;
; Output: Y0 = sine (signed fractional) ;
; ;
; Registers modified: A, B, X0, Y ;
;---------------------------------------------------------------------------*/
asm Frac16 Sin(Frac16 angle);
/*---------------------------------------------------------------------------;
; This function computes the cosine of an angle using the Maclaurin series ;
; expansion method. Input angle is signed fractional where -1 corresponds to ;
; -pi and +1 to +pi; output is signed fractional and the minimum negative ;
; value is limited to -1+2^(-15) (0x8001 hexadeximal) to avoid overflow in ;
; subsequent calculations. ;
; ;
; Input: Y0 = angle (signed fractional; -1 = -pi, +1 = +pi) ;
; ;
; Output: Y0 = cosine (signed fractional) ;
; ;
; Registers modified: A, B, X0, Y ;
;---------------------------------------------------------------------------*/
asm Frac16 Cos(Frac16 angle);
#endif //ifndef __SINCOS_H__
/* -------------- SinCos.h ends -------------- */
/* -------------- SinCos.c begins -------------- */
/*---------------------------------------------------------------------------;
; This function computes the sine of an angle using the Maclaurin series ;
; expansion method. Input angle is signed fractional where -1 corresponds to ;
; -pi and +1 to +pi; output is signed fractional and the minimum negative ;
; value is limited to -1+2^(-15) (0x8001 hexadeximal) to avoid overflow in ;
; subsequent calculations. ;
; ;
; Input: Y0 = angle (signed fractional; -1 = -pi, +1 = +pi) ;
; ;
; Output: Y0 = sine (signed fractional) ;
; ;
; Registers modified: A, B, X0, Y ;
;---------------------------------------------------------------------------*/
asm Frac16 Sin(Frac16 angle){
CLR.W Y1 //Clear Y1
MOVE.W Y0,B //Compute absolute value of angle
ABS B
CMP.W #$4000,B //|angle| > pi/2?
BLT SinAngleOK //No, proceed
ADD.W #$8000,Y0 //Yes, add pi and set flag for
BFSET #$0001,Y1 //final negation of result
SinAngleOK: MPYR Y0,Y0,A //Compute angle squared
MOVE.W A,X0 //X0 = angle squared
MOVE.W #$0002,B //Compute Maclaurin expansion
MOVE.W #$FFE2,A
MACR X0,B1,A //A1 = first partial result
MOVE.W #$0150,B
MACR X0,A1,B //B1 = second partial result
MOVE.W #$F669,A
MACR X0,B1,A //A1 = third partial result
MOVE.W #$28CD,B
MACR X0,A1,B //B1 = fourth partial result
MOVE.W #$AD52,A
MACR X0,B1,A //A1 = fifth partial result
MOVE.W #$3244,B
MACR X0,A1,B //B1 = sixth partial result
MOVE.W #$0003,X0 //Required shift amount
MPYR B1,Y0,A //A = result / 8
CMP #$1000,A //If magnitude is maximum
BNE SinCheckSatL //saturate the result
MOVE.W #$7FFF,A
BRA SinValOK
SinCheckSatL: CMP.W #$F000,A
BNE SinNoSat
MOVE.W #$8001,A
BRA SinValOK
SinNoSat: ASLL.L X0,A //Shift to get correct result
//(Maclaurin coefficients are
//divided by 8 to avoid overflow)
SinValOK: BRCLR #$0001,Y1,Update_Sin //Negate result if |angle| was
NEG A //greater than pi/2
Update_Sin: MOVE.W A,Y0 //Save result
RTS //Return from subroutine
}
/*---------------------------------------------------------------------------;
; This function computes the cosine of an angle using the Maclaurin series ;
; expansion method. Input angle is signed fractional where -1 corresponds to ;
; -pi and +1 to +pi; output is signed fractional and the minimum negative ;
; value is limited to -1+2^(-15) (0x8001 hexadeximal) to avoid overflow in ;
; subsequent calculations. ;
; ;
; Input: Y0 = angle (signed fractional; -1 = -pi, +1 = +pi) ;
; ;
; Output: Y0 = cosine (signed fractional) ;
; ;
; Registers modified: A, B, X0, Y ;
;---------------------------------------------------------------------------*/
asm Frac16 Cos(Frac16 angle){
ADD.W #$4000,Y0 //Add pi/2 to angle
JSR Sin //Call sine function
RTS //Return from subroutine
}
/* -------------- SinCos.c ends -------------- */
/* -------------- Usage example begins ------------- */
/* application specific includes */
#include "SinCos.h"
/* global variables */
Frac16 Angle,cosphi,sinphi;
/* Function calls */
cosphi=Cos(Angle); //Compute sin and cos of angle
sinphi=Sin(Angle); //(for use in Park transforms)
/* -------------- Usage example ends ------------- */Clarke and Park direct and inverse transforms
/* -------------- CPTrans.h begins -------------- */
#ifndef __CPTRANS_H__
#define __CPTRANS_H__
#include "types.h"
#define Minus1Over2 0xC000
#define Root3Over2 0x6EDA
#define TwoOver3 0x5555
#define Minus1Over3 0xD555
#define OneOnRoot3 0x49E7
/*---------------------------------------------------------------------------;
; Clarke transform (Vuvw -> Vab) with constant amplitudes. ;
; For optimal results it is advised to call this function with saturation ;
; disabled. ;
; ;
; Input: (R2) = Vu ;
; (R2+1) = Vv ;
; (R2+2) = Vw ;
; (R3) = Destination memory location for Va ;
; (R3+1) = Destination memory location for Vb ;
; ;
; Output: None ;
; ;
; Registers modified: A, X0, Y ;
;---------------------------------------------------------------------------*/
asm void Clarke(Frac16 *Vuvw, Frac16 *Vab);
/*---------------------------------------------------------------------------;
; Inverse Clarke transform (Vab -> Vuvw) with constant amplitudes. ;
; For optimal results it is advised to call this function with saturation ;
; disabled. ;
; ;
; Input: (R2) = Va ;
; (R2+1) = Vb ;
; (R3) = Destination memory location for Vu ;
; (R3+1) = Destination memory location for Vv ;
; (R3+2) = Destination memory location for Vw ;
; ;
; Output: None ;
; ;
; Registers modified: A, B, X0, Y ;
;---------------------------------------------------------------------------*/
asm void InvClarke(Frac16 *Vab, Frac16 *Vuvw);
/*---------------------------------------------------------------------------;
; Park transform (Vab -> Vdq). ;
; For optimal results it is advised to call this function with saturation ;
; disabled. ;
; ;
; Input: Y0 = cos(phi) ;
; Y1 = sin(phi) ;
; (R2) = Va ;
; (R2+1) = Vb ;
; (R3) = Destination memory location for Vd ;
; (R3+1) = Destination memory location for Vq ;
; ;
; Output: None ;
; ;
; Registers modified: A, B, X0 ;
;---------------------------------------------------------------------------*/
asm void Park(Frac16 cosphi, Frac16 sinphi, Frac16 *Vab, Frac16 *Vdq);
/*---------------------------------------------------------------------------;
; Inverse Park transform (Vdq -> Vab). ;
; For optimal results it is advised to call this function with saturation ;
; disabled. ;
; ;
; Input: Y0 = cos(phi) ;
; Y1 = sin(phi) ;
; (R2) = Vd ;
; (R2+1) = Vq ;
; (R3) = Destination memory location for Va ;
; (R3+1) = Destination memory location for Vb ;
; ;
; Output: None ;
; ;
; Registers modified: A, B, X0 ;
;---------------------------------------------------------------------------*/
asm void InvPark(Frac16 cosphi, Frac16 sinphi, Frac16 *Vdq, Frac16 *Vab);
#endif //ifndef __CPTRANS_H__
/* -------------- CPTrans.h ends -------------- */
/* -------------- CPTrans.c begins -------------- */
/*---------------------------------------------------------------------------;
; Clarke transform (Vuvw -> Vab) with constant amplitudes. ;
; For optimal results it is advised to call this function with saturation ;
; disabled. ;
; ;
; Input: (R2) = Vu ;
; (R2+1) = Vv ;
; (R2+2) = Vw ;
; (R3) = Destination memory location for Va ;
; (R3+1) = Destination memory location for Vb ;
; ;
; Output: None ;
; ;
; Registers modified: A, X0, Y ;
;---------------------------------------------------------------------------*/
asm void Clarke(Frac16 *Vuvw, Frac16 *Vab){
MOVE.W X:(R2),X0 //X0 = Vu
MOVE.W #TwoOver3,Y0 //Y0 = 2/3
MPY X0,Y0,A //A = 2/3*Vu
MOVE.W #Minus1Over3,X0 //X0 = -1/3
MOVE.W X:(R2+1),Y0 //Y0 = Vv
MOVE.W X:(R2+2),Y1 //Y1 = Vw
MAC X0,Y0,A //A = 2/3*Vu-1/3*Vv
MAC X0,Y1,A //A = 2/3*Vu-1/3*Vv-1/3*Vw
MOVE.W A,X:(R3) //Va = 2/3*Vu-1/3*Vv-1/3*Vw
MOVE.W #OneOnRoot3,X0 //X0 = 1/sqrt(3)
MPY X0,Y0,A //A = 1/sqrt(3)*Vv
MACR -X0,Y1,A //A = 1/sqrt(3)*Vv-1/sqrt(3)*Vw
MOVE.W A,X:(R3+1) //Vb = 1/sqrt(3)*Vv-1/sqrt(3)*Vw
RTS //Return from subroutine
}
/*---------------------------------------------------------------------------;
; Inverse Clarke transform (Vab -> Vuvw) with constant amplitudes. ;
; For optimal results it is advised to call this function with saturation ;
; disabled. ;
; ;
; Input: (R2) = Va ;
; (R2+1) = Vb ;
; (R3) = Destination memory location for Vu ;
; (R3+1) = Destination memory location for Vv ;
; (R3+2) = Destination memory location for Vw ;
; ;
; Output: None ;
; ;
; Registers modified: A, B, X0, Y ;
;---------------------------------------------------------------------------*/
asm void InvClarke(Frac16 *Vab, Frac16 *Vuvw){
MOVE.W X:(R2),Y0 //Y0 = Va
MOVE.W X:(R2+1),Y1 //Y1 = Vb
MOVE.W Y0,X:(R3) //Vu = Va
MOVE.W #Minus1Over2,X0 //X0 = -1/2
MPY X0,Y0,A //A = -1/2*Va
TFR A,B //B = -1/2*Va
MOVE.W #Root3Over2,X0 //X0 = sqrt(3)/2
MAC X0,Y1,A //A = -1/2*Va+sqrt(3)/2*Vb
MAC -X0,Y1,B //B = -1/2*Va-sqrt(3)/2*Vb
MOVE.W A,X:(R3+1) //Vv = -1/2*Va+sqrt(3)/2*Vb
MOVE.W B,X:(R3+2) //Vw = -1/2*Va-sqrt(3)/2*Vb
RTS //Return from subroutine
}
/*---------------------------------------------------------------------------;
; Park transform (Vab -> Vdq). ;
; For optimal results it is advised to call this function with saturation ;
; disabled. ;
; ;
; Input: Y0 = cos(phi) ;
; Y1 = sin(phi) ;
; (R2) = Va ;
; (R2+1) = Vb ;
; (R3) = Destination memory location for Vd ;
; (R3+1) = Destination memory location for Vq ;
; ;
; Output: None ;
; ;
; Registers modified: A, B, X0 ;
;---------------------------------------------------------------------------*/
asm void Park(Frac16 cosphi, Frac16 sinphi, Frac16 *Vab, Frac16 *Vdq){
MOVE.W X:(R2),X0 //X0 = Va
MPY X0,Y0,A //A = Va*cos(phi)
MPY -X0,Y1,B //B = -Va*sin(phi)
MOVE.W X:(R2+1),X0 //X0 = Vb
MAC X0,Y1,A //A = Va*cos(phi)+Vb*sin(phi)
MAC X0,Y0,B //B = -Va*sin(phi)+Vb*cos(phi)
MOVE.W A,X:(R3) //Vd = Va*cos(phi)+Vb*sin(phi)
MOVE.W B,X:(R3+1) //Vq = -Va*sin(phi)+Vb*cos(phi)
RTS //Return from subroutine
}
/*---------------------------------------------------------------------------;
; Inverse Park transform (Vdq -> Vab). ;
; For optimal results it is advised to call this function with saturation ;
; disabled. ;
; ;
; Input: Y0 = cos(phi) ;
; Y1 = sin(phi) ;
; (R2) = Vd ;
; (R2+1) = Vq ;
; (R3) = Destination memory location for Va ;
; (R3+1) = Destination memory location for Vb ;
; ;
; Output: None ;
; ;
; Registers modified: A, B, X0 ;
;---------------------------------------------------------------------------*/
asm void InvPark(Frac16 cosphi, Frac16 sinphi, Frac16 *Vdq, Frac16 *Vab){
MOVE.W X:(R2),X0 //X0 = Vd
MPY X0,Y0,A //A = Vd*cos(phi)
MPY X0,Y1,B //B = Vd*sin(phi)
MOVE.W X:(R2+1),X0 //X0 = Vq
MAC -X0,Y1,A //A = Vd*cos(phi)-Vq*sin(phi)
MAC X0,Y0,B //B = Vd*sin(phi)+Vq*cos(phi)
MOVE.W A,X:(R3) //Vd = Vd*cos(phi)-Vq*sin(phi)
MOVE.W B,X:(R3+1) //Vq = Vd*sin(phi)+Vq*cos(phi)
RTS //Return from subroutine
}
/* -------------- CPTrans.c ends -------------- */
/* -------------- Usage example begins ------------- */
/* application specific includes */
#include "CPTrans.h"
/* global variables */
Frac16 cosphi,sinphi;
Frac16 Vuvw[3];
Frac16 Vab[2];
Frac16 Vdq[2];
Frac16 Vab1[2];
Frac16 Vuvw1[3];
/* Transform function calls */
//This example performs the full processing chain from three-phase fixed to
//two-phase rotating reference and back. In the end Vuvw and Vuvw1 should be
//equal save for rounding errors.
Clarke((Frac16*)&Vuvw,(Frac16*)&Vab);
Park(cosphi,sinphi,(Frac16*)&Vab,(Frac16*)&Vdq);
InvPark(cosphi,sinphi,(Frac16*)&Vdq,(Frac16*)&Vab1);
InvClarke((Frac16*)&Vab1,(Frac16*)&Vuvw1);
/* -------------- Usage example ends ------------- */Second order IIR filter with decimation
/* -------------- IIR2.h begins -------------- */
#ifndef __IIR2_H__
#define __IIR2_H__
#include "types.h"
typedef struct
{
unsigned int decimation;
Frac16 b0over2;
Frac16 b1over2;
Frac16 b2over2;
Frac16 ma1over2;
Frac16 ma2over2;
}IIR2params;
typedef struct
{
unsigned int deccnt;
Frac16 xkm1;
Frac16 xkm2;
Frac16 ykm1;
Frac16 ykm2;
}IIR2data;
/*---------------------------------------------------------------------------;
; Initialize decimation divider and past data of filter to zero. ;
; ;
; Input: A10 = Integral memory initialization value ;
; Y0 = Past input initialization value ;
; (R2) = Integral memory storage destination ;
; (R2+2) = Past input storage destination ;
; ;
; Output: None ;
; ;
; Registers modified: R2 ;
;---------------------------------------------------------------------------*/
asm void IIR2Init(IIR2data *data);
/*---------------------------------------------------------------------------;
; This function implements a second order IIR Butteworth filter. ;
; The result yk is computed as a function of current (xk) and past (xk-1, ;
; xk-2) inputs and past (yk-1, yk-2) outputs as follows: ;
; yk = b0*xk + b1*xk-1 + b2*xk-2 - a1*yk-1 - a2*yk-2. ;
; Filter coefficients bi and aj are stored halved to ensure their magnitude ;
; does not exceed 1 (coefficients magnitude can be at most equal to 2). ;
; For optimal results it is advised to call this function with saturation ;
; disabled. ;
; ;
; Input: Y0 = xk (input sample) ;
; (R2) = decimation (1 = no decimation, 2 = discard every other ;
; sample, and so on) ;
; (R2+1) = b0/2 ;
; (R2+2) = b1/2 ;
; (R2+3) = b2/2 ;
; (R2+4) = -a1/2 ;
; (R2+5) = -a2/2 ;
; (R3) = decimation counter (used internally for data decimation) ;
; (R3+1) = xk-1 ;
; (R3+2) = xk-2 ;
; (R3+3) = yk-1 ;
; (R3+4) = yk-2 ;
; ;
; Output: Y0 = yk ;
; ;
; Registers modified: A, X0, Y, R0, R3 ;
;---------------------------------------------------------------------------*/
asm Frac16 IIR2(Frac16 xk, IIR2params *params, IIR2data *data);
#endif //ifndef __IIR2_H__
/* -------------- IIR2.h ends -------------- */
/* -------------- IIR2.c begins -------------- */
/*---------------------------------------------------------------------------;
; Initialize decimation divider and past data of filter to zero. ;
; ;
; Input: A10 = Integral memory initialization value ;
; Y0 = Past input initialization value ;
; (R2) = Integral memory storage destination ;
; (R2+2) = Past input storage destination ;
; ;
; Output: None ;
; ;
; Registers modified: R2 ;
;---------------------------------------------------------------------------*/
asm void IIR2Init(IIR2data *data){
MOVE.W #0001,X:(R2) //Initialize decimation divider
ADDA #1,R2 //to execute the first call
REP #4 //Initialize past data to zero
CLR.W X:(R2)+
RTS //Return from subroutine
}
/*---------------------------------------------------------------------------;
; This function implements a second order IIR Butteworth filter. ;
; The result yk is computed as a function of current (xk) and past (xk-1, ;
; xk-2) inputs and past (yk-1, yk-2) outputs as follows: ;
; yk = b0*xk + b1*xk-1 + b2*xk-2 - a1*yk-1 - a2*yk-2. ;
; Filter coefficients bi and aj are stored halved to ensure their magnitude ;
; does not exceed 1 (coefficients magnitude can be at most equal to 2). ;
; For optimal results it is advised to call this function with saturation ;
; disabled. ;
; ;
; Input: Y0 = xk (input sample) ;
; (R2) = decimation (1 = no decimation, 2 = discard every other ;
; sample, and so on) ;
; (R2+1) = b0/2 ;
; (R2+2) = b1/2 ;
; (R2+3) = b2/2 ;
; (R2+4) = -a1/2 ;
; (R2+5) = -a2/2 ;
; (R3) = decimation counter (used internally for data decimation) ;
; (R3+1) = xk-1 ;
; (R3+2) = xk-2 ;
; (R3+3) = yk-1 ;
; (R3+4) = yk-2 ;
; ;
; Output: Y0 = yk ;
; ;
; Registers modified: A, X0, Y, R0, R3 ;
;---------------------------------------------------------------------------*/
asm Frac16 IIR2(Frac16 xk, IIR2params *params, IIR2data *data){
DEC.W X:(R3) //Decrement decimation counter
BEQ DoFilter //Decremented to zero?
MOVE.W X:(R3+3),Y0 //No, output old value and exit
BRA FilterDone
DoFilter: MOVE.W X:(R2),X0 //Yes, reload decimation counter
MOVE.W X0,X:(R3)+
ADDA #1,R2,R0 //Use R0 for parallel moves
MOVE.W Y0,Y1 //Save xk
MOVE.W X:(R0)+,X0 //b0/2
MPY Y0,X0,A X:(R0)+,Y0 X:(R3)+,X0 //b0/2*xk, b1/2, xk-1
MAC Y0,X0,A X:(R0)+,Y0 X:(R3)+,X0 //b1/2*xk-1, b2/2, xk-2
MAC Y0,X0,A X:(R0)+,Y0 X:(R3)+,X0 //b2/2*xk-2, -a2/2, yk-1
MAC Y0,X0,A X:(R0)+,Y0 X:(R3)-,X0 //-a2/2*yk-1, -a3/2, yk-2
MAC Y0,X0,A X:(R0)+,Y0 X:(R3)+,X0 //-a3/2*yk-2, n/a, yk-1
ASL A //Compensate for halved coeffs
MOVE.W A,Y0 //Saturate result if necessary
MOVE.W X0,X:(R3)- //yk-2=yk-1
MOVE.W Y0,X:(R3)- //yk-1=yk
ADDA #-1,R3 //Point to xk-1
MOVE.W X:(R3)+,X0 //xk-1
MOVE.W X0,X:(R3)- //xk-2=xk-1
MOVE.W Y1,X:(R3)+ //xk-1=xk
FilterDone: RTS //Return from subroutine
}
/* -------------- IIR2.c ends -------------- */
/* -------------- Usage example begins ------------- */
/* application specific constants */
#define IIRDec 160 //IIR filter decimation factor
#define IIRb0 0x0019 //IIR filter parameters: 128 Hz
#define IIRb1 0x0032 //lowpass (with 10 kHz sampling
#define IIRb2 0x0019 //frequency)
#define IIRa1 0x78BA
#define IIRa2 0xC6E1
/* application specific includes */
#include "IIR2.h"
/* global variables */
IIR2params IIRparams;
IIR2data IIRdata;
int FiltIn,IIROut;
/* initializations */
IIRparams.decimation=IIRDec; //Initialize filter decimation
IIRparams.b0over2=IIRb0; //Initialize filter parameters
IIRparams.b1over2=IIRb1;
IIRparams.b2over2=IIRb2;
IIRparams.ma1over2=IIRa1;
IIRparams.ma2over2=IIRa2;
IIR2Init(&IIRdata); //Initialize IIR filter past data
/* IIR filter function call */
IIROut=IIR2(FiltIn,&IIRparams,&IIRdata); //Call IIR filter
/* -------------- Usage example ends ------------- */Moving average filter with decimation
/* -------------- MovAvg.h begins -------------- */
#ifndef __MOVAVG_H__
#define __MOVAVG_H__
#include "types.h"
typedef struct
{
unsigned int decimation;
unsigned int deccnt;
unsigned int BufHeadPtr;
unsigned int BufLength;
Frac16 multiplier;
Frac16 ykm1;
}MovAvgparams;
/*---------------------------------------------------------------------------;
; Initialize buffer length, pointer and decimation divider of moving average ;
; filter; initialize memory buffer of moving average filter to all zeroes. ;
; ;
; Input: Y0 = buffer length ;
; (R2+1) = decimation counter (used internally for data decimation) ;
; (R2+2) = index of buffer head ;
; (R2+3) = buffer length (filter order) ;
; R3 = pointer to storage buffer for the past input samples ;
; ;
; Output: None ;
; ;
; Registers modified: R2 ;
;---------------------------------------------------------------------------*/
asm void MovAvgInit(unsigned int length, MovAvgparams *params, Frac16 *data);
/*---------------------------------------------------------------------------;
; This function implements a moving average Nth order FIR filter. ;
; The result yk is computed as the sum of the current and the past N input ;
; samples multiplied by the multiplier value. Canonical moving average ;
; filtering is obtained if multiplier = 1/(N+1). ;
; For optimal results it is advised to call this function with saturation ;
; disabled. ;
; ;
; Input: Y0 = xk (input sample) ;
; (R2) = decimation (1 = no decimation, 2 = discard every other ;
; sample, and so on) ;
; (R2+1) = decimation counter (used internally for data decimation) ;
; (R2+2) = index of buffer head ;
; (R2+3) = N (length of buffer and filter order) ;
; (R2+4) = fractional multiplier ;
; (R2+5) = yk-1 (past output) ;
; R3 = Pointer to storage buffer for the N past input samples ;
; ;
; Output: Y0 = yk ;
; ;
; Registers modified: A, X0, Y, R0, R3 ;
;---------------------------------------------------------------------------*/
asm Frac16 MovAvg(Frac16 xk, MovAvgparams *params, Frac16 *data);
#endif //ifndef __MOVAVG_H__
/* -------------- MovAvg.h ends -------------- */
/* -------------- MovAvg.c begins -------------- */
/*---------------------------------------------------------------------------;
; Initialize buffer length, pointer and decimation divider of moving average ;
; filter; initialize memory buffer of moving average filter to all zeroes. ;
; ;
; Input: Y0 = buffer length ;
; (R2+1) = decimation counter (used internally for data decimation) ;
; (R2+2) = index of buffer head ;
; (R2+3) = buffer length (filter order) ;
; R3 = pointer to storage buffer for the past input samples ;
; ;
; Output: None ;
; ;
; Registers modified: R2 ;
;---------------------------------------------------------------------------*/
asm void MovAvgInit(unsigned int length, MovAvgparams *params, Frac16 *data){
MOVE.W #0001,X:(R2+1) //Initialize decimation divider
CLR.W X:(R2+2) //Initialize buffer head pointer
MOVE.W Y0,X:(R2+3) //Initialize buffer length
REP Y0 //Initialize past data to zero
CLR.W X:(R3)+
RTS //Return from subroutine
}
/*---------------------------------------------------------------------------;
; This function implements a moving average Nth order FIR filter. ;
; The result yk is computed as the sum of the current and the past N input ;
; samples multiplied by the multiplier value. Canonical moving average ;
; filtering is obtained if multiplier = 1/(N+1). ;
; For optimal results it is advised to call this function with saturation ;
; disabled. ;
; ;
; Input: Y0 = xk (input sample) ;
; (R2) = decimation (1 = no decimation, 2 = discard every other ;
; sample, and so on) ;
; (R2+1) = decimation counter (used internally for data decimation) ;
; (R2+2) = index of buffer head ;
; (R2+3) = N (length of buffer and filter order) ;
; (R2+4) = fractional multiplier ;
; (R2+5) = yk-1 (past output) ;
; R3 = Pointer to storage buffer for the N past input samples ;
; ;
; Output: Y0 = yk ;
; ;
; Registers modified: A, X0, Y, R2, R3 ;
;---------------------------------------------------------------------------*/
asm Frac16 MovAvg(Frac16 xk, MovAvgparams *params, Frac16 *data){
DEC.W X:(R2+1) //Decrement decimation counter
BEQ DoMovAvg //Decremented to zero?
MOVE.W X:(R2+5),Y0 //No, output old value and exit
BRA MovAvgDone
DoMovAvg: MOVE.W X:(R2)+,X0 //Yes, reload decimation counter
MOVE.W X0,X:(R2)+
ADDA #1,SP //Preserve N (needed if called
MOVE.W N,X:(SP) //from an ISR)
MOVE.W X:(R2)+,A //Load index of buffer head into
MOVE.W A1,N //address modifier register
INC.W A X:(R2)+,Y1 //Point to next buffer location
//and load buffer length
CMP.W Y1,A //If buffer head index = buffer
BNE MAIndexOK //length, then reset buffer head
CLR.W A1 //index
MAIndexOK: MOVE.W A1,X:(R2-2) //Save buffer head index
MOVE.W X:(R3+N),A1 //Load oldest buffer sample
MOVE.W Y0,X:(R3+N) //Save new input sample
MOVE.W X:(R2)+,Y0 //Load multiplier
MPY A1,Y0,A X:(R3)+,X0 //Accumulate oldest sample
REP Y1 //Accumulate the latest N samples
MAC X0,Y0,A X:(R3)+,X0
MOVE.W A,Y0 //Return accumulation result
MOVE.W Y0,X:(R2) //Save new output sample
MOVE.W X:(SP)-,X0 //Restore N
MOVE.W X0,N
MovAvgDone: RTS //Return from subroutine
}
/* -------------- MovAvg.c ends -------------- */
/* -------------- Usage example begins ------------- */
/* application specific constants */
#define MovAvgOrder 40 //Order of moving average filter
#define MovAvgDecimation 100 //Decimation factor for m.a.
/* application specific includes */
#include "MovAvg.h"
/* global variables */
MovAvgparams MAparams;
Frac16 MAdata[MovAvgOrder];
int FiltIn,MAOut;
/* initializations */
MAparams.decimation=MovAvgDecimation;
MAparams.multiplier=FRAC16((float)1/(MovAvgOrder+1));
MovAvgInit(MovAvgOrder,&MAparams,(Frac16*)&MAdata); //Initialize m.a. filter
/* Moving average filter function call */
MAOut=MovAvg(FiltIn,&MAparams,(Frac16*)&MAdata); //Call moving av. filter
/* -------------- Usage example ends ------------- */32 bit Proportional-Integral (PI) Regulator
/* -------------- PI32.h begins -------------- */
#ifndef __PI32_H__
#define __PI32_H__
#include "types.h"
typedef struct
{
Frac16 c0;
Frac16 c1;
Frac16 IntSatH;
Frac16 IntSatL;
Frac16 kp;
UInt16 nkp;
Frac16 OutSatH;
Frac16 OutSatL;
}PI32params;
typedef struct
{
Frac32 Skm1;
Frac16 xkm1;
}PI32data;
/*---------------------------------------------------------------------------;
; Initialize integral memory and past input of PI regulator. ;
; ;
; Input: A10 = Integral memory initialization value ;
; Y0 = Past input initialization value ;
; (R2) = Integral memory storage destination ;
; (R2+2) = Past input storage destination ;
; ;
; Output: None ;
; ;
; Registers modified: R2 ;
;---------------------------------------------------------------------------*/
asm void PI32Init(Frac32 S, Frac16 xp, PI32data *data);
/*---------------------------------------------------------------------------;
; This function implements a 32 bit precision PI regulator with separate ;
; saturations on the integral portion and on the whole output. ;
; The routine first computes the integral portion at current sampling ;
; instant k (Sk) as Sk = Sk-1 + c0*xk + c1*xk-1. Sk is subsequently ;
; saturated between IntSatH and IntSatL and saved for later use at the next ;
; sampling instant. ;
; Next, proportional portion Pk is computed as kp*2^nkp*xk and saturated ;
; between -1 and +1. The two portions are added together and the result ;
; yk=Sk+Pk is saturated between OutSatH and OutSatL. ;
; Coefficients c0 and c1 are equal and amount to ki*Ts/2, where Ts is the ;
; sampling period and ki is the integral multiplier. The proportional ;
; multiplier kp*2^nkp is split into a fractional part and an exponent in ;
; order to allow values greater than unity. ;
; For optimal results it is advised to call this function with saturation ;
; disabled. ;
; ;
; Input: Y0 = xk (input sample of error signal) ;
; (R2) = c0 ;
; (R2+1) = c1 ;
; (R2+2) = IntSatH ;
; (R2+3) = IntSatL ;
; (R2+4) = kp ;
; (R2+5) = nkp ;
; (R2+6) = OutSatH ;
; (R2+7) = OutSatL ;
; (R3) = Sk-1 (32 bits) ;
; (R3+2) = xk-1 ;
; ;
; Output: A = yk (32 significant bits) ;
; ;
; Registers modified: A, B, X0, Y, R0 ;
;---------------------------------------------------------------------------*/
asm Frac32 PI32(Frac16 xk, PI32params *params, PI32data *data);
#endif //ifndef __PI32_H__
/* -------------- PI32.h ends -------------- */
/* -------------- PI32.c begins -------------- */
/*---------------------------------------------------------------------------;
; Initialize integral memory and past input of PI regulator. ;
; ;
; Input: A10 = Integral memory initialization value ;
; Y0 = Past input initialization value ;
; (R2) = Integral memory storage destination ;
; (R2+2) = Past input storage destination ;
; ;
; Output: None ;
; ;
; Registers modified: R2 ;
;---------------------------------------------------------------------------*/
asm void PI32Init(Frac32 S, Frac16 xp, PI32data *data){
MOVE.L A10,X:(R2)+ //Initialize integral memory
MOVE.W Y0,X:(R2) //Initialize past input
RTS //Return from subroutine
}
/*---------------------------------------------------------------------------;
; This function implements a 32 bit precision PI regulator with separate ;
; saturations on the integral portion and on the whole output. ;
; The routine first computes the integral portion at current sampling ;
; instant k (Sk) as Sk = Sk-1 + c0*xk + c1*xk-1. Sk is subsequently ;
; saturated between IntSatH and IntSatL and saved for later use at the next ;
; sampling instant. ;
; Next, proportional portion Pk is computed as kp*2^nkp*xk and saturated ;
; between -1 and +1. The two portions are added together and the result ;
; yk=Sk+Pk is saturated between OutSatH and OutSatL. ;
; Coefficients c0 and c1 are equal and amount to ki*Ts/2, where Ts is the ;
; sampling period and ki is the integral multiplier. The proportional ;
; multiplier kp*2^nkp is split into a fractional part and an exponent in ;
; order to allow values greater than unity. ;
; For optimal results it is advised to call this function with saturation ;
; disabled. ;
; ;
; Input: Y0 = xk (input sample of error signal) ;
; (R2) = c0 ;
; (R2+1) = c1 ;
; (R2+2) = IntSatH ;
; (R2+3) = IntSatL ;
; (R2+4) = kp ;
; (R2+5) = nkp ;
; (R2+6) = OutSatH ;
; (R2+7) = OutSatL ;
; (R3) = Sk-1 (32 bits) ;
; (R3+2) = xk-1 ;
; ;
; Output: A = yk (32 significant bits) ;
; ;
; Registers modified: A, B, X0, Y, R0 ;
;---------------------------------------------------------------------------*/
asm Frac32 PI32(Frac16 xk, PI32params *params, PI32data *data){
MOVE.W R2,X0
MOVE.W X0,R0
MOVE.L X:(R3)+,A //Load Sk-1
MOVE.W X:(R0)+,X0 //Load c0
MOVE.W Y0,Y1 //Save xk
MAC Y0,X0,A X:(R0)+,Y0 X:(R3)+,X0 //c0*xk, c1, xk-1
MAC Y0,X0,A X:(R0)+,Y0 X:(R3)-,X0 //c1*xk-1, IntSatH, decr. R3
MOVE.W Y0,B //Load IntSatH into B
CMP A,B Y1,X:(R3)+ //Check upper satur. and save xk
BGT CheckSatLo //If Sk<IntSatH no need for sat.
MOVE.W Y0,A //else saturate and increment R0
ADDA #1,R0 //to skip IntSatL in the
BRA IntSatOK //parameter structure
CheckSatLo: MOVE.W X:(R0)+,B //Load IntSatL
CMP A,B //Check lower saturation
BLE IntSatOK //If Sk>IntSatL no need for sat.
MOVE.W B1,A //else saturate
IntSatOK: ADDA #-3,R3,R3 //Point to Sk storage location
MOVE.L A10,X:(R3) //Save Sk
MOVE.W X:(R0)+,Y0 //Load kp
MPY Y0,Y1,B X:(R0)+,Y0 //kp*xk, nkp
CLB B,X0 //Number of leading bits of
CMP Y0,X0 //result greater or equal than
NOP //desired shift count?
BLT PropSat //No, saturate
ASLL.L Y0,B //Yes, shift
BRA PropSatOK
PropSat: TST B //Saturate proportional portion
BLT PropSatNeg //according to sign
MOVE.W #$7FFF,B
BRA PropSatOK
PropSatNeg: MOVE.W #$8000,B
PropSatOK: ADD B,A X:(R0)+,B //Compute yk, load OutSatH
CMP A,B X:(R0)+,X0 //Check upper sat., load OutSatL
BGT CheckSatOutLo //If yk<OutSatH no need for sat.
MOVE.W B1,A //else saturate
BRA OutSatOK
CheckSatOutLo: CMP X0,A //Check lower saturation
BGT OutSatOK //If yk>OutSatL no need for sat.
MOVE.W X0,A //else saturate
OutSatOK: RTS //Return from subroutine
}
/* -------------- PI32.c ends -------------- */
/* -------------- Usage example begins ------------- */
/* application specific constants */
#define PIc0 100
#define PIc1 100
#define PIIntSatH 15000
#define PIIntSatL -15000
#define PIkp 0x4000
#define PInkp 1
#define PIOutSatH 30000
#define PIOutSatL -30000
/* application specific includes */
#include "PI32.h"
/* global variables */
PI32params PIparams;
PI32data PIdata;
int PIIn,PIOut;
/* initializations */
PIparams.c0=PIc0; //Initialize PI regulator
PIparams.c1=PIc1; //parameters and past data
PIparams.IntSatH=PIIntSatH;
PIparams.IntSatL=PIIntSatL;
PIparams.kp=PIkp;
PIparams.nkp=PInkp;
PIparams.OutSatH=PIOutSatH;
PIparams.OutSatL=PIOutSatL;
PI32Init(0,0, &PIdata);
/* PI32 regulator function call */
PIOut=(int)(PI32(PIIn,&PIparams,&PIdata)>>16); //Call PI regulator
/* -------------- Usage example ends ------------- */
