DSPRelated.com
Online · Live + on demand
February 9-11, 2027
Price goes up Nov 1: register now to lock in today's rate
Register Now →
   Code Snippets

Biquad bandpass filter bank

● February 2, 2011 ● Coded in Mixed C and ASM for the SHARC
// Efficient implementation of biquad bandpass filter bank.
// This is the core computation for musical physical models ("modal models").
//
// This code takes 1.5 instructions to compute each floating-point bandpass filter;
// for a SHARC running at 3ns instruction cycle this is 4.5ns / biquad.
//
// Written by Lippold Haken of Haken Audio, 2010, using ADSP-21364 and VisualDSP++ C.
//
//  dmXmod - pointer to DM data memory
//  pmXmod - pointer to PM data memory
//  bfbXComputePairs - optimized computation of a bank of bandpass filters
//  bfbXCoef - coefficient computation for the bandpass filters

// CFDSP hardware-specific definitions.
#include "cfdsp21364.h"

// === Start of definitions for .h file
// The following structure definitions are shared by this code and by the calling code,
// so they should be moved to a common .h file.

// Data in dm.
typedef struct 
{
    #define bfb4FilterSections  64      // number of modes (number of bandpass filter sections)
    float bfb_Y[2][bfb4FilterSections];         // save state: y[n-1] and y[n-2]
} DmX_bfb;

// Data in pm.
typedef struct 
{
    // The bfb_C[] contains 3 coefficients for each biquad, with a pair of biquad's coefficients 
    // interleaved so that biquads can be processed in pairs by the optimized SIMD assembly loop.
    float bfb_C[ 3 * bfb4FilterSections ];      // bfb filter coefficients, see comment above
} PmX_bfb;

// Pointers to data, and a samples counter.
DmX_bfb *dmXmod;
PmX_bfb *pmXmod;
int sampInFrame;                    // sample counter

#define sr 48000                    // sample rate in Hz
#define Pi    (3.14159265)
#define ABS(x) ( ((x)>(0)) ? (x) : -(x) )
void sinCos( float fRadians, float *fSin, float *fCos ); // see Lippold Haken's sinCos code snippet 

// === End of definitions for .h file

// Parameter arrays for Biquad Filter Bank (modal physical modelling) synthesis.
//
// If P is the biquad pair number, and R=0/R=1 distinguishes the two biquads within the pair:
//  x[n]   coefficient for biquad k is at pmXmod->bfb_C[ 6*P + R + 0 ]; this is also -x[n-2] coefficient.
//  y[n-1] coefficient for biquad k is at pmXmod->bfb_C[ 6*P + R + 2 ]; this is scaled by 0.5 to avoid overflow
//  y[n-2] coefficient for biquad k is at pmXmod->bfb_C[ 6*P + R + 4 ]
// The first index to dmXmod->bfb_Y[] is based on lsbs of sample counter.
// The second index to dmXmod->bfb_Y[] is 2P+R.
// Note: R=0 always for even voices, R=1 always for odd voices.
#define bfb_Ynm1(P) &(dmXmod->bfb_Y[sampInFrame & 1][2*(P)])        // start in y[n-1][] array
#define bfb_Ynm2(P) &(dmXmod->bfb_Y[1-(sampInFrame & 1)][2*(P)])    // start in y[n-2][] array

void bfbXComputePairs(int biquadP, int pairs, 
            float inDiff0,          // x[n]-x[n-2] for even biquads
            float inDiff1,          // x[n]-x[n-2] for odd biquads
            float *fSum0,           // output for even biquad sum
            float *fSum1)           // output for odd biquad sum
