## Sine and cosine functions through MacLaurin expansion

November 18, 2010 Coded in Mixed C and ASM for the Freescale DSP56F8xx
``````/* -------------- 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
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
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){
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

November 18, 20101 comment Coded in Mixed C and ASM for the Freescale DSP56F8xx
``````/* -------------- 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;
Frac16 Vab;
Frac16 Vdq;
Frac16 Vab1;
Frac16 Vuvw1;

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

November 18, 2010 Coded in Mixed C and ASM for the Freescale DSP56F8xx
``````/* -------------- 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
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

November 18, 2010 Coded in Mixed C and ASM for the Freescale DSP56F8xx
``````/* -------------- MovAvg.h begins -------------- */

#ifndef __MOVAVG_H__
#define __MOVAVG_H__

#include "types.h"

typedef struct
{
unsigned int decimation;
unsigned int deccnt;
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)
INC.W A X:(R2)+,Y1           //Point to next buffer location
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
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;
int FiltIn,MAOut;

/* initializations */
MAparams.decimation=MovAvgDecimation;
MAparams.multiplier=FRAC16((float)1/(MovAvgOrder+1));

/* Moving average filter function call */

/* -------------- Usage example ends ------------- */``````

## 32 bit Proportional-Integral (PI) Regulator

November 18, 20101 comment Coded in Mixed C and ASM for the Freescale DSP56F8xx
``````/* -------------- 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.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
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
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
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 ------------- */``````