Get the audio data samples in floating point numbers

Shingo

Member
Hi Paul,

I'm developing a howling suppression mixer using Teensy 4.1. I need to get the data samples to perform my howling detection and suppression algorithm. My system model is shown in the picture attached. However, the data samples provided by the 'queue' block is 16bit int (fixed point?). I don't know how to process calculation in fixed point format. Is it same as that in floating point? Is there any method to convert the fixed point data samples to floating point? Besides, after my algorithm, I need to convert the floating point back to fixed point. 微信截图_20220407024915.png
 
Just read each int16_t from the array, type cast to float, and multiply by a scaling factor of (1.0f / 32768.0f) to convert to -1.0 to +1.0 numerical range.

To store the numbers, just do those steps in reverse. Multiply by a scaling factor, typecast to int16_t and write to the array.

For an example, look at filter_ladder.cpp. It processing everything as floats. Look for the places with 32768.0f scaling factors.
 
Dear Paul,

It has an error shown in the attached picture. Could you please tell me what's the problem? Many thanks!微信截图_20220407024915.png
 

Attachments

  • 微信截图_20220407024915.png
    微信截图_20220407024915.png
    21.6 KB · Views: 56
You need to convert every sample in the array, one by one, that's how C/C++ works. In your first attempt you tried to multiply a scalar by an array, which gives a type error.

Second attempt you are casting the pointer, which does nothing to convert the values in the array.

You need to convert each element in the array - perhaps into a different destination array?

BTW please do not post images, use copy/paste and code tags to quote the text in a form that's usable. The Forum rule is sound advice, which you've not followed, otherwise I might have simply been able to suggest an actual fix.
 
Hi MarkT,

Here is the error message: Arduino: 1.8.16 (Windows 10), TD: 1.55, Board: "Teensy 4.1, Audio, 600 MHz, Faster, US English"
C:\Users\wuxue\AppData\Local\Temp\arduino_build_554703\sketch\NHS.ino.cpp.o: In function `loop':

D:\NHS/NHS.ino:227: undefined reference to `AudioPlayQueue::play(short const*, unsigned long)'