// Sum the output of a sequence of at least 2 biquad pairs.
// For each of the sequence of pairs, the first (even) biquad in each pair has a common input "input0",
// and the second (odd) biquad of each pair has a (possibly different) common input "input1". 
{
    // Assmebly-coded bfb filter loop using floating-point math.
    // The core loop takes 3 instructions for 2 biquads, or 4.5ns per biquad.
    // We use the even-numbered biquads for one voice, the odd-numbered biquads for a second voice.
    asm(
    "#include <def21364.h>                          \n"
    "BIT SET MODE1 SIMD;                            \n"

    // For each biquad k:  (k=6*biquadP for R=0)
    //   y[k](n) = C0[k] * (x[k](n) - x[k](n-2))
    //           + C1[k] * 2 * y[k](n-1) 
    //           + C2[k] * y[k](n-2)
    //
    // Register usage for biquad k (R==0):
    //   F0       = C0[k], C1[k], and C2[k]
    //   F4       = input0-old_input0 aka x[k](n)-x[k](n-2)
    //   F6       = y[k](n-1), y[k](n-2)
    //   F8       = scratch for computing y[k](n)
    //   F10      = sum of even biquad's y[](n) in each biquad pair
    //   F12      = scratch for computing y[k](n)
    //   F13      = final value of y[k-2](n)
    //   DM(I4)   = C0[k], C1[k], and C2[k]
    //   PM(I10)  = read y[k](n-2) write to y[k-2](n)
    //   PM(I12)  = read y[k](n-1)
    // Register usage for biquad k+1 (R==1):
    //   SF0      = C0[k+1], C1[k+1], and C2[k+1]
    //   SF4      = input1-old_input1 aka x[k+1](n)-x[k+1](n-2)
    //   SF6      = y[k+1](n-1), y[k+1](n-2)
    //   SF8      = scratch for computing y[k+1](n)
    //   SF10     = sum of odd biquad's y[](n) in each biquad pair
    //   SF12     = scratch for computing y[k+1](n)
    //   SF13     = final value of y[k-1](n)
    //   DM(I4+1) = C0[k+1], C1[k+1], and C2[k+1]
    //   PM(I10+1)= read y[k+1](n-2) write y[k-1](n)
    //   PM(I12+1)= read y[k+1](n-1)
    // Register usage for even and odd biquads:
    //   M4=M12=2
    //   M10=+4
    //   M11=-2
    //
    // The three instruction loop below computes two biquads simultaneously, for k and k+1,
    // in each SIMD loop execution.  The comments are written just for biquad k;
    // the biquad k+1 code and comments are implied by SIMD mode.

    // Set pointer increments.
    "      M4=2;M10=4;M11=-2;M12=2;                 \n"

    // Loop priming, with k=0 (and SIMD k=1).

        // Initialize F10=SF10=0                    fetch C0[k]         
    "      R10=R10-R10,                             F0=DM(I4,M4);                           \n"

        // C0[k]*(x(n)-x(n-2))                      fetch C1[k]         fetch y[k](n-1)
    "      F8=F0*F4,                                F0=DM(I4,M4),       F6=PM(I12,M12);     \n"
        // C1[k] * y[k](n-1)                        fetch C2[k]         fetch y[k](n-2)
    "      F12=F0*F6,                               F0=DM(I4,M4),       F6=PM(I10,M12);     \n"
        // C2[k] * y[k](n-2)    biquad k sum        fetch C0[k+2]
    "      F12=F0*F6,           F8=F8+F12,          F0=DM(I4,M4);                           \n"

    // Main loop, for k = 2..K-2 by 2 (and SIMD for k = 1..K-1 by 2).
    "DO (PC, endx) UNTIL LCE;   \n"  

        // C0[k]*(x(n)-x(n-2))  F13 is y[k-2](n)    fetch C1[k]         fetch y[k](n-1)
    "      F8=F0*F4,            F13=F8+F12,         F0=DM(I4,M4),       F6=PM(I12,M12);     \n"
        // C1[k] * y[k](n-1)    output sum          fetch C2[k]         fetch y[k](n-2)
    "      F12=F0*F6,           F10=F10+F13,        F0=DM(I4,M4),       F6=PM(I10,M11);     \n"
        // C2[k] * y[k](n-2)    biquad k sum        fetch C0[k+2]       save y[k-2](n)
    "endx: F12=F0*F6,           F8=F8+F12,          F0=DM(I4,M4),       PM(I10,M10)=F13;    \n"

        //                      F13 is y[k-2](n)                                     
    "                           F13=F8+F12,                             modify(I10,M11);    \n"
        //                      output sum                              save y[k-2](n)
    "                           F10=F10+F13,                            PM(I10,M10)=F13;    \n"

    "BIT CLR MODE1 SIMD;                            \n"     // disable SIMD
    "      NOP;                                     \n"     // wait for SIMD disable

    // Output register list, each element: "=rN" (varName)
            :   "=R10" (*fSum0),                    // F10 has floating sum of even & odd biquads
                "=S10" (*fSum1)                     // SF10 has floating sum of even & odd biquads
    // Input register list, each element: "rN" (varName)
            :   "lcntr" (pairs-1),
                "R4" (inDiff0),                     // x[n]-x[n-2] for even biquads
                "S4" (inDiff1),                     // x[n]-x[n-2] for odd biquads
                "I4" (&pmXmod->bfb_C[6 * biquadP]), // coefficients for all biquads
                "I10" (bfb_Ynm2(biquadP)),          // y[n-2] for all biquads, updated to y[n]
                "I12" (bfb_Ynm1(biquadP))           // y[n-1] for all biquads
    // Clobbered register list:  
    // We must avoid "do not use" regs, Compiler and Library Manual p.1-245.
    // We try to use mostly scratch regs, Compiler and Library Manual p.1-248.
            :   "r0", "r4", "r6", "r8", "r10", "r12", "r13", 
                "i4", "i10", "i12", 
                "m4", "m10", "m11", "m12" );
}

