Dear Paul,
Even I bypass all the algorithm and let the function return its input value, it still doesn't come out any sound. Whereas it works well when I remove the function and directly write the data from record queue to the play queue. The code which keep the function is shown below. Could you please tell me what 's the problem? Many thanks!
Even I bypass all the algorithm and let the function return its input value, it still doesn't come out any sound. Whereas it works well when I remove the function and directly write the data from record queue to the play queue. The code which keep the function is shown below. Could you please tell me what 's the problem? Many thanks!
Code:
#include <Arduino.h>
#include <arm_math.h>
#include <arm_const_structs.h>
#include <Audio.h>
#include <Wire.h>
#include <SPI.h>
#include <SD.h>
#include <SerialFlash.h>
// GUItool: begin automatically generated code
AudioInputI2S2 i2s2_1; //xy=119.99999618530273,390.0000228881836
AudioInputUSB usb1; //xy=120.90909090909089,217.27272727272725
AudioMixer4 mixer3; //xy=278.8888969421387,403.3333740234375
AudioEffectFreeverb freeverb1; //xy=442.2222213745117,403.33334732055664
AudioMixer4 mixer1; //xy=669.9999313354492,233.3333396911621
AudioMixer4 mixer2; //xy=672.2222023010254,409.9999809265137
AudioRecordQueue queue1; //xy=831.1110763549805,233.33334159851074
AudioRecordQueue queue2; //xy=832.2222061157227,410.00000190734863
AudioPlayQueue queue3; //xy=1007.7778244018555,265.5555648803711
AudioPlayQueue queue4; //xy=1008.8888549804688,378.8889217376709
AudioOutputUSB usb2; //xy=1257.6189994812012,262.8571529388428
AudioOutputI2S i2s1; //xy=1258.0951690673828,378.8888645172119
AudioConnection patchCord1(i2s2_1, 0, mixer3, 0);
AudioConnection patchCord2(i2s2_1, 1, mixer3, 1);
AudioConnection patchCord3(usb1, 0, mixer1, 0);
AudioConnection patchCord4(usb1, 1, mixer2, 0);
AudioConnection patchCord5(mixer3, freeverb1);
AudioConnection patchCord6(freeverb1, 0, mixer1, 1);
AudioConnection patchCord7(freeverb1, 0, mixer2, 1);
AudioConnection patchCord8(mixer1, queue1);
AudioConnection patchCord9(mixer2, queue2);
AudioConnection patchCord10(queue3, 0, usb2, 0);
AudioConnection patchCord11(queue3, 0, i2s1, 0);
AudioConnection patchCord12(queue4, 0, usb2, 1);
AudioConnection patchCord13(queue4, 0, i2s1, 1);
// GUItool: end automatically generated code
int fftsize = 512;
int numofnotch = 10;
float32_t r = 0.85;
void setup() {
// put your setup code here, to run once:
AudioMemory(120);
queue3.setBehaviour(AudioPlayQueue::ORIGINAL);
queue4.setBehaviour(AudioPlayQueue::ORIGINAL);
mixer3.gain(0, 1.0);
mixer3.gain(1, -1.0);
mixer1.gain(0, 1.0);
mixer1.gain(1, 1.0);
mixer2.gain(0, 1.0);
mixer2.gain(1, 1.0);
queue1.begin();
queue2.begin();
}
void loop() {
// put your main code here, to run repeatedly:
if (queue1.available() == fftsize/128 && queue2.available() == fftsize/128){
arm_rfft_fast_instance_f32 fftL_handler;
arm_rfft_fast_instance_f32 fftR_handler;
arm_rfft_fast_init_f32(&fftL_handler, fftsize);
arm_rfft_fast_init_f32(&fftR_handler, fftsize);
float32_t fftL_in_buf[fftsize];
float32_t fftL_out_buf[fftsize];
float32_t fftR_in_buf[fftsize];
float32_t fftR_out_buf[fftsize];
float32_t freqpowL[fftsize/2];
float32_t freqpowR[fftsize/2];
int numofpeaksL = 0;
int peaksindexL[fftsize/4];
int numofpeaksR = 0;
int peaksindexR[fftsize/4];
float32_t avgpowL;
float32_t avgpowR;
int numofhowlingL = 0;
int numofhowlingR = 0;
static int PAPRthreshold = 6;
for (int j = 0; j < fftsize/128; j = j+1){
short * qpL = queue1.readBuffer();
short * qpR = queue2.readBuffer();
for (int i = 128*j; i < 128*(j+1); i = i+1){
fftL_in_buf[i] = (float32_t)(qpL[i-128*j]) * (1.0f / 32768.0f);
fftL_in_buf[i] *= 0.5 * (1 - arm_cos_f32(2*PI*(float32_t)(i/fftsize)));
fftR_in_buf[i] = (float32_t)(qpR[i-128*j]) * (1.0f / 32768.0f);
fftR_in_buf[i] *= 0.5 * (1 - arm_cos_f32(2*PI*(float32_t)(i/fftsize)));
}
queue1.freeBuffer();
queue2.freeBuffer();
}
// arm_rfft_fast_f32(&fftL_handler, fftL_in_buf, fftL_out_buf, 0);
// arm_rfft_fast_f32(&fftR_handler, fftR_in_buf, fftR_out_buf, 0);
//
// arm_cmplx_mag_squared_f32(fftL_out_buf, freqpowL, fftsize/2);
// arm_cmplx_mag_squared_f32(fftR_out_buf, freqpowR, fftsize/2);
//
// for (int i = 0; i < fftsize/2; i = i+1){
// if (i == 0){
// if (freqpowL[i+1]-freqpowL[i] < 0){
// numofpeaksL += 1;
// peaksindexL[numofpeaksL] = i;
// }
//
// if (freqpowR[i+1]-freqpowR[i] < 0){
// numofpeaksR += 1;
// peaksindexR[numofpeaksR] = i;
// }
// }
// else if (i == fftsize/2 - 1){
// if (freqpowL[i]-freqpowL[i-1] > 0){
// numofpeaksL += 1;
// peaksindexL[numofpeaksL] = i;
// }
//
// if (freqpowR[i]-freqpowR[i-1] > 0){
// numofpeaksR += 1;
// peaksindexR[numofpeaksR] = i;
// }
// }
// else {
// if ((freqpowL[i]-freqpowL[i-1])*(freqpowL[i+1]-freqpowL[i]) < 0){
// numofpeaksL += 1;
// peaksindexL[numofpeaksL] = i;
// }
//
// if ((freqpowR[i]-freqpowR[i-1])*(freqpowR[i+1]-freqpowR[i]) < 0){
// numofpeaksR += 1;
// peaksindexR[numofpeaksR] = i;
// }
// }
// }
//
// int howlingindexL[numofpeaksL];
// int howlingindexR[numofpeaksR];
//
// arm_mean_f32(freqpowL, fftsize/2, &avgpowL);
// arm_mean_f32(freqpowR, fftsize/2, &avgpowR);
//
// for (int i = 0; i < numofpeaksL; i = i+1){
// if (10*log10(freqpowL[peaksindexL[i]]/avgpowL) > PAPRthreshold){
// numofhowlingL += 1;
// howlingindexL[numofhowlingL] = peaksindexL[i];
// }
// }
//
// for (int i = 0; i < numofpeaksR; i = i+1){
// if (10*log10(freqpowR[peaksindexR[i]]/avgpowR) > PAPRthreshold){
// numofhowlingR += 1;
// howlingindexR[numofhowlingR] = peaksindexR[i];
// }
// }
//
// while ((numofhowlingL > numofnotch)||(numofhowlingR > numofnotch)){
// numofhowlingL = 0;
// numofhowlingR = 0;
// PAPRthreshold += 1;
// for (int i = 0; i < numofpeaksL; i = i+1){
// if (10*log10(freqpowL[peaksindexL[i]]/avgpowL) > PAPRthreshold){
// numofhowlingL += 1;
// howlingindexL[numofhowlingL] = peaksindexL[i];
// }
// }
//
// for (int i = 0; i < numofpeaksR; i = i+1){
// if (10*log10(freqpowR[peaksindexR[i]]/avgpowR) > PAPRthreshold){
// numofhowlingR += 1;
// howlingindexR[numofhowlingR] = peaksindexR[i];
// }
// }
// }
int16_t outsamplesL[fftsize];
int16_t outsamplesR[fftsize];
// float32_t filterL_in_buf[fftsize];
// memcpy(filterL_in_buf, fftL_in_buf, fftsize * sizeof(float32_t));
// float32_t filterR_in_buf[fftsize];
// memcpy(filterR_in_buf, fftR_in_buf, fftsize * sizeof(float32_t));
// for (int i = 0; i < numofnotch; i = i+1){
// float32_t filterL_out_buf[fftsize];
// float32_t filterR_out_buf[fftsize];
float32_t NotchCoeffsL[5];
// if (i <= numofhowlingL){
// NotchCoeffsL[0] = 1.0f;
// NotchCoeffsL[1] = -2.0f * arm_cos_f32(PI*howlingindexL[i]/(fftsize/2));
// NotchCoeffsL[2] = 1.0f;
// NotchCoeffsL[3] = -2.0f * r * arm_cos_f32(PI*howlingindexL[i]/(fftsize/2));
// NotchCoeffsL[4] = r * r;
// }
// else {
NotchCoeffsL[0] = 1.0f;
NotchCoeffsL[1] = 0.0f;
NotchCoeffsL[2] = 0.0f;
NotchCoeffsL[3] = 0.0f;
NotchCoeffsL[4] = 0.0f;
// }
float32_t NotchCoeffsR[5];
// if (i <= numofhowlingR){
// NotchCoeffsR[0] = 1.0f;
// NotchCoeffsR[1] = -2.0f * arm_cos_f32(PI*howlingindexR[i]/(fftsize/2));
// NotchCoeffsR[2] = 1.0f;
// NotchCoeffsR[3] = -2.0f * r * arm_cos_f32(PI*howlingindexR[i]/(fftsize/2));
// NotchCoeffsR[4] = r * r;
// }
// else {
NotchCoeffsR[0] = 1.0f;
NotchCoeffsR[1] = 0.0f;
NotchCoeffsR[2] = 0.0f;
NotchCoeffsR[3] = 0.0f;
NotchCoeffsR[4] = 0.0f;
// }
for (int j = 0; j < fftsize; j = j+1){
outsamplesL[j] = (int16_t)(BiquadNotch(fftL_in_buf[j], NotchCoeffsL, 0, 1) * 32768.0f);
outsamplesR[j] = (int16_t)(BiquadNotch(fftR_in_buf[j], NotchCoeffsR, 1, 1) * 32768.0f);
}
// }
queue3.play(outsamplesL, fftsize);
queue4.play(outsamplesR, fftsize);
}
}
float32_t BiquadNotch (float32_t insample, float32_t *NotchCoeffs, int channel, int filterindex)
{
// static float32_t sZ1L[11];
// static float32_t sZ2L[11];
// static float32_t sZ1R[11];
// static float32_t sZ2R[11];
// float32_t outsample;
// if (channel == 0){
// outsample = NotchCoeffs[0] * insample + NotchCoeffs[1] * sZ1L[filterindex] + NotchCoeffs[2] * sZ2L[filterindex] - NotchCoeffs[3] * sZ1L[filterindex + 1] - NotchCoeffs[4] * sZ2L[filterindex + 1];
// sZ2L[filterindex] = sZ1L[filterindex];
// sZ2L[filterindex + 1] = sZ1L[filterindex + 1];
// sZ1L[filterindex] = insample;
// sZ1L[filterindex + 1] = outsample;
// }
// else if (channel == 1){
// outsample = NotchCoeffs[0] * insample + NotchCoeffs[1] * sZ1R[filterindex] + NotchCoeffs[2] * sZ2R[filterindex] - NotchCoeffs[3] * sZ1R[filterindex + 1] - NotchCoeffs[4] * sZ2R[filterindex + 1];
// sZ2R[filterindex] = sZ1R[filterindex];
// sZ2R[filterindex + 1] = sZ1R[filterindex + 1];
// sZ1R[filterindex] = insample;
// sZ1R[filterindex + 1] = outsample;
// }
// outsample = insample;
return insample;
}