/* -------------- 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 ------------- */
/* -------------- 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 ------------- */
/* -------------- 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 ------------- */
/* -------------- 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 ------------- */
/* -------------- 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 ------------- */
/* -------------- 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 ------------- */