D:\NHS/NHS.ino:228: undefined reference to `AudioPlayQueue::play(short const*, unsigned long)'

collect2.exe: error: ld returned 1 exit status

Multiple libraries were found for "SD.h"

Used: D:\Arduino\hardware\teensy\avr\libraries\SD

Not used: D:\Arduino\libraries\SD

Error compiling for board Teensy 4.1.



This report would have more information with
"Show verbose output during compilation"
option enabled in File -> Preferences.

Here are my codes. Could you please tell me what's the problem? Many thanks!

#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
AudioInputUSB usb1; //xy=266.5,251
AudioInputI2S i2s1; //xy=268.25,391.75
AudioMixer4 mixer3; //xy=489,404
AudioEffectFreeverb freeverb1; //xy=713.25,403.75
AudioMixer4 mixer1; //xy=994.611083984375,263.0793762207031
AudioMixer4 mixer2; //xy=996.0753173828125,410.98419189453125
AudioRecordQueue queue1; //xy=1253.416748046875,264.2777862548828
AudioRecordQueue queue2; //xy=1254.8333740234375,410.8611145019531
AudioPlayQueue queue3; //xy=1438,267
AudioPlayQueue queue4; //xy=1439,411
AudioOutputUSB usb2; //xy=1664.4444160461426,273.33335399627686
AudioOutputI2S i2s2; //xy=1665.3333282470703,404.33333015441895
AudioConnection patchCord1(usb1, 0, mixer1, 0);
AudioConnection patchCord2(usb1, 1, mixer2, 0);
AudioConnection patchCord3(i2s1, 0, mixer3, 0);
AudioConnection patchCord4(i2s1, 1, mixer3, 1);
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, i2s2, 0);
AudioConnection patchCord11(queue3, 0, usb2, 0);
AudioConnection patchCord12(queue4, 0, i2s2, 1);
AudioConnection patchCord13(queue4, 0, usb2, 1);
// GUItool: end automatically generated code

float32_t sZ1L[11] = {0};
float32_t sZ2L[11] = {0};
float32_t sZ1R[11] = {0};
float32_t sZ2R[11] = {0};

void setup() {
// put your setup code here, to run once:
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() == 4 && queue2.available() == 4)
{
int fftsize = 512;
int numofnotch = 10;
float32_t r = 0.85;
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 tmp_find_candidatesL[fftsize/4];
// float32_t tmp_find_candidatesR[fftsize/4];
// int numofcandidates = 6;
// float32_t candidatespowL[numofcandidates];
// uint32_t candindexL[numofcandidates];
// float32_t candidatespowR[numofcandidates];
// uint32_t candindexR[numofcandidates];
float32_t avgpowL;
float32_t avgpowR;
int PAPRthreshold = 6;
int numofhowlingL = 0;
int numofhowlingR = 0;

for (int j = 0; j < fftsize/128; j = j+1){
for (int i = 128*j; i < 128*(j+1); i = i+1){
fftL_in_buf = (float)(queue1.readBuffer() * (1.0f / 32768.0f));
fftL_in_buf *= 0.5*(1-arm_cos_f32(2*PI*i/fftsize));
fftR_in_buf = (float)(queue2.readBuffer() * (1.0f / 32768.0f));
fftR_in_buf *= 0.5*(1-arm_cos_f32(2*PI*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 < 0){
numofpeaksL += 1;
peaksindexL[numofpeaksL] = i;
}

if (freqpowR[i+1]-freqpowR < 0){
numofpeaksR += 1;
peaksindexR[numofpeaksR] = i;
}
}
else if (i == fftsize/2 - 1){
if (freqpowL-freqpowL[i-1] > 0){
numofpeaksL += 1;
peaksindexL[numofpeaksL] = i;
}

if (freqpowR-freqpowR[i-1] > 0){
numofpeaksR += 1;
peaksindexR[numofpeaksR] = i;
}
}
else {
if((freqpowL-freqpowL[i-1])*(freqpowL[i+1]-freqpowL) < 0){
numofpeaksL += 1;
peaksindexL[numofpeaksL] = i;
}

if((freqpowR-freqpowR[i-1])*(freqpowR[i+1]-freqpowR) < 0){
numofpeaksR += 1;
peaksindexR[numofpeaksR] = i;
}
}
}

// for (int i = 0; i < numofpeaksL; i = i+1){
// tmp_find_candidatesL = freqpowL[peaksindexL];
// }
//
// for (int i = 0; i < numofpeaksR; i = i+1){
// tmp_find_candidatesR = freqpowR[peaksindexR];
// }
//
// for (int i = 0; i < numofcandidates; i = i+1){
// arm_max_f32(tmp_find_candidatesL, fftsize/2, &candidatespowL, &candindexL);
// arm_max_f32(tmp_find_candidatesR, fftsize/2, &candidatespowR, &candindexR);
// tmp_find_candidatesL[candindexL] = 0;
// tmp_find_candidatesR[candindexR] = 0;
// }

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]/avgpowL) > PAPRthreshold){
numofhowlingL += 1;
howlingindexL[numofhowlingL] = peaksindexL;
}
}

for (int i = 0; i < numofpeaksR; i = i+1){
if (10*log10(freqpowR[peaksindexR]/avgpowR) > PAPRthreshold){
numofhowlingR += 1;
howlingindexR[numofhowlingR] = peaksindexR;
}
}

int16_t outsamplesL[fftsize];
int16_t outsamplesR[fftsize];

for (int i = 0; i < numofnotch; i = i+1){
float32_t NotchCoeffs[5];
if (i <= numofhowlingL){
NotchCoeffs[0] = 1.0f;
NotchCoeffs[1] = -2.0f * arm_cos_f32(PI*howlingindexL/(fftsize/2));
NotchCoeffs[2] = 1.0f;
NotchCoeffs[3] = -2.0f * r * arm_cos_f32(PI*howlingindexL/(fftsize/2));
NotchCoeffs[4] = r * r;
}
else {
NotchCoeffs[0] = 1.0f;
NotchCoeffs[1] = 0.0f;
NotchCoeffs[2] = 0.0f;
NotchCoeffs[3] = 0.0f;
NotchCoeffs[4] = 0.0f;
}
for (int j = 0; j < fftsize; j = j+1){
outsamplesL[j] = (int16_t)(BiquadNotch(fftL_in_buf[j], NotchCoeffs, sZ1L, sZ2L) * 32768.0f);
}

}

for (int i = 0; i < numofnotch; i = i+1){
float32_t NotchCoeffs[5];
if (i <= numofhowlingR){
NotchCoeffs[0] = 1.0f;
NotchCoeffs[1] = -2.0f * arm_cos_f32(PI*howlingindexR/(fftsize/2));
NotchCoeffs[2] = 1.0f;
NotchCoeffs[3] = -2.0f * r * arm_cos_f32(PI*howlingindexR/(fftsize/2));
NotchCoeffs[4] = r * r;
}
else {
NotchCoeffs[0] = 1.0f;
NotchCoeffs[1] = 0.0f;
NotchCoeffs[2] = 0.0f;
NotchCoeffs[3] = 0.0f;
NotchCoeffs[4] = 0.0f;
}
for (int j = 0; j < fftsize; j = j+1){
outsamplesR[j] = (int16_t)(BiquadNotch(fftR_in_buf[j], NotchCoeffs, sZ1R, sZ2R) * 32768.0f);
}
}

queue3.play(outsamplesL, fftsize);
queue4.play(outsamplesR, fftsize);
}
}

float32_t BiquadNotch (float32_t insample, float32_t* NotchCoeffs, float32_t* sZ1, float32_t* sZ2){
return insample;
}
 
Can you post entire complete verbose errors (in code tags preferrably)?

Unless code is placed in code tags it gets corrupted with emoticons and suchlike, and can't be investigated easily - code tags mean its verbatim.
 
Code:
Arduino: 1.8.16 (Windows 10), TD: 1.55, Board: "Teensy 4.1, Audio, 600 MHz, Faster, US English"





















C:\Users\wuxue\AppData\Local\Temp\arduino_build_674868\sketch\NHS.ino.cpp.o: In function `loop':

