/* -------------- PI32.h begins -------------- */
#ifndef __PI32_H__
#define __PI32_H__
#include "types.h"
typedef struct
{
Frac16 c0;
Frac16 c1;
Frac16 IntSatH;
Frac16 IntSatL;
Frac16 kp;
UInt16 nkp;
Frac16 OutSatH;
Frac16 OutSatL;
}PI32params;
typedef struct
{
Frac32 Skm1;
Frac16 xkm1;
}PI32data;
/*---------------------------------------------------------------------------;
; Initialize integral memory and past input of PI regulator. ;
; ;
; Input: A10 = Integral memory initialization value ;
; Y0 = Past input initialization value ;
; (R2) = Integral memory storage destination ;
; (R2+2) = Past input storage destination ;
; ;
; Output: None ;
; ;
; Registers modified: R2 ;
;---------------------------------------------------------------------------*/
asm void PI32Init(Frac32 S, Frac16 xp, PI32data *data);
/*---------------------------------------------------------------------------;
; This function implements a 32 bit precision PI regulator with separate ;
; saturations on the integral portion and on the whole output. ;
; The routine first computes the integral portion at current sampling ;
; instant k (Sk) as Sk = Sk-1 + c0*xk + c1*xk-1. Sk is subsequently ;
; saturated between IntSatH and IntSatL and saved for later use at the next ;
; sampling instant. ;
; Next, proportional portion Pk is computed as kp*2^nkp*xk and saturated ;
; between -1 and +1. The two portions are added together and the result ;
; yk=Sk+Pk is saturated between OutSatH and OutSatL. ;
; Coefficients c0 and c1 are equal and amount to ki*Ts/2, where Ts is the ;
; sampling period and ki is the integral multiplier. The proportional ;
; multiplier kp*2^nkp is split into a fractional part and an exponent in ;
; order to allow values greater than unity. ;
; For optimal results it is advised to call this function with saturation ;
; disabled. ;
; ;
; Input: Y0 = xk (input sample of error signal) ;
; (R2) = c0 ;
; (R2+1) = c1 ;
; (R2+2) = IntSatH ;
; (R2+3) = IntSatL ;
; (R2+4) = kp ;
; (R2+5) = nkp ;
; (R2+6) = OutSatH ;
; (R2+7) = OutSatL ;
; (R3) = Sk-1 (32 bits) ;
; (R3+2) = xk-1 ;
; ;
; Output: A = yk (32 significant bits) ;
; ;
; Registers modified: A, B, X0, Y, R0 ;
;---------------------------------------------------------------------------*/
asm Frac32 PI32(Frac16 xk, PI32params *params, PI32data *data);
#endif //ifndef __PI32_H__
/* -------------- PI32.h ends -------------- */
/* -------------- PI32.c begins -------------- */
/*---------------------------------------------------------------------------;
; Initialize integral memory and past input of PI regulator. ;
; ;
; Input: A10 = Integral memory initialization value ;
; Y0 = Past input initialization value ;
; (R2) = Integral memory storage destination ;
; (R2+2) = Past input storage destination ;
; ;
; Output: None ;
; ;
; Registers modified: R2 ;
;---------------------------------------------------------------------------*/
asm void PI32Init(Frac32 S, Frac16 xp, PI32data *data){
MOVE.L A10,X:(R2)+ //Initialize integral memory
MOVE.W Y0,X:(R2) //Initialize past input
RTS //Return from subroutine
}
/*---------------------------------------------------------------------------;
; This function implements a 32 bit precision PI regulator with separate ;
; saturations on the integral portion and on the whole output. ;
; The routine first computes the integral portion at current sampling ;
; instant k (Sk) as Sk = Sk-1 + c0*xk + c1*xk-1. Sk is subsequently ;
; saturated between IntSatH and IntSatL and saved for later use at the next ;
; sampling instant. ;
; Next, proportional portion Pk is computed as kp*2^nkp*xk and saturated ;
; between -1 and +1. The two portions are added together and the result ;
; yk=Sk+Pk is saturated between OutSatH and OutSatL. ;
; Coefficients c0 and c1 are equal and amount to ki*Ts/2, where Ts is the ;
; sampling period and ki is the integral multiplier. The proportional ;
; multiplier kp*2^nkp is split into a fractional part and an exponent in ;
; order to allow values greater than unity. ;
; For optimal results it is advised to call this function with saturation ;
; disabled. ;
; ;
; Input: Y0 = xk (input sample of error signal) ;
; (R2) = c0 ;
; (R2+1) = c1 ;
; (R2+2) = IntSatH ;
; (R2+3) = IntSatL ;
; (R2+4) = kp ;
; (R2+5) = nkp ;
; (R2+6) = OutSatH ;
; (R2+7) = OutSatL ;
; (R3) = Sk-1 (32 bits) ;
; (R3+2) = xk-1 ;
; ;
; Output: A = yk (32 significant bits) ;
; ;
; Registers modified: A, B, X0, Y, R0 ;
;---------------------------------------------------------------------------*/
asm Frac32 PI32(Frac16 xk, PI32params *params, PI32data *data){
MOVE.W R2,X0
MOVE.W X0,R0
MOVE.L X:(R3)+,A //Load Sk-1
MOVE.W X:(R0)+,X0 //Load c0
MOVE.W Y0,Y1 //Save xk
MAC Y0,X0,A X:(R0)+,Y0 X:(R3)+,X0 //c0*xk, c1, xk-1
MAC Y0,X0,A X:(R0)+,Y0 X:(R3)-,X0 //c1*xk-1, IntSatH, decr. R3
MOVE.W Y0,B //Load IntSatH into B
CMP A,B Y1,X:(R3)+ //Check upper satur. and save xk
BGT CheckSatLo //If Sk<IntSatH no need for sat.
MOVE.W Y0,A //else saturate and increment R0
ADDA #1,R0 //to skip IntSatL in the
BRA IntSatOK //parameter structure
CheckSatLo: MOVE.W X:(R0)+,B //Load IntSatL
CMP A,B //Check lower saturation
BLE IntSatOK //If Sk>IntSatL no need for sat.
MOVE.W B1,A //else saturate
IntSatOK: ADDA #-3,R3,R3 //Point to Sk storage location
MOVE.L A10,X:(R3) //Save Sk
MOVE.W X:(R0)+,Y0 //Load kp
MPY Y0,Y1,B X:(R0)+,Y0 //kp*xk, nkp
CLB B,X0 //Number of leading bits of
CMP Y0,X0 //result greater or equal than
NOP //desired shift count?
BLT PropSat //No, saturate
ASLL.L Y0,B //Yes, shift
BRA PropSatOK
PropSat: TST B //Saturate proportional portion
BLT PropSatNeg //according to sign
MOVE.W #$7FFF,B
BRA PropSatOK
PropSatNeg: MOVE.W #$8000,B
PropSatOK: ADD B,A X:(R0)+,B //Compute yk, load OutSatH
CMP A,B X:(R0)+,X0 //Check upper sat., load OutSatL
BGT CheckSatOutLo //If yk<OutSatH no need for sat.
MOVE.W B1,A //else saturate
BRA OutSatOK
CheckSatOutLo: CMP X0,A //Check lower saturation
BGT OutSatOK //If yk>OutSatL no need for sat.
MOVE.W X0,A //else saturate
OutSatOK: RTS //Return from subroutine
}
/* -------------- PI32.c ends -------------- */
/* -------------- Usage example begins ------------- */
/* application specific constants */
#define PIc0 100
#define PIc1 100
#define PIIntSatH 15000
#define PIIntSatL -15000
#define PIkp 0x4000
#define PInkp 1
#define PIOutSatH 30000
#define PIOutSatL -30000
/* application specific includes */
#include "PI32.h"
/* global variables */
PI32params PIparams;
PI32data PIdata;
int PIIn,PIOut;
/* initializations */
PIparams.c0=PIc0; //Initialize PI regulator
PIparams.c1=PIc1; //parameters and past data
PIparams.IntSatH=PIIntSatH;
PIparams.IntSatL=PIIntSatL;
PIparams.kp=PIkp;
PIparams.nkp=PInkp;
PIparams.OutSatH=PIOutSatH;
PIparams.OutSatL=PIOutSatL;
PI32Init(0,0, &PIdata);
/* PI32 regulator function call */
PIOut=(int)(PI32(PIIn,&PIparams,&PIdata)>>16); //Call PI regulator
/* -------------- Usage example ends ------------- */
/* -------------- SinCos.h begins -------------- */
#ifndef __SINCOS_H__
#define __SINCOS_H__
#include "types.h"
/*---------------------------------------------------------------------------;
; This function computes the sine of an angle using the Maclaurin series ;
; expansion method. Input angle is signed fractional where -1 corresponds to ;
; -pi and +1 to +pi; output is signed fractional and the minimum negative ;
; value is limited to -1+2^(-15) (0x8001 hexadeximal) to avoid overflow in ;
; subsequent calculations. ;
; ;
; Input: Y0 = angle (signed fractional; -1 = -pi, +1 = +pi) ;
; ;
; Output: Y0 = sine (signed fractional) ;
; ;
; Registers modified: A, B, X0, Y ;
;---------------------------------------------------------------------------*/
asm Frac16 Sin(Frac16 angle);
/*---------------------------------------------------------------------------;
; This function computes the cosine of an angle using the Maclaurin series ;
; expansion method. Input angle is signed fractional where -1 corresponds to ;
; -pi and +1 to +pi; output is signed fractional and the minimum negative ;
; value is limited to -1+2^(-15) (0x8001 hexadeximal) to avoid overflow in ;
; subsequent calculations. ;
; ;
; Input: Y0 = angle (signed fractional; -1 = -pi, +1 = +pi) ;
; ;
; Output: Y0 = cosine (signed fractional) ;
; ;
; Registers modified: A, B, X0, Y ;
;---------------------------------------------------------------------------*/
asm Frac16 Cos(Frac16 angle);
#endif //ifndef __SINCOS_H__
/* -------------- SinCos.h ends -------------- */
/* -------------- SinCos.c begins -------------- */
/*---------------------------------------------------------------------------;
; This function computes the sine of an angle using the Maclaurin series ;
; expansion method. Input angle is signed fractional where -1 corresponds to ;
; -pi and +1 to +pi; output is signed fractional and the minimum negative ;
; value is limited to -1+2^(-15) (0x8001 hexadeximal) to avoid overflow in ;
; subsequent calculations. ;
; ;
; Input: Y0 = angle (signed fractional; -1 = -pi, +1 = +pi) ;
; ;
; Output: Y0 = sine (signed fractional) ;
; ;
; Registers modified: A, B, X0, Y ;
;---------------------------------------------------------------------------*/
asm Frac16 Sin(Frac16 angle){
CLR.W Y1 //Clear Y1
MOVE.W Y0,B //Compute absolute value of angle
ABS B
CMP.W #$4000,B //|angle| > pi/2?
BLT SinAngleOK //No, proceed
ADD.W #$8000,Y0 //Yes, add pi and set flag for
BFSET #$0001,Y1 //final negation of result
SinAngleOK: MPYR Y0,Y0,A //Compute angle squared
MOVE.W A,X0 //X0 = angle squared
MOVE.W #$0002,B //Compute Maclaurin expansion
MOVE.W #$FFE2,A
MACR X0,B1,A //A1 = first partial result
MOVE.W #$0150,B
MACR X0,A1,B //B1 = second partial result
MOVE.W #$F669,A
MACR X0,B1,A //A1 = third partial result
MOVE.W #$28CD,B
MACR X0,A1,B //B1 = fourth partial result
MOVE.W #$AD52,A
MACR X0,B1,A //A1 = fifth partial result
MOVE.W #$3244,B
MACR X0,A1,B //B1 = sixth partial result
MOVE.W #$0003,X0 //Required shift amount
MPYR B1,Y0,A //A = result / 8
CMP #$1000,A //If magnitude is maximum
BNE SinCheckSatL //saturate the result
MOVE.W #$7FFF,A
BRA SinValOK
SinCheckSatL: CMP.W #$F000,A
BNE SinNoSat
MOVE.W #$8001,A
BRA SinValOK
SinNoSat: ASLL.L X0,A //Shift to get correct result
//(Maclaurin coefficients are
//divided by 8 to avoid overflow)
SinValOK: BRCLR #$0001,Y1,Update_Sin //Negate result if |angle| was
NEG A //greater than pi/2
Update_Sin: MOVE.W A,Y0 //Save result
RTS //Return from subroutine
}
/*---------------------------------------------------------------------------;
; This function computes the cosine of an angle using the Maclaurin series ;
; expansion method. Input angle is signed fractional where -1 corresponds to ;
; -pi and +1 to +pi; output is signed fractional and the minimum negative ;
; value is limited to -1+2^(-15) (0x8001 hexadeximal) to avoid overflow in ;
; subsequent calculations. ;
; ;
; Input: Y0 = angle (signed fractional; -1 = -pi, +1 = +pi) ;
; ;
; Output: Y0 = cosine (signed fractional) ;
; ;
; Registers modified: A, B, X0, Y ;
;---------------------------------------------------------------------------*/
asm Frac16 Cos(Frac16 angle){
ADD.W #$4000,Y0 //Add pi/2 to angle
JSR Sin //Call sine function
RTS //Return from subroutine
}
/* -------------- SinCos.c ends -------------- */
/* -------------- Usage example begins ------------- */
/* application specific includes */
#include "SinCos.h"
/* global variables */
Frac16 Angle,cosphi,sinphi;
/* Function calls */
cosphi=Cos(Angle); //Compute sin and cos of angle
sinphi=Sin(Angle); //(for use in Park transforms)
/* -------------- Usage example ends ------------- */
/* -------------- MovAvg.h begins -------------- */
#ifndef __MOVAVG_H__
#define __MOVAVG_H__
#include "types.h"
typedef struct
{
unsigned int decimation;
unsigned int deccnt;
unsigned int BufHeadPtr;
unsigned int BufLength;
Frac16 multiplier;
Frac16 ykm1;
}MovAvgparams;
/*---------------------------------------------------------------------------;
; Initialize buffer length, pointer and decimation divider of moving average ;
; filter; initialize memory buffer of moving average filter to all zeroes. ;
; ;
; Input: Y0 = buffer length ;
; (R2+1) = decimation counter (used internally for data decimation) ;
; (R2+2) = index of buffer head ;
; (R2+3) = buffer length (filter order) ;
; R3 = pointer to storage buffer for the past input samples ;
; ;
; Output: None ;
; ;
; Registers modified: R2 ;
;---------------------------------------------------------------------------*/
asm void MovAvgInit(unsigned int length, MovAvgparams *params, Frac16 *data);
/*---------------------------------------------------------------------------;
; This function implements a moving average Nth order FIR filter. ;
; The result yk is computed as the sum of the current and the past N input ;
; samples multiplied by the multiplier value. Canonical moving average ;
; filtering is obtained if multiplier = 1/(N+1). ;
; For optimal results it is advised to call this function with saturation ;
; disabled. ;
; ;
; Input: Y0 = xk (input sample) ;
; (R2) = decimation (1 = no decimation, 2 = discard every other ;
; sample, and so on) ;
; (R2+1) = decimation counter (used internally for data decimation) ;
; (R2+2) = index of buffer head ;
; (R2+3) = N (length of buffer and filter order) ;
; (R2+4) = fractional multiplier ;
; (R2+5) = yk-1 (past output) ;
; R3 = Pointer to storage buffer for the N past input samples ;
; ;
; Output: Y0 = yk ;
; ;
; Registers modified: A, X0, Y, R0, R3 ;
;---------------------------------------------------------------------------*/
asm Frac16 MovAvg(Frac16 xk, MovAvgparams *params, Frac16 *data);
#endif //ifndef __MOVAVG_H__
/* -------------- MovAvg.h ends -------------- */
/* -------------- MovAvg.c begins -------------- */
/*---------------------------------------------------------------------------;
; Initialize buffer length, pointer and decimation divider of moving average ;
; filter; initialize memory buffer of moving average filter to all zeroes. ;
; ;
; Input: Y0 = buffer length ;
; (R2+1) = decimation counter (used internally for data decimation) ;
; (R2+2) = index of buffer head ;
; (R2+3) = buffer length (filter order) ;
; R3 = pointer to storage buffer for the past input samples ;
; ;
; Output: None ;
; ;
; Registers modified: R2 ;
;---------------------------------------------------------------------------*/
asm void MovAvgInit(unsigned int length, MovAvgparams *params, Frac16 *data){
MOVE.W #0001,X:(R2+1) //Initialize decimation divider
CLR.W X:(R2+2) //Initialize buffer head pointer
MOVE.W Y0,X:(R2+3) //Initialize buffer length
REP Y0 //Initialize past data to zero
CLR.W X:(R3)+
RTS //Return from subroutine
}
/*---------------------------------------------------------------------------;
; This function implements a moving average Nth order FIR filter. ;
; The result yk is computed as the sum of the current and the past N input ;
; samples multiplied by the multiplier value. Canonical moving average ;
; filtering is obtained if multiplier = 1/(N+1). ;
; For optimal results it is advised to call this function with saturation ;
; disabled. ;
; ;
; Input: Y0 = xk (input sample) ;
; (R2) = decimation (1 = no decimation, 2 = discard every other ;
; sample, and so on) ;
; (R2+1) = decimation counter (used internally for data decimation) ;
; (R2+2) = index of buffer head ;
; (R2+3) = N (length of buffer and filter order) ;
; (R2+4) = fractional multiplier ;
; (R2+5) = yk-1 (past output) ;
; R3 = Pointer to storage buffer for the N past input samples ;
; ;
; Output: Y0 = yk ;
; ;
; Registers modified: A, X0, Y, R2, R3 ;
;---------------------------------------------------------------------------*/
asm Frac16 MovAvg(Frac16 xk, MovAvgparams *params, Frac16 *data){
DEC.W X:(R2+1) //Decrement decimation counter
BEQ DoMovAvg //Decremented to zero?
MOVE.W X:(R2+5),Y0 //No, output old value and exit
BRA MovAvgDone
DoMovAvg: MOVE.W X:(R2)+,X0 //Yes, reload decimation counter
MOVE.W X0,X:(R2)+
ADDA #1,SP //Preserve N (needed if called
MOVE.W N,X:(SP) //from an ISR)
MOVE.W X:(R2)+,A //Load index of buffer head into
MOVE.W A1,N //address modifier register
INC.W A X:(R2)+,Y1 //Point to next buffer location
//and load buffer length
CMP.W Y1,A //If buffer head index = buffer
BNE MAIndexOK //length, then reset buffer head
CLR.W A1 //index
MAIndexOK: MOVE.W A1,X:(R2-2) //Save buffer head index
MOVE.W X:(R3+N),A1 //Load oldest buffer sample
MOVE.W Y0,X:(R3+N) //Save new input sample
MOVE.W X:(R2)+,Y0 //Load multiplier
MPY A1,Y0,A X:(R3)+,X0 //Accumulate oldest sample
REP Y1 //Accumulate the latest N samples
MAC X0,Y0,A X:(R3)+,X0
MOVE.W A,Y0 //Return accumulation result
MOVE.W Y0,X:(R2) //Save new output sample
MOVE.W X:(SP)-,X0 //Restore N
MOVE.W X0,N
MovAvgDone: RTS //Return from subroutine
}
/* -------------- MovAvg.c ends -------------- */
/* -------------- Usage example begins ------------- */
/* application specific constants */
#define MovAvgOrder 40 //Order of moving average filter
#define MovAvgDecimation 100 //Decimation factor for m.a.
/* application specific includes */
#include "MovAvg.h"
/* global variables */
MovAvgparams MAparams;
Frac16 MAdata[MovAvgOrder];
int FiltIn,MAOut;
/* initializations */
MAparams.decimation=MovAvgDecimation;
MAparams.multiplier=FRAC16((float)1/(MovAvgOrder+1));
MovAvgInit(MovAvgOrder,&MAparams,(Frac16*)&MAdata); //Initialize m.a. filter
/* Moving average filter function call */
MAOut=MovAvg(FiltIn,&MAparams,(Frac16*)&MAdata); //Call moving av. filter
/* -------------- Usage example ends ------------- */
// 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;
}
}
// 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;
}
}
/* -------------- SinCos.h begins -------------- */
#ifndef __SINCOS_H__
#define __SINCOS_H__
#include "types.h"
/*---------------------------------------------------------------------------;
; This function computes the sine of an angle using the Maclaurin series ;
; expansion method. Input angle is signed fractional where -1 corresponds to ;
; -pi and +1 to +pi; output is signed fractional and the minimum negative ;
; value is limited to -1+2^(-15) (0x8001 hexadeximal) to avoid overflow in ;
; subsequent calculations. ;
; ;
; Input: Y0 = angle (signed fractional; -1 = -pi, +1 = +pi) ;
; ;
; Output: Y0 = sine (signed fractional) ;
; ;
; Registers modified: A, B, X0, Y ;
;---------------------------------------------------------------------------*/
asm Frac16 Sin(Frac16 angle);
/*---------------------------------------------------------------------------;
; This function computes the cosine of an angle using the Maclaurin series ;
; expansion method. Input angle is signed fractional where -1 corresponds to ;
; -pi and +1 to +pi; output is signed fractional and the minimum negative ;
; value is limited to -1+2^(-15) (0x8001 hexadeximal) to avoid overflow in ;
; subsequent calculations. ;
; ;
; Input: Y0 = angle (signed fractional; -1 = -pi, +1 = +pi) ;
; ;
; Output: Y0 = cosine (signed fractional) ;
; ;
; Registers modified: A, B, X0, Y ;
;---------------------------------------------------------------------------*/
asm Frac16 Cos(Frac16 angle);
#endif //ifndef __SINCOS_H__
/* -------------- SinCos.h ends -------------- */
/* -------------- SinCos.c begins -------------- */
/*---------------------------------------------------------------------------;
; This function computes the sine of an angle using the Maclaurin series ;
; expansion method. Input angle is signed fractional where -1 corresponds to ;
; -pi and +1 to +pi; output is signed fractional and the minimum negative ;
; value is limited to -1+2^(-15) (0x8001 hexadeximal) to avoid overflow in ;
; subsequent calculations. ;
; ;
; Input: Y0 = angle (signed fractional; -1 = -pi, +1 = +pi) ;
; ;
; Output: Y0 = sine (signed fractional) ;
; ;
; Registers modified: A, B, X0, Y ;
;---------------------------------------------------------------------------*/
asm Frac16 Sin(Frac16 angle){
CLR.W Y1 //Clear Y1
MOVE.W Y0,B //Compute absolute value of angle
ABS B
CMP.W #$4000,B //|angle| > pi/2?
BLT SinAngleOK //No, proceed
ADD.W #$8000,Y0 //Yes, add pi and set flag for
BFSET #$0001,Y1 //final negation of result
SinAngleOK: MPYR Y0,Y0,A //Compute angle squared
MOVE.W A,X0 //X0 = angle squared
MOVE.W #$0002,B //Compute Maclaurin expansion
MOVE.W #$FFE2,A
MACR X0,B1,A //A1 = first partial result
MOVE.W #$0150,B
MACR X0,A1,B //B1 = second partial result
MOVE.W #$F669,A
MACR X0,B1,A //A1 = third partial result
MOVE.W #$28CD,B
MACR X0,A1,B //B1 = fourth partial result
MOVE.W #$AD52,A
MACR X0,B1,A //A1 = fifth partial result
MOVE.W #$3244,B
MACR X0,A1,B //B1 = sixth partial result
MOVE.W #$0003,X0 //Required shift amount
MPYR B1,Y0,A //A = result / 8
CMP #$1000,A //If magnitude is maximum
BNE SinCheckSatL //saturate the result
MOVE.W #$7FFF,A
BRA SinValOK
SinCheckSatL: CMP.W #$F000,A
BNE SinNoSat
MOVE.W #$8001,A
BRA SinValOK
SinNoSat: ASLL.L X0,A //Shift to get correct result
//(Maclaurin coefficients are
//divided by 8 to avoid overflow)
SinValOK: BRCLR #$0001,Y1,Update_Sin //Negate result if |angle| was
NEG A //greater than pi/2
Update_Sin: MOVE.W A,Y0 //Save result
RTS //Return from subroutine
}
/*---------------------------------------------------------------------------;
; This function computes the cosine of an angle using the Maclaurin series ;
; expansion method. Input angle is signed fractional where -1 corresponds to ;
; -pi and +1 to +pi; output is signed fractional and the minimum negative ;
; value is limited to -1+2^(-15) (0x8001 hexadeximal) to avoid overflow in ;
; subsequent calculations. ;
; ;
; Input: Y0 = angle (signed fractional; -1 = -pi, +1 = +pi) ;
; ;
; Output: Y0 = cosine (signed fractional) ;
; ;
; Registers modified: A, B, X0, Y ;
;---------------------------------------------------------------------------*/
asm Frac16 Cos(Frac16 angle){
ADD.W #$4000,Y0 //Add pi/2 to angle
JSR Sin //Call sine function
RTS //Return from subroutine
}
/* -------------- SinCos.c ends -------------- */
/* -------------- Usage example begins ------------- */
/* application specific includes */
#include "SinCos.h"
/* global variables */
Frac16 Angle,cosphi,sinphi;
/* Function calls */
cosphi=Cos(Angle); //Compute sin and cos of angle
sinphi=Sin(Angle); //(for use in Park transforms)
/* -------------- Usage example ends ------------- */
/* -------------- MovAvg.h begins -------------- */
#ifndef __MOVAVG_H__
#define __MOVAVG_H__
#include "types.h"
typedef struct
{
unsigned int decimation;
unsigned int deccnt;
unsigned int BufHeadPtr;
unsigned int BufLength;
Frac16 multiplier;
Frac16 ykm1;
}MovAvgparams;
/*---------------------------------------------------------------------------;
; Initialize buffer length, pointer and decimation divider of moving average ;
; filter; initialize memory buffer of moving average filter to all zeroes. ;
; ;
; Input: Y0 = buffer length ;
; (R2+1) = decimation counter (used internally for data decimation) ;
; (R2+2) = index of buffer head ;
; (R2+3) = buffer length (filter order) ;
; R3 = pointer to storage buffer for the past input samples ;
; ;
; Output: None ;
; ;
; Registers modified: R2 ;
;---------------------------------------------------------------------------*/
asm void MovAvgInit(unsigned int length, MovAvgparams *params, Frac16 *data);
/*---------------------------------------------------------------------------;
; This function implements a moving average Nth order FIR filter. ;
; The result yk is computed as the sum of the current and the past N input ;
; samples multiplied by the multiplier value. Canonical moving average ;
; filtering is obtained if multiplier = 1/(N+1). ;
; For optimal results it is advised to call this function with saturation ;
; disabled. ;
; ;
; Input: Y0 = xk (input sample) ;
; (R2) = decimation (1 = no decimation, 2 = discard every other ;
; sample, and so on) ;
; (R2+1) = decimation counter (used internally for data decimation) ;
; (R2+2) = index of buffer head ;
; (R2+3) = N (length of buffer and filter order) ;
; (R2+4) = fractional multiplier ;
; (R2+5) = yk-1 (past output) ;
; R3 = Pointer to storage buffer for the N past input samples ;
; ;
; Output: Y0 = yk ;
; ;
; Registers modified: A, X0, Y, R0, R3 ;
;---------------------------------------------------------------------------*/
asm Frac16 MovAvg(Frac16 xk, MovAvgparams *params, Frac16 *data);
#endif //ifndef __MOVAVG_H__
/* -------------- MovAvg.h ends -------------- */
/* -------------- MovAvg.c begins -------------- */
/*---------------------------------------------------------------------------;
; Initialize buffer length, pointer and decimation divider of moving average ;
; filter; initialize memory buffer of moving average filter to all zeroes. ;
; ;
; Input: Y0 = buffer length ;
; (R2+1) = decimation counter (used internally for data decimation) ;
; (R2+2) = index of buffer head ;
; (R2+3) = buffer length (filter order) ;
; R3 = pointer to storage buffer for the past input samples ;
; ;
; Output: None ;
; ;
; Registers modified: R2 ;
;---------------------------------------------------------------------------*/
asm void MovAvgInit(unsigned int length, MovAvgparams *params, Frac16 *data){
MOVE.W #0001,X:(R2+1) //Initialize decimation divider
CLR.W X:(R2+2) //Initialize buffer head pointer
MOVE.W Y0,X:(R2+3) //Initialize buffer length
REP Y0 //Initialize past data to zero
CLR.W X:(R3)+
RTS //Return from subroutine
}
/*---------------------------------------------------------------------------;
; This function implements a moving average Nth order FIR filter. ;
; The result yk is computed as the sum of the current and the past N input ;
; samples multiplied by the multiplier value. Canonical moving average ;
; filtering is obtained if multiplier = 1/(N+1). ;
; For optimal results it is advised to call this function with saturation ;
; disabled. ;
; ;
; Input: Y0 = xk (input sample) ;
; (R2) = decimation (1 = no decimation, 2 = discard every other ;
; sample, and so on) ;
; (R2+1) = decimation counter (used internally for data decimation) ;
; (R2+2) = index of buffer head ;
; (R2+3) = N (length of buffer and filter order) ;
; (R2+4) = fractional multiplier ;
; (R2+5) = yk-1 (past output) ;
; R3 = Pointer to storage buffer for the N past input samples ;
; ;
; Output: Y0 = yk ;
; ;
; Registers modified: A, X0, Y, R2, R3 ;
;---------------------------------------------------------------------------*/
asm Frac16 MovAvg(Frac16 xk, MovAvgparams *params, Frac16 *data){
DEC.W X:(R2+1) //Decrement decimation counter
BEQ DoMovAvg //Decremented to zero?
MOVE.W X:(R2+5),Y0 //No, output old value and exit
BRA MovAvgDone
DoMovAvg: MOVE.W X:(R2)+,X0 //Yes, reload decimation counter
MOVE.W X0,X:(R2)+
ADDA #1,SP //Preserve N (needed if called
MOVE.W N,X:(SP) //from an ISR)
MOVE.W X:(R2)+,A //Load index of buffer head into
MOVE.W A1,N //address modifier register
INC.W A X:(R2)+,Y1 //Point to next buffer location
//and load buffer length
CMP.W Y1,A //If buffer head index = buffer
BNE MAIndexOK //length, then reset buffer head
CLR.W A1 //index
MAIndexOK: MOVE.W A1,X:(R2-2) //Save buffer head index
MOVE.W X:(R3+N),A1 //Load oldest buffer sample
MOVE.W Y0,X:(R3+N) //Save new input sample
MOVE.W X:(R2)+,Y0 //Load multiplier
MPY A1,Y0,A X:(R3)+,X0 //Accumulate oldest sample
REP Y1 //Accumulate the latest N samples
MAC X0,Y0,A X:(R3)+,X0
MOVE.W A,Y0 //Return accumulation result
MOVE.W Y0,X:(R2) //Save new output sample
MOVE.W X:(SP)-,X0 //Restore N
MOVE.W X0,N
MovAvgDone: RTS //Return from subroutine
}
/* -------------- MovAvg.c ends -------------- */
/* -------------- Usage example begins ------------- */
/* application specific constants */
#define MovAvgOrder 40 //Order of moving average filter
#define MovAvgDecimation 100 //Decimation factor for m.a.
/* application specific includes */
#include "MovAvg.h"
/* global variables */
MovAvgparams MAparams;
Frac16 MAdata[MovAvgOrder];
int FiltIn,MAOut;
/* initializations */
MAparams.decimation=MovAvgDecimation;
MAparams.multiplier=FRAC16((float)1/(MovAvgOrder+1));
MovAvgInit(MovAvgOrder,&MAparams,(Frac16*)&MAdata); //Initialize m.a. filter
/* Moving average filter function call */
MAOut=MovAvg(FiltIn,&MAparams,(Frac16*)&MAdata); //Call moving av. filter
/* -------------- Usage example ends ------------- */
/* -------------- PI32.h begins -------------- */
#ifndef __PI32_H__
#define __PI32_H__
#include "types.h"
typedef struct
{
Frac16 c0;
Frac16 c1;
Frac16 IntSatH;
Frac16 IntSatL;
Frac16 kp;
UInt16 nkp;
Frac16 OutSatH;
Frac16 OutSatL;
}PI32params;
typedef struct
{
Frac32 Skm1;
Frac16 xkm1;
}PI32data;
/*---------------------------------------------------------------------------;
; Initialize integral memory and past input of PI regulator. ;
; ;
; Input: A10 = Integral memory initialization value ;
; Y0 = Past input initialization value ;
; (R2) = Integral memory storage destination ;
; (R2+2) = Past input storage destination ;
; ;
; Output: None ;
; ;
; Registers modified: R2 ;
;---------------------------------------------------------------------------*/
asm void PI32Init(Frac32 S, Frac16 xp, PI32data *data);
/*---------------------------------------------------------------------------;
; This function implements a 32 bit precision PI regulator with separate ;
; saturations on the integral portion and on the whole output. ;
; The routine first computes the integral portion at current sampling ;
; instant k (Sk) as Sk = Sk-1 + c0*xk + c1*xk-1. Sk is subsequently ;
; saturated between IntSatH and IntSatL and saved for later use at the next ;
; sampling instant. ;
; Next, proportional portion Pk is computed as kp*2^nkp*xk and saturated ;
; between -1 and +1. The two portions are added together and the result ;
; yk=Sk+Pk is saturated between OutSatH and OutSatL. ;
; Coefficients c0 and c1 are equal and amount to ki*Ts/2, where Ts is the ;
; sampling period and ki is the integral multiplier. The proportional ;
; multiplier kp*2^nkp is split into a fractional part and an exponent in ;
; order to allow values greater than unity. ;
; For optimal results it is advised to call this function with saturation ;
; disabled. ;
; ;
; Input: Y0 = xk (input sample of error signal) ;
; (R2) = c0 ;
; (R2+1) = c1 ;
; (R2+2) = IntSatH ;
; (R2+3) = IntSatL ;
; (R2+4) = kp ;
; (R2+5) = nkp ;
; (R2+6) = OutSatH ;
; (R2+7) = OutSatL ;
; (R3) = Sk-1 (32 bits) ;
; (R3+2) = xk-1 ;
; ;
; Output: A = yk (32 significant bits) ;
; ;
; Registers modified: A, B, X0, Y, R0 ;
;---------------------------------------------------------------------------*/
asm Frac32 PI32(Frac16 xk, PI32params *params, PI32data *data);
#endif //ifndef __PI32_H__
/* -------------- PI32.h ends -------------- */
/* -------------- PI32.c begins -------------- */
/*---------------------------------------------------------------------------;
; Initialize integral memory and past input of PI regulator. ;
; ;
; Input: A10 = Integral memory initialization value ;
; Y0 = Past input initialization value ;
; (R2) = Integral memory storage destination ;
; (R2+2) = Past input storage destination ;
; ;
; Output: None ;
; ;
; Registers modified: R2 ;
;---------------------------------------------------------------------------*/
asm void PI32Init(Frac32 S, Frac16 xp, PI32data *data){
MOVE.L A10,X:(R2)+ //Initialize integral memory
MOVE.W Y0,X:(R2) //Initialize past input
RTS //Return from subroutine
}
/*---------------------------------------------------------------------------;
; This function implements a 32 bit precision PI regulator with separate ;
; saturations on the integral portion and on the whole output. ;
; The routine first computes the integral portion at current sampling ;
; instant k (Sk) as Sk = Sk-1 + c0*xk + c1*xk-1. Sk is subsequently ;
; saturated between IntSatH and IntSatL and saved for later use at the next ;
; sampling instant. ;
; Next, proportional portion Pk is computed as kp*2^nkp*xk and saturated ;
; between -1 and +1. The two portions are added together and the result ;
; yk=Sk+Pk is saturated between OutSatH and OutSatL. ;
; Coefficients c0 and c1 are equal and amount to ki*Ts/2, where Ts is the ;
; sampling period and ki is the integral multiplier. The proportional ;
; multiplier kp*2^nkp is split into a fractional part and an exponent in ;
; order to allow values greater than unity. ;
; For optimal results it is advised to call this function with saturation ;
; disabled. ;
; ;
; Input: Y0 = xk (input sample of error signal) ;
; (R2) = c0 ;
; (R2+1) = c1 ;
; (R2+2) = IntSatH ;
; (R2+3) = IntSatL ;
; (R2+4) = kp ;
; (R2+5) = nkp ;
; (R2+6) = OutSatH ;
; (R2+7) = OutSatL ;
; (R3) = Sk-1 (32 bits) ;
; (R3+2) = xk-1 ;
; ;
; Output: A = yk (32 significant bits) ;
; ;
; Registers modified: A, B, X0, Y, R0 ;
;---------------------------------------------------------------------------*/
asm Frac32 PI32(Frac16 xk, PI32params *params, PI32data *data){
MOVE.W R2,X0
MOVE.W X0,R0
MOVE.L X:(R3)+,A //Load Sk-1
MOVE.W X:(R0)+,X0 //Load c0
MOVE.W Y0,Y1 //Save xk
MAC Y0,X0,A X:(R0)+,Y0 X:(R3)+,X0 //c0*xk, c1, xk-1
MAC Y0,X0,A X:(R0)+,Y0 X:(R3)-,X0 //c1*xk-1, IntSatH, decr. R3
MOVE.W Y0,B //Load IntSatH into B
CMP A,B Y1,X:(R3)+ //Check upper satur. and save xk
BGT CheckSatLo //If Sk<IntSatH no need for sat.
MOVE.W Y0,A //else saturate and increment R0
ADDA #1,R0 //to skip IntSatL in the
BRA IntSatOK //parameter structure
CheckSatLo: MOVE.W X:(R0)+,B //Load IntSatL
CMP A,B //Check lower saturation
BLE IntSatOK //If Sk>IntSatL no need for sat.
MOVE.W B1,A //else saturate
IntSatOK: ADDA #-3,R3,R3 //Point to Sk storage location
MOVE.L A10,X:(R3) //Save Sk
MOVE.W X:(R0)+,Y0 //Load kp
MPY Y0,Y1,B X:(R0)+,Y0 //kp*xk, nkp
CLB B,X0 //Number of leading bits of
CMP Y0,X0 //result greater or equal than
NOP //desired shift count?
BLT PropSat //No, saturate
ASLL.L Y0,B //Yes, shift
BRA PropSatOK
PropSat: TST B //Saturate proportional portion
BLT PropSatNeg //according to sign
MOVE.W #$7FFF,B
BRA PropSatOK
PropSatNeg: MOVE.W #$8000,B
PropSatOK: ADD B,A X:(R0)+,B //Compute yk, load OutSatH
CMP A,B X:(R0)+,X0 //Check upper sat., load OutSatL
BGT CheckSatOutLo //If yk<OutSatH no need for sat.
MOVE.W B1,A //else saturate
BRA OutSatOK
CheckSatOutLo: CMP X0,A //Check lower saturation
BGT OutSatOK //If yk>OutSatL no need for sat.
MOVE.W X0,A //else saturate
OutSatOK: RTS //Return from subroutine
}
/* -------------- PI32.c ends -------------- */
/* -------------- Usage example begins ------------- */
/* application specific constants */
#define PIc0 100
#define PIc1 100
#define PIIntSatH 15000
#define PIIntSatL -15000
#define PIkp 0x4000
#define PInkp 1
#define PIOutSatH 30000
#define PIOutSatL -30000
/* application specific includes */
#include "PI32.h"
/* global variables */
PI32params PIparams;
PI32data PIdata;
int PIIn,PIOut;
/* initializations */
PIparams.c0=PIc0; //Initialize PI regulator
PIparams.c1=PIc1; //parameters and past data
PIparams.IntSatH=PIIntSatH;
PIparams.IntSatL=PIIntSatL;
PIparams.kp=PIkp;
PIparams.nkp=PInkp;
PIparams.OutSatH=PIOutSatH;
PIparams.OutSatL=PIOutSatL;
PI32Init(0,0, &PIdata);
/* PI32 regulator function call */
PIOut=(int)(PI32(PIIn,&PIparams,&PIdata)>>16); //Call PI regulator
/* -------------- Usage example ends ------------- */