void bfbXCoef(int biquadP, int biquadR, float freq, float amp, float bw)
// Compute coefficients for a biquad filter section of the floating-point Biquad Filter Bank routine.
{
    int startingIndex = 6 * biquadP + biquadR;          // 6 * biquad pair number (+1 if second in pair)
    float *pC = &pmXmod->bfb_C[ startingIndex ];        // index biquad filter coefficients array
    float *y0 = &dmXmod->bfb_Y[0][2*biquadP+biquadR];   // filter memory for biquad
    float *y1 = &dmXmod->bfb_Y[1][2*biquadP+biquadR];   // filter memory for biquad

    // Is center frequency is below aliasing-cutoff limit?
    if (freq != 0.0 && freq < 0.5 * sr)
    {
        // Compute sin and cos of the mode frequency.
        float phase = freq * 2. * Pi / sr;
        float fSin, fCos;
        sinCos(phase, &fSin, &fCos); // efficient sine and cosine; see Lippold Haken's DSPrelated code snippet.

        // These formulas are adapted from Robert Bristow-Johnson BLT biquad web posting.
        // I use the BPF with "constant skirt gain, peak gain = Q", with amplitude scaling of input coefficients.
        // Compute intermediate parameters, alpha and beta.
        //      alpha = sin(w0)/(2*Q)
        //      beta  = 1/(1+alpha)
        //      y[n] = (beta * sin(w0)/2   )  *  (x[n] - x[n-2])
        //           + (beta * 2*cos(w0)   )  *  y[n-1]
        //           + (beta * (alpha - 1) )  *  y[n-2] 
        // In addition, the input x-coefficients are scaled by the amplitude of the biquad --
        // I use **twice** the amplitude value.
        float alpha = fSin * bw / 2.0;      // this is sin/(2*q)
        float beta = 1.0 / (1.0 + alpha); 
        pC[0] = amp * beta * fSin;          // x[n] coefficient, and -x[n-2] coefficient
        pC[2] = beta * 2. * fCos;           // y[n-1] coefficient
        pC[4] = beta * (alpha - 1.0);       // y[n-2] coefficient
    }
    else
    {
        // Frequency of bfb filter is beyond aliasing frequency,
        // or if we are about to start a new note.
        // Zero bfb filter coefficients and zero out the filter memory.
        pC[0] = pC[2] = pC[4] = 0;
        *y0 = *y1 = 0;
    }
}

