Hello all,
I am a student helping a professor port some code from some dsp labs from a TI TMDSDSK6713 board onto the Teensy 4.0 and audio shield.
The labs include a moving average and IIR filter implementation. So far I've used https://forum.pjrc.com/index.php?threads/59171/ as a helpful reference in order to understand how I can access the data streams directly to implement the filters using the Aduio library. The only change that I really made was to extend the number of data blocks that are stored in a data buffer and made it circular to simplify the dsp algorithms.
I've gotten the moving average working correctly, so I'm fairly certain that the memory accessing of the circular data buffers are correct, but the IIR implementation is giving me trouble and i'm not quite sure what is breaking. The IIR filter is implemented in the Direct Form II method where the input is multiplied by the a coefficients to create an intermediate function that is then multiplied by the b coefficients to form the output. What I can see from the serial buffer is that after runs through the calculation of the first 20 or so values on start up and the values seem to blow up to a huge number and overflow. The filter itself was designed using MATLAB so it is stable. Also, I'm initializing the arrays of the input buffers to 0 so these initial values should be 0.
At this point i'm not sure if it's a problem with my code or is a Teensy specific problem so i'd appreciate the help!
Here is the code for the IIR Filter:
I am a student helping a professor port some code from some dsp labs from a TI TMDSDSK6713 board onto the Teensy 4.0 and audio shield.
The labs include a moving average and IIR filter implementation. So far I've used https://forum.pjrc.com/index.php?threads/59171/ as a helpful reference in order to understand how I can access the data streams directly to implement the filters using the Aduio library. The only change that I really made was to extend the number of data blocks that are stored in a data buffer and made it circular to simplify the dsp algorithms.
I've gotten the moving average working correctly, so I'm fairly certain that the memory accessing of the circular data buffers are correct, but the IIR implementation is giving me trouble and i'm not quite sure what is breaking. The IIR filter is implemented in the Direct Form II method where the input is multiplied by the a coefficients to create an intermediate function that is then multiplied by the b coefficients to form the output. What I can see from the serial buffer is that after runs through the calculation of the first 20 or so values on start up and the values seem to blow up to a huge number and overflow. The filter itself was designed using MATLAB so it is stable. Also, I'm initializing the arrays of the input buffers to 0 so these initial values should be 0.
At this point i'm not sure if it's a problem with my code or is a Teensy specific problem so i'd appreciate the help!
Here is the code for the IIR Filter:
Code:
#include <Audio.h>
#include <stdio.h>
#define AUDIO_MASK 0x0000FFFF
#define CIRCULAR_BUFFER_LENGTH AUDIO_BLOCK_SAMPLES*4
#define CIRCULAR_BUFFER_MASK (CIRCULAR_BUFFER_LENGTH - 1)
#define MAX_SAMPLE_BLOCKS (CIRCULAR_BUFFER_LENGTH / AUDIO_BLOCK_SAMPLES)-1
//Audio library macro, AUDIO_BLOCK_SAMPLES = 128
#define IIR_ORDER 10
#define VOLUME_AMPLIFY 1
AudioInputI2S i2s_in;
AudioOutputI2S i2s_out;
AudioRecordQueue Q_in_L;
AudioRecordQueue Q_in_R;
AudioPlayQueue Q_out_L;
AudioPlayQueue Q_out_R;
AudioConnection patchCord1(i2s_in, 0, Q_in_L, 0);
AudioConnection patchCord2(i2s_in, 1, Q_in_R, 0);
AudioConnection patchCord3(Q_out_L, 0, i2s_out, 0);
AudioConnection patchCord4(Q_out_R, 0, i2s_out, 1);
AudioControlSGTL5000 sgtl5000;
const int myInput = AUDIO_INPUT_LINEIN;
double buf_L[CIRCULAR_BUFFER_LENGTH] = {0.0};
double buf_R[CIRCULAR_BUFFER_LENGTH] = {0.0};
int curSample = 0;
int curBlock = 0;
double a[] = {1.0, // a0
-9.7723, // a1
42.9767, // a2
-112.0082, // a3
191.5851, // a4
-224.7201, // a5
183.0563, // a6
-102.2575, // a7
37.4887, // a8
-8.1449, // a9
0.7964 // a10
};
double b[] = {0.0029E-015, // b0
0.0287E-015, // b1
0.1290E-015, // b2
0.344E-015, // b3
0.6022E-015, // b4
0.7226E-015, // b5
0.6022E-015, // b6
0.3441E-015, // b7
0.1290E-015, // b8
0.0287E-015, // b9
0.0029E-015 // b10
};
//500hz low pass
void setup(void)
{
// Audio connections require memory. and the record queue
// uses this memory to buffer incoming audio.
AudioMemory(10);
// Enable the audio shield. select input. and enable output
sgtl5000.enable();
sgtl5000.inputSelect(myInput);
sgtl5000.volume(0.5);
// Start the record queues
Q_in_L.begin();
Q_in_R.begin();
Serial.begin(9600);
}
void loop(void)
{
short *bp_L, *bp_R;
// Wait for left and right input channels to have content
while (!Q_in_L.available() && !Q_in_R.available());
bp_L = Q_in_L.readBuffer();
bp_R = Q_in_R.readBuffer();
int startIndex = curBlock * AUDIO_BLOCK_SAMPLES;
for (int i = 0; i < AUDIO_BLOCK_SAMPLES; i++) {
curSample = startIndex + i;
buf_L[curSample] = (double)bp_L[i];
buf_R[curSample] = (double)bp_R[i];
}
Q_in_L.freeBuffer();
Q_in_R.freeBuffer();
// Get pointers to "empty" output buffers
bp_L = Q_out_L.getBuffer();
bp_R = Q_out_R.getBuffer();
//Temperary arrays to hold new values of current sample block
double leftOut[AUDIO_BLOCK_SAMPLES] = {0.0};
double rightOut[AUDIO_BLOCK_SAMPLES] = {0.0};
// Operate on each value in current block that has been recieved
for (int i = 0; i < AUDIO_BLOCK_SAMPLES; i++) {
curSample = startIndex + i;
/* Start user code */
//Type II direct implementation of IIR filter
// w[curSample] = w[curSample] - a1*w1 - a2*w2 - ... - a[j]*w[n]
for (int j = 1; j <= IIR_ORDER; j++)
{
buf_L[curSample] -= a[j] * buf_L[(curSample - j) & CIRCULAR_BUFFER_MASK];
buf_R[curSample] -= a[j] * buf_R[(curSample - j) & CIRCULAR_BUFFER_MASK];
}
Serial.print(curSample);
Serial.print(": ");
Serial.print(buf_L[curSample],20);
Serial.print("\n");
leftOut[i] = 0;
rightOut[i] = 0;
// y = b0*w0 + b1*w1 + ... +b[n]*w[j]
for(int j =0; j <= IIR_ORDER; j++)
{
leftOut[i] += b[j] * buf_L[(curSample - j) & CIRCULAR_BUFFER_MASK];
rightOut[i] += b[j] * buf_R[(curSample - j) & CIRCULAR_BUFFER_MASK];
}
leftOut[i] *= VOLUME_AMPLIFY;
rightOut[i]*= VOLUME_AMPLIFY;
/* End user code */
}
// copy the processed data block back to the output
for (int i = 0; i < AUDIO_BLOCK_SAMPLES; i++) {
bp_L[i] = (short)leftOut[i];
bp_R[i] = (short)rightOut[i];
}
curBlock++;
curBlock &= MAX_SAMPLE_BLOCKS;
// and play them back into the audio queues
Q_out_L.playBuffer();
Q_out_R.playBuffer();
}