D:\NHS/NHS.ino:227: undefined reference to `AudioPlayQueue::play(short const*, unsigned long)'

D:\NHS/NHS.ino:228: undefined reference to `AudioPlayQueue::play(short const*, unsigned long)'

collect2.exe: error: ld returned 1 exit status

Multiple libraries were found for "SD.h"

 Used: D:\Arduino\hardware\teensy\avr\libraries\SD

 Not used: D:\Arduino\libraries\SD

Error compiling for board Teensy 4.1.



This report would have more information with
"Show verbose output during compilation"
option enabled in File -> Preferences.

Code:
#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
AudioInputUSB            usb1;           //xy=266.5,251
AudioInputI2S            i2s1;           //xy=268.25,391.75
AudioMixer4              mixer3;         //xy=489,404
AudioEffectFreeverb      freeverb1;      //xy=713.25,403.75
AudioMixer4              mixer1;         //xy=994.611083984375,263.0793762207031
AudioMixer4              mixer2;         //xy=996.0753173828125,410.98419189453125
AudioRecordQueue         queue1;         //xy=1253.416748046875,264.2777862548828
AudioRecordQueue         queue2;         //xy=1254.8333740234375,410.8611145019531
AudioPlayQueue           queue3;         //xy=1438,267
AudioPlayQueue           queue4;         //xy=1439,411
AudioOutputUSB           usb2;           //xy=1664.4444160461426,273.33335399627686
AudioOutputI2S           i2s2;           //xy=1665.3333282470703,404.33333015441895
AudioConnection          patchCord1(usb1, 0, mixer1, 0);
AudioConnection          patchCord2(usb1, 1, mixer2, 0);
AudioConnection          patchCord3(i2s1, 0, mixer3, 0);
AudioConnection          patchCord4(i2s1, 1, mixer3, 1);
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, i2s2, 0);
AudioConnection          patchCord11(queue3, 0, usb2, 0);
AudioConnection          patchCord12(queue4, 0, i2s2, 1);
AudioConnection          patchCord13(queue4, 0, usb2, 1);
// GUItool: end automatically generated code

float32_t sZ1L[11] = {0};
float32_t sZ2L[11] = {0};
float32_t sZ1R[11] = {0};
float32_t sZ2R[11] = {0};