Fast SIMD sine and cosine

● January 31, 2011 ● Coded in Mixed C and ASM for the SHARC
// Fast sine and cosine for SHARC ADSP-21364, callable from VisualDSP++ C.
// Simultaneously compute sine and cosine of a given angle.
// Polynomial approximation developed by Flemming Pedersen of CERN.  
// This implementation by Lippold Haken of Haken Audio, March 2010, October 2012.
//
// C prototype for this function:
// typedef struct { float sin, cos; } SinCos;
// void sinCos( float fRadians, SinCos *result );
//
// 23 cycles to execute this function:
//    18 cycles for the computations
//     5 cycles for compiler environment overhead.

#include <def21364.h>

     // Data table for sinCos function.
    .section/dm/DOUBLE32 seg_dmda;
    sinCosData:
    .global sinCosData;
    .type sinCosData,STT_OBJECT;
    .var =

         // Integer 3 for "mod 4" operation.
         0x00000003, 0x00000003,

         // 1.0 for neutralizing final cos multiply.
         0x3F800000, 0x3F800000,

         // Coefficients for sin and cos polynomials.
         0xBB96BD89, // SQ7 for sin polynomial   -4.6002309092153379e-003
         0xBCA75707, // CQ6 for cos polynomial   -2.0427240364907607e-002
         0x3DA32F1D, // SQ5 for sin polynomial    7.9679708649230657e-002
         0x3E81D8BD, // CQ4 for cos polynomial    2.5360671639164339e-001
         0xBF255DDD, // SQ3 for sin polynomial   -6.4596348437163809e-001
         0xBF9DE9D1, // CQ2 for cos polynomial   -1.2336979844380824e+000
         0x3FC90FDB, // SQ1 for sin polynomial    1.5707963267948966e+000
         0x3F800000, // CQ0 for cos polynomial    1.0000000000000000e+000

         // Sign adjustment table, indexed by integer quadrant.
         0x3F800000, 0x3F800000,    //  1.0, 1.0
         0x3F800000, 0xBF800000,    //  1.0,-1.0
         0xBF800000, 0xBF800000,    // -1.0,-1.0
         0xBF800000, 0x3F800000;    // -1.0, 1.0

    sinCosData.end:

    .section/pm/DOUBLE32 seg_pmco;
    _sinCos:

    // Registers in VisualDSP environment:
        // M5,M13   = 0
        // M6,M14   = 1
        // M7,M15   = -1
        // F4       = first function argument (input angle)
        // R8       = pointer to address for storing sin,cos result
        // R0,R1,R2,R4,R8,R12,I4,I12,I13,M4 need not be preserved
        // I6,I7    = stack and frame pointers
        //            C calling routine has: "CJUMP (DB); DM(I7,M7)=R2; DM(I7,M7)=PC;"
        //            CJUMP does these operations: "R2=I6, I6=I7"
        //            Before exiting must do RFRAME: "I7=I6, I6=DM(0,I6)" 

    // Preamble.
        SF4 = F4;
        BIT SET MODE1 SIMD;

    // Pointer to constants.
        I4 = sinCosData;

    // Compute integer quadrant, fractional quadrant, fractional quadrant squared.
        // Quadrant "q" = fRadians * 2./Pi
        // Quadrant values (0. .. 4.) correspond to (0. .. 2Pi)
        R1 = 0x3F22F983;                            // F1 = 2./Pi
        F0 = F1 * F4,       I12 = DM(M7,I6);        // I12 is execution return address
        // Integer quadrant = round(q) mod 4    (0..3)
        // Since MODE1 TRUNC bit is zero, FIX implements round-to-nearest.
        R1 = FIX F0,        R2  = DM(I4,2);
        R2 = R1 AND R2,     F12 = DM(I4,2);
        // Fractional quadrant "Xq" = q - round(q)    (-.5 .. .5)
        // Fractional quadrant values (-.5 .. .5) correspond to (-Pi/4..Pi/4)
        F1 = FLOAT R1,      F4 = DM(I4,2);
        F1 = F0 - F1,       M4 = R2;
        // Set SZ if integer quadrant is even.
        R2 = LSHIFT R2 BY 31;
        // Fractional quadrant squared "Xq2" = Xq * Xq
        // Set SF1 = 1.0 to avoid final Xq multiply for cosine polynomial.
        F2 = F1 * F1,       F12 <-> SF1;

    // Registers involved in sin and cos computation:
        // F1     = fractional quadrant Xq 
        // SF1    = 1.0
        // F2,SF2 = fractional quadrant squared Xq2 
        // F4     = SQ7 
        // SF4    = CQ6 
        // M4     = integer quadrant
        // SZ     = indicates integer quadrant is even
        // DM(I4) = pairs of cosine and sine coefficients

    // Compute sine polynomial (PEx) and cosine polynomial (PEy).
        // Xq2*SQ7                          Xq2*CQ6
        F0 = F2 * F4,       F4 = DM(I4,2);
        // SQ5+Xq2*SQ7                      CQ4+Xq2*CQ6
        F0 = F0 + F4;
        // Xq2(SQ5+Xq2*SQ7)                 Xq2(CQ4+Xq2*CQ6)
        F0 = F0 * F2,       F4 = DM(I4,2);
        // SQ3+Xq2(SQ5+Xq2*SQ7)             CQ2+Xq2(CQ4+Xq2*CQ6)
        F0 = F0 + F4,       F4 = DM(I4,2);
        // Xq2(SQ3+Xq2(SQ5+Xq2*SQ7))        Xq2(CQ2+Xq2(CQ4+Xq2*CQ6))
        F0 = F0 * F2,       MODIFY(I4,M4);
        // SQ1+Xq2(SQ3+Xq2(SQ5+Xq2*SQ7)))   CQ0+Xq2(CQ2+Xq2(CQ4+Xq2*CQ6)))
        F0 = F0 + F4,       F4 = DM(M4,I4);
        // Xq(SQ1+Xq2(SQ3+Xq2(SQ5+Xq2*SQ7))))     no change on PEy
        F0 = F0 * F1,       I4 = R8;        // I4 is data return address 

    // Registers:
        // F0     = sine result so far
        // SF0    = cosine result so far
        // F4,SF4 = +/-1 for inverting sign for quadrant
        // SZ     = set for integer quadrant even

    // Invert and swap sin/cos according to integer quadrant:
    //  quadrant F0      SF0
    //  0 = 00   SIN     COS
    //  1 = 01   COS    -SIN   swap results, invert sine
    //  2 = 10  -SIN    -COS   invert signs
    //  3 = 11  -COS     SIN   swap results, invert cosine
        IF NOT SZ F0 <-> SF0;               // swap sin,cos if odd quadrant
        RFRAME;                             // stack frame management before exit
        JUMP (M14,I12) (DB), F0 = F0 * F4;  // negate sin/cos if needed
        DM(M5,I4) = F0;                     // store sin at I4, cos (SF0) at I4+1
        BIT CLR MODE1 SIMD;                 // end SIMD during JUMP (DB) stall

    ._sinCos.end:
    .global _sinCos;
    .type _sinCos,STT_FUNC;

Sine and cosine functions through MacLaurin expansion

Carlo Concari ● 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
                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

Carlo Concari ● November 18, 2010●1 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[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

Carlo Concari ● 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
                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

Carlo Concari ● 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 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

Carlo Concari ● November 18, 2010●1 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.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 ------------- */