added squelch to FM

RadioFMDetector_F32.cpp

@@ -35,8 +35,10 @@
 
 //  ====  UPDATE  ====
 void RadioFMDetector_F32::update(void) {
-  audio_block_f32_t  *blockIn, *blockOut=NULL;
+  audio_block_f32_t  *blockIn, *blockOut=NULL, *blockZero;
 
+  static float saveIn = 0.0f;  // for squelch
+  static float saveOut = 0.0f;
   uint16_t i, index_sine;
   float32_t deltaPhase, a, b, dtemp1, dtemp2;
   float32_t  v_i[128];  // max size
@@ -52,7 +54,6 @@ void RadioFMDetector_F32::update(void) {
   // Get input to FM Detector block
   blockIn = AudioStream_F32::receiveWritable_f32(0);
   if (!blockIn) {
-     if(errorPrintFM)  Serial.println("FMDET-ERR: No input memory");
      return;
      }
 
@@ -77,6 +78,9 @@ void RadioFMDetector_F32::update(void) {
     return;
     }
 
+  blockZero = AudioStream_F32::allocate_f32();
+     for(int kk=0; kk<128; kk++)  blockZero->data[kk] = 0.0f;
+
   // Generate sine and cosine of center frequency and double-balance mix
   // these with the input signal to produce an intermediate result
   // saved as v_i[] and v_q[]
@@ -123,11 +127,32 @@ void RadioFMDetector_F32::update(void) {
 
     // Do output FIR filter.  Data now in blockIn.
     arm_fir_f32(&FMDet_Out_inst, blockIn->data, blockOut->data,  (uint32_t)blockIn->length);
-    AudioStream_F32::release(blockIn);
 
-    // Transmit the data
+    // Squelch picks the audio from before the output filter and does a 4-pole BiQuad BPF
+    // blockIn->data still has the data we need
+    arm_biquad_cascade_df1_f32(&iirSqIn_inst, blockIn->data,
+             blockIn->data, blockIn->length);
+
+    // Update the Squelch full-wave envelope detector and single pole LPF
+    float sumNoise = 0.0f;
+    for(i=0; i<block_size; i++)
+       sumNoise += fabsf(blockIn->data[i]);  // Ave of rectified noise
+    squelchLevel = alpha*(sumNoise + saveIn) + gamma*saveOut;  // 1 pole
+    saveIn = sumNoise;
+    saveOut = squelchLevel;
+    // Add hysteresis here  <<<<
+
+    // Squelch gate sends audio to output 1 (right) if squelch is open
+    if(squelchLevel < squelchThreshold)
+        AudioStream_F32::transmit(blockOut, 1);
+    else
+        AudioStream_F32::transmit(blockZero, 1);
+
+    // The un-squelch controlled audio for tone decoding, etc., goes to output 0
     AudioStream_F32::transmit(blockOut, 0);
-    AudioStream_F32::release(blockOut);
+
+    AudioStream_F32::release(blockIn);        // Give back the blocks
+    AudioStream_F32::release(blockOut);      AudioStream_F32::release(blockZero);
 #if TEST_TIME_FM
   t2 = tElapse;
   if(iitt++ < 0) {Serial.print("At end of FM Det ");  Serial.println (t2 - t1); }

RadioFMDetector_F32.h

@@ -115,7 +115,7 @@
 #define TEST_TIME_FM 0
 
 class RadioFMDetector_F32 : public AudioStream_F32 {
-//GUI: inputs:1, outputs:1  //this line used for automatic generation of GUI node
+//GUI: inputs:1, outputs:2  //this line used for automatic generation of GUI node
 //GUI: shortName: FMDetector
 public:
     // Default block size and sample rate:
@@ -163,7 +163,33 @@ public:
         initializeFM();
     }
 
-    void setSampleRate_Hz(float32_t _sampleRate_Hz) {
+    // Provide for changing to user supplied BiQuad for Squelch input.
+    void setSquelchFilter(float* _sqCoeffs)  {
+       if( _sqCoeffs==NULL)
+          pCfSq = coeffSqIn;   // Default filter
+       else
+          pCfSq = _sqCoeffs;
+       initializeFM();
+       }
+
+    // The squelch level reads nominally 0.0 to 1.0 where
+    float getSquelchLevel (void)  {
+        return squelchLevel;
+        }
+
+    // The squelch threshold is nominally 0.7 where
+    // 0.0 always lets audio through. 
+    void setSquelchThreshold (float _sqTh)  {
+        squelchThreshold = _sqTh;
+        }
+
+    void setSquelchDecay (float _sqDcy)  {
+        gamma = _sqDcy;
+        alpha = 0.5f*(1.0f - gamma);
+        }
+
+
+/*    void setSampleRate_Hz(float32_t _sampleRate_Hz) {
         if (fCenter > _sampleRate_Hz/2.0f) {  // Check freq range
             initializeFMErrors |= 8;
             return;
@@ -171,7 +197,8 @@ public:
         sampleRate_Hz = _sampleRate_Hz;
         // update phase increment for new frequency
         phaseIncrement = 512.0f * fCenter / sampleRate_Hz;
-    }
+        }  // NOT SUPPORTED - No Dynamic changes
+*/
 
     void showError(uint16_t e) {
         errorPrintFM = e;
@@ -207,6 +234,25 @@ private:
     float32_t* fir_IQ_Coeffs = fir_IQ29;
     uint16_t nFIR_Out = 39;
     float32_t* fir_Out_Coeffs = fir_Out39;
+
+    /* Info - The structure from arm_biquad_casd_df1_inst_f32 consists of
+     *    uint32_t  numStages;
+     *    const float32_t *pCoeffs;  //Points to the array of coefficients, length 5*numStages.
+     *    float32_t *pState;         //Points to the array of state variables, length 4*numStages. */
+    arm_biquad_casd_df1_inst_f32 iirSqIn_inst;
+    // Default 2 stage Squelch input BiQuad filter, 3000 Hz, 4000 Hz both Q=5
+    // The -6 dB points are  2680 and 4420 Hz
+    // The -20 dB points are 2300 and 5300 Hz
+    float  coeffSqIn[10] = {
+        0.0398031529f, 0.0f, -0.0398031529f, 1.74762569f, -0.92039369f,
+        0.0511929547f, 0.0f, -0.0511929547f, 1.59770204f, -0.89761409f};
+    float* pCfSq = coeffSqIn;
+    float stateSqIn[8];
+    float squelchThreshold = 0.7f;
+    float squelchLevel = 1.0f;
+    float gamma = 0.99;
+    float alpha = 0.5f*(1.0f - gamma);
+
 #if TEST_TIME_FM
 elapsedMicros tElapse;
 int32_t iitt = 999000;     // count up to a million during startup
@@ -242,6 +288,10 @@ int32_t iitt = 999000;     // count up to a million during startup
             }
         else  initializeFMErrors |= B0010;
         dLast = 0.0;
+
+        // Initialize squelch Input BPF BiQuad instance (ARM DSP Math Library)
+        // https://www.keil.com/pack/doc/CMSIS/DSP/html/group__BiquadCascadeDF1.html
+        arm_biquad_cascade_df1_init_f32(&iirSqIn_inst, 2, pCfSq,  &stateSqIn[0]);
         }
 
     /* FIR filter designed with http://t-filter.appspot.com

examples/ReceiverFM/ReceiverFM.ino

@@ -1,11 +1,23 @@
 /* ReceiverFM.ino      Bob Larkin   26 April 2020
- * This is a simple test of introducing a sine wave to the
+ * No copyright.
+ * 
+ * This INO runs the FM detector on input data from the Codec. It
+ * requires an FM modulated signal source at 15 kHz.  The Serial out is
+ * the squelch level generated by the squelch noise detector.
+ * 
+ * Commented out is a test of introducing a sine wave to the
  * FM Detector and taking 512 samples of the output.  It is
  * a static test with a fixed frequency for test and so
  * the output DC value and noise can be tested.  Note that the 512
  * samples include the startup transient, so the first 300, 
  * or so, points should be ignored in seeing the DC value.
  * 
+ * An option is to change the squelch noise filter (commented out)
+ * and this serves as an example of changing the 10 coefficients.
+ * 
+ * Another commented out loop code will print the observed spectrum using
+ * the 1024 point FFT.  This shows the NBFM de-emphasis curve.
+ * 
  * Change the value of sine1.frequency to see the DC output change.
  * See FMReceiver2.ino for testing with real AC modulation.
  * 
@@ -22,32 +34,32 @@
 #define SINE_ADC 0
 
 #if SINE_ADC
+AudioSynthGaussian_F32      gwn1;
 AudioSynthWaveformSine_F32  sine1;
+AudioMixer4_F32             mix1;
 RadioFMDetector_F32         fmDet1;
+AudioOutputI2S_F32          i2sOut;
+AudioAnalyzeFFT1024_F32     fft1;
 AudioRecordQueue_F32        queue1;
-AudioConvert_F32toI16       cnvrtOut;
-AudioOutputI2S              i2sOut;
 AudioControlSGTL5000        sgtl5000_1;
-AudioConnection_F32    connect1(sine1,    0, fmDet1,   0);
-AudioConnection_F32    connect3(fmDet1,   0, cnvrtOut, 0);
-AudioConnection        connect4(cnvrtOut, 0, i2sOut,   0); // left
-AudioConnection_F32    connect5(fmDet1,   0, queue1,   0);
+AudioConnection_F32     connectA(gwn1,   0, mix1,   0);
+AudioConnection_F32     connectB(sine1,  0, mix1,   1);
+AudioConnection_F32     connectC(mix1,   0, fmDet1, 0);
 #else            // Input from Teensy Audio Adaptor SGTL5000
 // Note - With no input, the FM detector output is all noise.  This
-// can be loud, so one can add a gain block at the fmDet1 output (like 0.05 gain).
-AudioInputI2S          i2sIn;
-AudioConvert_I16toF32  cnvrtIn;
+// can be loud, so lower the gain below (like 0.05 gain).
+AudioInputI2S_F32         i2sIn;
 RadioFMDetector_F32       fmDet1;
+AudioAnalyzeFFT1024_F32   fft1;
 AudioRecordQueue_F32      queue1; 
-AudioConvert_F32toI16  cnvrtOut;
-AudioOutputI2S         i2sOut;
+AudioOutputI2S_F32        i2sOut;
 AudioControlSGTL5000      sgtl5000_1;
-AudioConnection        connect1(i2sIn,    0, cnvrtIn,  0); // left
-AudioConnection_F32    connect2(cnvrtIn,  0, fmDet1,   0);
-AudioConnection_F32    connect3(fmDet1,   0, cnvrtOut, 0);
-AudioConnection_F32    connect5(fmDet1,   0, queue1,   0);
-AudioConnection        connect7(cnvrtOut, 0, i2sOut,   0);
+AudioConnection_F32     connectD(i2sIn,   0, fmDet1,  0); // left
 #endif
+// Common for both input sources
+AudioConnection_F32     connect1(fmDet1,  1, i2sOut,  0); // Squelched
+AudioConnection_F32     connect2(fmDet1,  0, fft1,    0);
+AudioConnection_F32     connect3(fmDet1,  0, queue1,  0);
 
 float dt1[512];    // Place to save output
 float *pq1, *pd1;
@@ -56,17 +68,22 @@ int i;
 
 void setup(void) {
   AudioMemory(5);
-  AudioMemory_F32(5);
-  Serial.begin(300);  delay(1000);  // Any rate is OK
+  AudioMemory_F32(100);
+  Serial.begin(300);  delay(500);  // Any rate is OK
   Serial.println("Serial Started");
  
   sgtl5000_1.enable();
   sgtl5000_1.inputSelect(AUDIO_INPUT_LINEIN);   
 
 #if SINE_ADC
-  sine1.frequency(14000.0);
+  sine1.frequency(15000.0);
+  sine1.amplitude(0.0001f);
+  gwn1.amplitude(0.1f);
 #endif
 
+  fft1.setOutputType(FFT_DBFS);
+  fft1.setNAverage(10);
+
   // The FM detector has error checking during object construction
   // when Serial.print is not available.  See RadioFMDetector_F32.h:
   Serial.print("FM Initialization errors: ");
@@ -76,20 +93,49 @@ void setup(void) {
   // Output goes to the Serial (USB) Monitor.  Normally, this is quiet.
   fmDet1.showError(1);
 
+  fmDet1.setSquelchThreshold(0.7f);
+
+  // Here is an example of designing and using a non-default squelch
+  // noise filter.  The INO provides storage for the new coefficients.
+//  static float newFilter[10];
+  //                  freq     Q    ptr to Coeff
+//  setBandpassBiQuad(2000.0f, 3.0f, &newFilter[0]);
+//  setBandpassBiQuad(4000.0f, 3.0f, &newFilter[5]);
+//  fmDet1.setSquelchFilter(newFilter);
+  
+  // Set the volume control (0.0 to 1.0)
+  i2sOut.setGain(0.05f);
+
   queue1.begin(); 
   i = 0;  k=0;
 }
 
 void loop(void) {
+ float lData[512];
+
+  Serial.print("sqLevel "); Serial.println(fmDet1.getSquelchLevel(), 6);
+  delay(500);
+ 
+/*
+ if( fft1.available() ) {
+    float* pd = fft1.getData();
+    for(int k=0; k<512; k++)
+        lData[k] = pd[k];
+    for(int k=0; k<512; k++)
+        Serial.println(lData[k],3);
+    Serial.println(" -------");
+    }
+ */
+
+/*
   // Collect 512 samples and output to Serial
   // This "if" will be active for i = 0,1,2,3
   if (queue1.available() >= 1) {
     if( i>=0 && i<4) {
       pq1 = queue1.readBuffer();
       pd1 = &dt1[i*128];
-      for(k=0; k<128; k++) {
+      for(k=0; k<128; k++)
          *pd1++ = *pq1++; 
-      }
       queue1.freeBuffer();
       if(i++==3) {
          i=4;   // Only collect 4 blocks
@@ -132,4 +178,21 @@ void loop(void) {
     Serial.println(AudioMemoryUsageMax_F32());
     Serial.println();
     }
+ */
+  }
+
+    // Find BiQuad Coefficients for Squelch Noise filter.
+    // Only used if the default -6 dB points of 2680 and 4420 Hz
+    // are not suitable
+    void setBandpassBiQuad(float frequency, float q, float* cf) {
+        double w0 = frequency * (2 * 3.141592654 / 44100);   // sampleRate_Hz);
+        double sinW0 = sin(w0);
+        double alpha = sinW0 / ((double)q * 2.0);
+        double cosW0 = cos(w0);
+        double scale = 1.0 / (1.0+alpha);
+        /* b0 */ cf[0] = alpha * scale;
+        /* b1 */ cf[1] = 0;
+        /* b2 */ cf[2] = (-alpha) * scale;
+        /* a1 */ cf[3] = -(-2.0 * cosW0) * scale;
+        /* a2 */ cf[4] = -(1.0 - alpha) * scale;
         }