void setup() {
  // put your setup code here, to run once:
  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() == 4 && queue2.available() == 4)
  {
    int fftsize = 512;
    int numofnotch = 10;
    float32_t r = 0.85;
    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 tmp_find_candidatesL[fftsize/4];
//    float32_t tmp_find_candidatesR[fftsize/4];
//    int numofcandidates = 6;
//    float32_t candidatespowL[numofcandidates];
//    uint32_t candindexL[numofcandidates];
//    float32_t candidatespowR[numofcandidates];
//    uint32_t candindexR[numofcandidates];
    float32_t avgpowL;
    float32_t avgpowR;
    int PAPRthreshold = 6;
    int numofhowlingL = 0;
    int numofhowlingR = 0;

    for (int j = 0; j < fftsize/128; j = j+1){
      for (int i = 128*j; i < 128*(j+1); i = i+1){
        fftL_in_buf[i] = (float)(queue1.readBuffer()[i] * (1.0f / 32768.0f));
        fftL_in_buf[i] *= 0.5*(1-arm_cos_f32(2*PI*i/fftsize));
        fftR_in_buf[i] = (float)(queue2.readBuffer()[i] * (1.0f / 32768.0f));
        fftR_in_buf[i] *= 0.5*(1-arm_cos_f32(2*PI*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;
        }
      }
    }
    
//    for (int i = 0; i < numofpeaksL; i = i+1){
//      tmp_find_candidatesL[i] = freqpowL[peaksindexL[i]];
//    }
//
//    for (int i = 0; i < numofpeaksR; i = i+1){
//      tmp_find_candidatesR[i] = freqpowR[peaksindexR[i]];
//    }
//
//    for (int i = 0; i < numofcandidates; i = i+1){
//      arm_max_f32(tmp_find_candidatesL, fftsize/2, &candidatespowL[i], &candindexL[i]);
//      arm_max_f32(tmp_find_candidatesR, fftsize/2, &candidatespowR[i], &candindexR[i]);
//      tmp_find_candidatesL[candindexL[i]] = 0;
//      tmp_find_candidatesR[candindexR[i]] = 0;
//    }

    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];
      }
    }

    int16_t outsamplesL[fftsize];
    int16_t outsamplesR[fftsize];
    
    for (int i = 0; i < numofnotch; i = i+1){
      float32_t NotchCoeffs[5];
      if (i <= numofhowlingL){
        NotchCoeffs[0] = 1.0f;
        NotchCoeffs[1] = -2.0f * arm_cos_f32(PI*howlingindexL[i]/(fftsize/2));
        NotchCoeffs[2] = 1.0f;
        NotchCoeffs[3] = -2.0f * r * arm_cos_f32(PI*howlingindexL[i]/(fftsize/2));
        NotchCoeffs[4] = r * r;
      }
      else {
        NotchCoeffs[0] = 1.0f;
        NotchCoeffs[1] = 0.0f;
        NotchCoeffs[2] = 0.0f;
        NotchCoeffs[3] = 0.0f;
        NotchCoeffs[4] = 0.0f;
      }
      for (int j = 0; j < fftsize; j = j+1){
        outsamplesL[j] = (int16_t)(BiquadNotch(fftL_in_buf[j], NotchCoeffs, sZ1L, sZ2L) * 32768.0f);
      }
      
    }

    for (int i = 0; i < numofnotch; i = i+1){
      float32_t NotchCoeffs[5];
      if (i <= numofhowlingR){
        NotchCoeffs[0] = 1.0f;
        NotchCoeffs[1] = -2.0f * arm_cos_f32(PI*howlingindexR[i]/(fftsize/2));
        NotchCoeffs[2] = 1.0f;
        NotchCoeffs[3] = -2.0f * r * arm_cos_f32(PI*howlingindexR[i]/(fftsize/2));
        NotchCoeffs[4] = r * r;
      }
      else {
        NotchCoeffs[0] = 1.0f;
        NotchCoeffs[1] = 0.0f;
        NotchCoeffs[2] = 0.0f;
        NotchCoeffs[3] = 0.0f;
        NotchCoeffs[4] = 0.0f;
      }
      for (int j = 0; j < fftsize; j = j+1){
        outsamplesR[j] = (int16_t)(BiquadNotch(fftR_in_buf[j], NotchCoeffs, sZ1R, sZ2R) * 32768.0f);
      }
    }

    queue3.play(outsamplesL, fftsize);
    queue4.play(outsamplesR, fftsize);
  }
}

float32_t BiquadNotch (float32_t insample, float32_t* NotchCoeffs, float32_t* sZ1, float32_t* sZ2){
  return insample;
}
 
Dear Paul,

I have finished all the part of my code and now the error is like this. My whole code and the error messages are shown below.
Code:
#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
AudioInputUSB            usb1;           //xy=266.5,251
AudioInputI2S            i2s1;           //xy=268.25,391.75
AudioMixer4              mixer3;         //xy=489,404
AudioEffectFreeverb      freeverb1;      //xy=713.25,403.75
AudioMixer4              mixer1;         //xy=994.611083984375,263.0793762207031
AudioMixer4              mixer2;         //xy=996.0753173828125,410.98419189453125
AudioRecordQueue         queue1;         //xy=1253.416748046875,264.2777862548828
AudioRecordQueue         queue2;         //xy=1254.8333740234375,410.8611145019531
AudioPlayQueue           queue3;         //xy=1438,267
AudioPlayQueue           queue4;         //xy=1439,411
AudioOutputUSB           usb2;           //xy=1664.4444160461426,273.33335399627686
AudioOutputI2S           i2s2;           //xy=1665.3333282470703,404.33333015441895
AudioConnection          patchCord1(usb1, 0, mixer1, 0);
AudioConnection          patchCord2(usb1, 1, mixer2, 0);
AudioConnection          patchCord3(i2s1, 0, mixer3, 0);
AudioConnection          patchCord4(i2s1, 1, mixer3, 1);
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, i2s2, 0);
AudioConnection          patchCord11(queue3, 0, usb2, 0);
AudioConnection          patchCord12(queue4, 0, i2s2, 1);
AudioConnection          patchCord13(queue4, 0, usb2, 1);
// GUItool: end automatically generated code

int fftsize = 512;
int numofnotch = 10;
float32_t r = 0.85;
int PAPRthreshold = 6;

void setup() {
  // put your setup code here, to run once:
  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;

    for (int j = 0; j < fftsize/128; j = j+1){
      for (int i = 128*j; i < 128*(j+1); i = i+1){
        fftL_in_buf[i] = (float)(queue1.readBuffer()[i] * (1.0f / 32768.0f));
        fftL_in_buf[i] *= 0.5*(1-arm_cos_f32(2*PI*i/fftsize));
        fftR_in_buf[i] = (float)(queue2.readBuffer()[i] * (1.0f / 32768.0f));
        fftR_in_buf[i] *= 0.5*(1-arm_cos_f32(2*PI*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];
      }
    }

    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 NotchCoeffs[5];
      if (i <= numofhowlingL){
        NotchCoeffs[0] = 1.0f;
        NotchCoeffs[1] = -2.0f * arm_cos_f32(PI*howlingindexL[i]/(fftsize/2));
        NotchCoeffs[2] = 1.0f;
        NotchCoeffs[3] = -2.0f * r * arm_cos_f32(PI*howlingindexL[i]/(fftsize/2));
        NotchCoeffs[4] = r * r;
      }
      else {
        NotchCoeffs[0] = 1.0f;
        NotchCoeffs[1] = 0.0f;
        NotchCoeffs[2] = 0.0f;
        NotchCoeffs[3] = 0.0f;
        NotchCoeffs[4] = 0.0f;
      }
      for (int j = 0; j < fftsize; j = j+1){
        outsamplesL[j] = (int16_t)(BiquadNotch(filterL_in_buf[j], NotchCoeffs, 0, i) * 32768.0f);
        filterL_in_buf[j] = BiquadNotch(filterL_in_buf[j], NotchCoeffs, 0, i);
      }
      
    }

    for (int i = 0; i < numofnotch; i = i+1){
      float32_t NotchCoeffs[5];
      if (i <= numofhowlingR){
        NotchCoeffs[0] = 1.0f;
        NotchCoeffs[1] = -2.0f * arm_cos_f32(PI*howlingindexR[i]/(fftsize/2));
        NotchCoeffs[2] = 1.0f;
        NotchCoeffs[3] = -2.0f * r * arm_cos_f32(PI*howlingindexR[i]/(fftsize/2));
        NotchCoeffs[4] = r * r;
      }
      else {
        NotchCoeffs[0] = 1.0f;
        NotchCoeffs[1] = 0.0f;
        NotchCoeffs[2] = 0.0f;
        NotchCoeffs[3] = 0.0f;
        NotchCoeffs[4] = 0.0f;
      }
      for (int j = 0; j < fftsize; j = j+1){
        outsamplesR[j] = (int16_t)(BiquadNotch(filterR_in_buf[j], NotchCoeffs, 1, i) * 32768.0f);
        filterR_in_buf[j] = BiquadNotch(filterR_in_buf[j], NotchCoeffs, 1, i);
      }
    }

    queue3.play(outsamplesL, fftsize);
    queue4.play(outsamplesR, fftsize);
  }
}

float32_t BiquadNotch(float32_t insample, float32_t* NotchCoeffs, int channel, int filterindex){
  static float32_t sZ1[2][11];
  static float32_t sZ2[2][11];
  float32_t outsample = NotchCoeffs[0] * insample + NotchCoeffs[1] * sZ1[channel][filterindex] + NotchCoeffs[2] * sZ2[channel][filterindex] - NotchCoeffs[3] * sZ1[channel][filterindex + 1] - NotchCoeffs[4] * sZ2[channel][filterindex + 1];
  sZ2[channel][filterindex] = sZ1[channel][filterindex];
  sZ2[channel][filterindex + 1] = sZ1[channel][filterindex + 1];
  sZ1[channel][filterindex] = insample;
  sZ1[channel][filterindex + 1] = outsample;
  return outsample;
}

Code:
Arduino: 1.8.16 (Windows 10), TD: 1.55, Board: "Teensy 4.1, Audio, 600 MHz, Faster, US English"





















C:\Users\wuxue\AppData\Local\Temp\arduino_build_658050\sketch\NHS.ino.cpp.o: In function `loop':

D:\NHS/NHS.ino:205: undefined reference to `AudioPlayQueue::play(short const*, unsigned long)'

D:\NHS/NHS.ino:206: undefined reference to `AudioPlayQueue::play(short const*, unsigned long)'

collect2.exe: error: ld returned 1 exit status

Multiple libraries were found for "SD.h"

 Used: D:\Arduino\hardware\teensy\avr\libraries\SD

 Not used: D:\Arduino\libraries\SD

Error compiling for board Teensy 4.1.



This report would have more information with
"Show verbose output during compilation"
option enabled in File -> Preferences.
 
You aren't using the most recent version of the Arduino IDE and Teensyduino. The version of Teensyduino you are using does not have the play() function in the Audio library AudioPlayQueue.
You should upgrade to Arduino IDE 1.8.19 and Teensyduino 1.56

Pete
 
Hi el_supremo,

My code pass the validation but it doesn't work. There is no sound come out. Here are my current code. Could you please tell me what's the problem? Many thanks!
Code:
#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
AudioInputUSB            usb1;           //xy=266.5,251
AudioInputI2S            i2s1;           //xy=268.25,391.75
AudioMixer4              mixer3;         //xy=489,404
AudioEffectFreeverb      freeverb1;      //xy=713.25,403.75
AudioMixer4              mixer1;         //xy=994.611083984375,263.0793762207031
AudioMixer4              mixer2;         //xy=996.0753173828125,410.98419189453125
AudioRecordQueue         queue1;         //xy=1253.416748046875,264.2777862548828
AudioRecordQueue         queue2;         //xy=1254.8333740234375,410.8611145019531
AudioPlayQueue           queue3;         //xy=1438,267
AudioPlayQueue           queue4;         //xy=1439,411
AudioOutputUSB           usb2;           //xy=1664.4444160461426,273.33335399627686
AudioOutputI2S           i2s2;           //xy=1665.3333282470703,404.33333015441895
AudioConnection          patchCord1(usb1, 0, mixer1, 0);
AudioConnection          patchCord2(usb1, 1, mixer2, 0);
AudioConnection          patchCord3(i2s1, 0, mixer3, 0);
AudioConnection          patchCord4(i2s1, 1, mixer3, 1);
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, i2s2, 0);
AudioConnection          patchCord11(queue3, 0, usb2, 0);
AudioConnection          patchCord12(queue4, 0, i2s2, 1);
AudioConnection          patchCord13(queue4, 0, usb2, 1);
// GUItool: end automatically generated code

int fftsize = 512;
int numofnotch = 10;
float32_t r = 0.85;
int PAPRthreshold = 6;

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;

    for (int j = 0; j < fftsize/128; j = j+1){
      for (int i = 128*j; i < 128*(j+1); i = i+1){
        fftL_in_buf[i] = (float)(queue1.readBuffer()[i]) * (1.0f / 32768.0f);
        fftL_in_buf[i] *= 0.5*(1-arm_cos_f32(2*PI*i/fftsize));
        fftR_in_buf[i] = (float)(queue2.readBuffer()[i]) * (1.0f / 32768.0f);
        fftR_in_buf[i] *= 0.5*(1-arm_cos_f32(2*PI*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];
      }
    }

    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 NotchCoeffs[5];
      if (i <= numofhowlingL){
        NotchCoeffs[0] = 1.0f;
        NotchCoeffs[1] = -2.0f * arm_cos_f32(PI*howlingindexL[i]/(fftsize/2));
        NotchCoeffs[2] = 1.0f;
        NotchCoeffs[3] = -2.0f * r * arm_cos_f32(PI*howlingindexL[i]/(fftsize/2));
        NotchCoeffs[4] = r * r;
      }
      else {
        NotchCoeffs[0] = 1.0f;
        NotchCoeffs[1] = 0.0f;
        NotchCoeffs[2] = 0.0f;
        NotchCoeffs[3] = 0.0f;
        NotchCoeffs[4] = 0.0f;
      }
      for (int j = 0; j < fftsize; j = j+1){
        outsamplesL[j] = (int16_t)(BiquadNotch(filterL_in_buf[j], NotchCoeffs, 0, i) * 32768.0f);
        filterL_in_buf[j] = BiquadNotch(filterL_in_buf[j], NotchCoeffs, 0, i);
      }
      
    }

    for (int i = 0; i < numofnotch; i = i+1){
      float32_t NotchCoeffs[5];
      if (i <= numofhowlingR){
        NotchCoeffs[0] = 1.0f;
        NotchCoeffs[1] = -2.0f * arm_cos_f32(PI*howlingindexR[i]/(fftsize/2));
        NotchCoeffs[2] = 1.0f;
        NotchCoeffs[3] = -2.0f * r * arm_cos_f32(PI*howlingindexR[i]/(fftsize/2));
        NotchCoeffs[4] = r * r;
      }
      else {
        NotchCoeffs[0] = 1.0f;
        NotchCoeffs[1] = 0.0f;
        NotchCoeffs[2] = 0.0f;
        NotchCoeffs[3] = 0.0f;
        NotchCoeffs[4] = 0.0f;
      }
      for (int j = 0; j < fftsize; j = j+1){
        outsamplesR[j] = (int16_t)(BiquadNotch(filterR_in_buf[j], NotchCoeffs, 1, i) * 32768.0f);
        filterR_in_buf[j] = BiquadNotch(filterR_in_buf[j], NotchCoeffs, 1, i);
      }
    }

    queue3.play(outsamplesL, fftsize);
    queue4.play(outsamplesR, fftsize);
  }
}

float32_t BiquadNotch(float32_t insample, float32_t* NotchCoeffs, int channel, int filterindex){
  static float32_t sZ1[2][11];
  static float32_t sZ2[2][11];
  float32_t outsample = NotchCoeffs[0] * insample + NotchCoeffs[1] * sZ1[channel][filterindex] + NotchCoeffs[2] * sZ2[channel][filterindex] - NotchCoeffs[3] * sZ1[channel][filterindex + 1] - NotchCoeffs[4] * sZ2[channel][filterindex + 1];
  sZ2[channel][filterindex] = sZ1[channel][filterindex];
  sZ2[channel][filterindex + 1] = sZ1[channel][filterindex + 1];
  sZ1[channel][filterindex] = insample;
  sZ1[channel][filterindex + 1] = outsample;
  return outsample;
}
 
Code:
(float)(queue1.readBuffer()[i])
This won't work. readBuffer returns the address of a buffer, if one is available, and returns NULL otherwise.
Using 'i' as an array index will also fail because it will have very large values when in fact it can only range from zero to 127.
You will have to save the address in a pointer and modify the array addressing - something like this:
Code:
    for (int j = 0; j < fftsize/128; j = j+1){
      short *qp1 = queue1.readBuffer();
      short *qp2 = queue2.readBuffer();
      for (int i = 0; i < 128; i = i+1){
        fftL_in_buf[128*j + i] = (float)(qp1[i]) * (1.0f / 32768.0f);
        fftL_in_buf[128*j + i] *= 0.5*(1-arm_cos_f32(2*PI*(float)(128*j+i)/fftsize));
        fftR_in_buf[128*j + i] = (float)(qqp2[i]) * (1.0f / 32768.0f);
        fftR_in_buf[128*j + i] *= 0.5*(1-arm_cos_f32(2*PI*(float)(128*j+i)/fftsize));
      }

      queue1.freeBuffer();
      queue2.freeBuffer();
    }

Pete
 
Dear el_supremo,

Thank you for your reply! I just noticed that the index 'i' has somo mistakes and I have modified it. The codes are:
Code:
    for (int j = 0; j < fftsize/128; j = j+1){
      for (int i = 128*j; i < 128*(j+1); i = i+1){
        fftL_in_buf[i] = (float32_t)(queue1.readBuffer()[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)(queue2.readBuffer()[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();
    }
I think this will do the same things that your code does. Is it?
However, it still doesn't come out any sound. I have use the PassThroughUSB example to test my hardware, and the hardware works well.
 
No it won't. You've fixed the index but you can't treat queue1.readBuffer() as an array. You have to save the address in a pointer and then use it as I showed in #12.

Pete
 
Hi el_supremo,

I have modified my code according to your suggestions. The problem of unstable device recognition has been solved. However, it still doesn't come out any sound. My current codes are:
Code:
#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
AudioInputUSB            usb1;           //xy=266.5,251
AudioInputI2S            i2s1;           //xy=268.25,391.75
AudioMixer4              mixer3;         //xy=489,404
AudioEffectFreeverb      freeverb1;      //xy=713.25,403.75
AudioMixer4              mixer1;         //xy=994.611083984375,263.0793762207031
AudioMixer4              mixer2;         //xy=996.0753173828125,410.98419189453125
AudioRecordQueue         queue1;         //xy=1253.416748046875,264.2777862548828
AudioRecordQueue         queue2;         //xy=1254.8333740234375,410.8611145019531
AudioPlayQueue           queue3;         //xy=1438,267
AudioPlayQueue           queue4;         //xy=1439,411
AudioOutputUSB           usb2;           //xy=1664.4444160461426,273.33335399627686
AudioOutputI2S           i2s2;           //xy=1665.3333282470703,404.33333015441895
AudioConnection          patchCord1(usb1, 0, mixer1, 0);
AudioConnection          patchCord2(usb1, 1, mixer2, 0);
AudioConnection          patchCord3(i2s1, 0, mixer3, 0);
AudioConnection          patchCord4(i2s1, 1, mixer3, 1);
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, i2s2, 0);
AudioConnection          patchCord11(queue3, 0, usb2, 0);
AudioConnection          patchCord12(queue4, 0, i2s2, 1);
AudioConnection          patchCord13(queue4, 0, usb2, 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){
      numofhowlingL = 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];
        }
      }
    }

    while (numofhowlingR > numofnotch){
      numofhowlingR = 0;
      PAPRthreshold += 1;
      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 NotchCoeffs[5];
      if (i <= numofhowlingL){
        NotchCoeffs[0] = 1.0f;
        NotchCoeffs[1] = -2.0f * arm_cos_f32(PI*howlingindexL[i]/(fftsize/2));
        NotchCoeffs[2] = 1.0f;
        NotchCoeffs[3] = -2.0f * r * arm_cos_f32(PI*howlingindexL[i]/(fftsize/2));
        NotchCoeffs[4] = r * r;
      }
      else {
        NotchCoeffs[0] = 1.0f;
        NotchCoeffs[1] = 0.0f;
        NotchCoeffs[2] = 0.0f;
        NotchCoeffs[3] = 0.0f;
        NotchCoeffs[4] = 0.0f;
      }
      
      for (int j = 0; j < fftsize; j = j+1){
        outsamplesL[j] = (int16_t)(BiquadNotch(filterL_in_buf[j], NotchCoeffs, 0, i) * 32768.0f);
        filterL_in_buf[j] = BiquadNotch(filterL_in_buf[j], NotchCoeffs, 0, i);
      }
    }

    for (int i = 0; i < numofnotch; i = i+1){
      float32_t NotchCoeffs[5];
      if (i <= numofhowlingR){
        NotchCoeffs[0] = 1.0f;
        NotchCoeffs[1] = -2.0f * arm_cos_f32(PI*howlingindexR[i]/(fftsize/2));
        NotchCoeffs[2] = 1.0f;
        NotchCoeffs[3] = -2.0f * r * arm_cos_f32(PI*howlingindexR[i]/(fftsize/2));
        NotchCoeffs[4] = r * r;
      }
      else {
        NotchCoeffs[0] = 1.0f;
        NotchCoeffs[1] = 0.0f;
        NotchCoeffs[2] = 0.0f;
        NotchCoeffs[3] = 0.0f;
        NotchCoeffs[4] = 0.0f;
      }
      
      for (int j = 0; j < fftsize; j = j+1){
        outsamplesR[j] = (int16_t)(BiquadNotch(filterR_in_buf[j], NotchCoeffs, 1, i) * 32768.0f);
        filterR_in_buf[j] = BiquadNotch(filterR_in_buf[j], NotchCoeffs, 1, i);
      }
    }

    queue3.play(outsamplesL, fftsize);
    queue4.play(outsamplesR, fftsize);
  }
}

float32_t BiquadNotch(float32_t insample, float32_t* NotchCoeffs, int channel, int filterindex){
  static float32_t sZ1[2][11];
  static float32_t sZ2[2][11];
  float32_t outsample = NotchCoeffs[0] * insample + NotchCoeffs[1] * sZ1[channel][filterindex] + NotchCoeffs[2] * sZ2[channel][filterindex] - NotchCoeffs[3] * sZ1[channel][filterindex + 1] - NotchCoeffs[4] * sZ2[channel][filterindex + 1];
  sZ2[channel][filterindex] = sZ1[channel][filterindex];
  sZ2[channel][filterindex + 1] = sZ1[channel][filterindex + 1];
  sZ1[channel][filterindex] = insample;
  sZ1[channel][filterindex + 1] = outsample;
  return outsample;
}
 
Back
Top