From 131bdce9de42226c66b9d19a63abc604394c265b Mon Sep 17 00:00:00 2001 From: boblark Date: Thu, 25 Mar 2021 11:52:46 -0700 Subject: [PATCH] added squelch to FM --- RadioFMDetector_F32.cpp | 63 +++++++++----- RadioFMDetector_F32.h | 130 ++++++++++++++++++++--------- examples/ReceiverFM/ReceiverFM.ino | 127 +++++++++++++++++++++------- 3 files changed, 229 insertions(+), 91 deletions(-) diff --git a/RadioFMDetector_F32.cpp b/RadioFMDetector_F32.cpp index 37823b1..81f5c1e 100644 --- a/RadioFMDetector_F32.cpp +++ b/RadioFMDetector_F32.cpp @@ -25,7 +25,7 @@ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE * SOFTWARE. - * + * * See RadioFMDetector_F32.h for usage details */ #include "RadioFMDetector_F32.h" @@ -35,39 +35,40 @@ // ==== 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 float32_t v_q[128]; mathDSP_F32 mathDSP1; // Math support functions - + #if TEST_TIME_FM if (iitt++ >1000000) iitt = -10; - uint32_t t1, t2; + uint32_t t1, t2; t1 = tElapse; #endif // Get input to FM Detector block blockIn = AudioStream_F32::receiveWritable_f32(0); if (!blockIn) { - if(errorPrintFM) Serial.println("FMDET-ERR: No input memory"); return; - } + } // If there's no coefficient table, give up. if (fir_IQ_Coeffs == NULL) { if(errorPrintFM) Serial.println("FMDET-ERR: No IQ FIR Coefficients"); AudioStream_F32::release(blockIn); return; - } + } if (fir_Out_Coeffs == NULL) { if(errorPrintFM) Serial.println("FMDET-ERR: No Out FIR Coefficients"); AudioStream_F32::release(blockIn); return; - } + } // Try to get a block for the FM output blockOut = AudioStream_F32::allocate_f32(); @@ -75,7 +76,10 @@ void RadioFMDetector_F32::update(void) { if(errorPrintFM) Serial.println("FMDET-ERR: No Output Memory"); AudioStream_F32::release(blockIn); 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 @@ -99,8 +103,8 @@ void RadioFMDetector_F32::update(void) { b = sinTable512_f32[index_sine+1]; /* deltaPhase will be the same as used for sin */ v_q[i] = blockIn->data[i] * (a + 0.001953125*(b-a)*deltaPhase); - } - + } + // Do I FIR and Q FIR. We can borrow blockIn and blockOut at this point //void arm_fir_f32( const arm_fir_instance_f32* S, float32_t* pSrc, float32_t* pDst, uint32_t blockSize) arm_fir_f32(&FMDet_I_inst, v_i, blockIn->data, (uint32_t)blockIn->length); @@ -119,18 +123,39 @@ void RadioFMDetector_F32::update(void) { // Data point is now dtemp2. Apply single pole de-emphasis LPF, in place dLast = Kdem * dtemp2 + OneMinusKdem * dLast; blockIn->data[i] = dLast; // and save to an array - } + } // 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; idata[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); -#if TEST_TIME_FM + + 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); } + if(iitt++ < 0) {Serial.print("At end of FM Det "); Serial.println (t2 - t1); } t1 = tElapse; -#endif +#endif } diff --git a/RadioFMDetector_F32.h b/RadioFMDetector_F32.h index a860f65..3086fe9 100644 --- a/RadioFMDetector_F32.h +++ b/RadioFMDetector_F32.h @@ -33,7 +33,7 @@ * frequency of the input sine wave frequency, i.e., an it is an FM detector. * The input needs to be band limited below the lower frequency side of the * input, typically 10 kHz. This is not part of this block. - * + * * NOTE: Due to the sample frequencies we are working with, like 44.1 kHz, this * detector cannot handle full FM broadcast bandwidths. It is suitable for * NBFM as used in communications, marine radio, ham radio, etc. @@ -51,17 +51,17 @@ * be removed before the atan2. A pair of FIR filters, using FIR_IQ_Coeffs * are used. These are again programmable and default to a 29-tap LPF with * a 5 kHz cutoff. - * + * * Status: Tested static, tested with FM modulated Fluke 6061B. * An input of about 60 microvolts to the SGTL5000 gave 12 dB SINAD. * The output sounded good. Tested T3.6 and T4.0. No known bugs - * + * * Output: Float, sensitivity is 2*pi*(f - fCenter)*sample_rate_Hz * For 44117Hz samplerate, this is 0.000142421 per Hz - * + * * Accuracy: The function used is precise. However, the approximations, such * fastAtan2, slightly limit the accuracy. A 200 point sample of a - * 14 kHz input had an average error of 0.03 Hz + * 14 kHz input had an average error of 0.03 Hz * and a standard deviation of 0.81 Hz. * The largest errors in this sample were about +/- 1.7 Hz. This is * with the default filters. @@ -80,24 +80,24 @@ * filterIQ(float *fir_IQ_Coeffs, uint nFIR_IQ) sets output filtering where: * float32_t* fir_IQ_Coeffs is an array of coefficients * uint nFIR_IQ is the number of coefficients, max 60 - * + * * setSampleRate_Hz(float32_t _sampleRate_Hz) allows dynamic changing of * the sample rate (experimental as of May 2020). - * + * * returnInitializeFMError() Returns the initialization errors. * B0001 (value 1) is an error in the IQ FIR Coefficients or quantity. * B0010 (value 2) is an error in the Output FIR Coefficients or quantity. * B0100 (value 4) is an error in the de-emphasis constant * B1000 (value 8) is center frequency above half-sample frequency. * All for debug. - * + * * showError(uint16_t e) Turns error printing in the update function on (e=1) * or off (e=0). For debug only. * * Time: For T3.6, an update of a 128 sample block, 370 microseconds, or * 2.9 microseconds per data point. * For T4.0, 87 microseconds, or 0.68 microseconds per data point. - * + * * Error checking: See functions setSampleRate_Hz() and returnInitializeFMError() * above. */ @@ -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,26 +163,53 @@ public: initializeFM(); } - void setSampleRate_Hz(float32_t _sampleRate_Hz) { - if (fCenter > _sampleRate_Hz/2.0f) { // Check freq range + // 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; - } + } 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; - } - + } + uint16_t returnInitializeFMError(void) { - return initializeFMErrors; - } + return initializeFMErrors; + } void update(void); - + private: // One input data pointer audio_block_f32_t *inputQueueArray_f32[1]; @@ -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 @@ -220,29 +266,33 @@ int32_t iitt = 999000; // count up to a million during startup arm_fir_instance_f32 FMDet_Out_inst; float32_t State_Out_F32[AUDIO_BLOCK_SAMPLES + MAX_FIR_OUT_COEFFS]; - + // Initialize the FM Detector, part of setting up and changing parameters void initializeFM(void) { - if (fir_IQ_Coeffs && nFIR_IQ <= MAX_FIR_IQ_COEFFS) { + if (fir_IQ_Coeffs && nFIR_IQ <= MAX_FIR_IQ_COEFFS) { /* the instance setup call - * void arm_fir_init_f32( + * void arm_fir_init_f32( * arm_fir_instance_f32* S, points to instance of floating-point FIR filter structure. * uint16_t numTaps, Number of filter coefficients in the filter. * float32_t* pCoeffs, points to the filter coefficients buffer. * float32_t* pState, points to the state buffer. * uint32_t blockSize) Number of samples that are processed per call. */ - arm_fir_init_f32(&FMDet_I_inst, nFIR_IQ, (float32_t*)fir_IQ_Coeffs, &State_I_F32[0], (uint32_t)block_size); - arm_fir_init_f32(&FMDet_Q_inst, nFIR_IQ, (float32_t*)fir_IQ_Coeffs, &State_Q_F32[0], (uint32_t)block_size); - } - else initializeFMErrors |= B0001; + arm_fir_init_f32(&FMDet_I_inst, nFIR_IQ, (float32_t*)fir_IQ_Coeffs, &State_I_F32[0], (uint32_t)block_size); + arm_fir_init_f32(&FMDet_Q_inst, nFIR_IQ, (float32_t*)fir_IQ_Coeffs, &State_Q_F32[0], (uint32_t)block_size); + } + else initializeFMErrors |= B0001; if (fir_Out_Coeffs && nFIR_Out <= MAX_FIR_OUT_COEFFS) { arm_fir_init_f32(&FMDet_Out_inst, nFIR_Out, (float32_t*)fir_Out_Coeffs, &State_Out_F32[0], (uint32_t)block_size); - } + } 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 * fs = 44100 Hz, < 5kHz ripple 0.29 dB, >9 kHz, -62 dB, 29 taps @@ -259,19 +309,19 @@ int32_t iitt = 999000; // count up to a million during startup /* FIR filter designed with http://t-filter.appspot.com * fs = 44100 Hz, < 3kHz ripple 0.36 dB, >6 kHz, -60 dB, 39 taps - * Corrected to give DC gain = 1.00 + * Corrected to give DC gain = 1.00 */ float32_t fir_Out39[39] = { - -0.0008908477f, -0.0008401274f, -0.0001837353f, 0.0017556005f, - 0.0049353322f, 0.0084952916f, 0.0107668722f, 0.0097441685f, - 0.0039877576f, -0.0063455016f, -0.0188069300f, -0.0287453055f, - -0.0303831521f, -0.0186809770f, 0.0085931270f, 0.0493875744f, - 0.0971742012f, 0.1423015880f, 0.1745838382f, 0.1863024485f, - 0.1745838382f, 0.1423015880f, 0.0971742012f, 0.0493875744f, - 0.0085931270f, -0.0186809770f, -0.0303831521f, -0.0287453055f, - -0.0188069300f, -0.0063455016f, 0.0039877576f, 0.0097441685f, - 0.0107668722f, 0.0084952916f, 0.0049353322f, 0.0017556005f, + -0.0008908477f, -0.0008401274f, -0.0001837353f, 0.0017556005f, + 0.0049353322f, 0.0084952916f, 0.0107668722f, 0.0097441685f, + 0.0039877576f, -0.0063455016f, -0.0188069300f, -0.0287453055f, + -0.0303831521f, -0.0186809770f, 0.0085931270f, 0.0493875744f, + 0.0971742012f, 0.1423015880f, 0.1745838382f, 0.1863024485f, + 0.1745838382f, 0.1423015880f, 0.0971742012f, 0.0493875744f, + 0.0085931270f, -0.0186809770f, -0.0303831521f, -0.0287453055f, + -0.0188069300f, -0.0063455016f, 0.0039877576f, 0.0097441685f, + 0.0107668722f, 0.0084952916f, 0.0049353322f, 0.0017556005f, -0.0001837353f, -0.0008401274f, -0.0008908477f }; -}; -#endif + }; +#endif diff --git a/examples/ReceiverFM/ReceiverFM.ino b/examples/ReceiverFM/ReceiverFM.ino index f2b1b9d..21e7cb9 100644 --- a/examples/ReceiverFM/ReceiverFM.ino +++ b/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 +/* ReceiverFM.ino Bob Larkin 26 April 2020 + * 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; -RadioFMDetector_F32 fmDet1; -AudioRecordQueue_F32 queue1; -AudioConvert_F32toI16 cnvrtOut; -AudioOutputI2S 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); +// can be loud, so lower the gain below (like 0.05 gain). +AudioInputI2S_F32 i2sIn; +RadioFMDetector_F32 fmDet1; +AudioAnalyzeFFT1024_F32 fft1; +AudioRecordQueue_F32 queue1; +AudioOutputI2S_F32 i2sOut; +AudioControlSGTL5000 sgtl5000_1; +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: "); @@ -75,31 +92,60 @@ void setup(void) { // The following enables error checking inside of the "ubdate()" // 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++) { - *pd1++ = *pq1++; - } + for(k=0; k<128; k++) + *pd1++ = *pq1++; queue1.freeBuffer(); if(i++==3) { i=4; // Only collect 4 blocks queue1.end(); // No more data to queue1 + } } - } else { queue1.freeBuffer(); + } } - } // We have 512 data samples. Serial.print them if(i == 4) { #if SINE_ADC @@ -111,9 +157,9 @@ void loop(void) { Serial.print (0.000022667*(float32_t)k, 6); Serial.print (", "); Serial.print (dt1[k],7); Serial.print (", "); Serial.println (dt1[k]/0.000142421, 2); // Convert to Hz - } + } i = 5; - } + } if(i==5) { i = 6; Serial.print("CPU: Percent Usage, Max: "); @@ -131,5 +177,22 @@ void loop(void) { Serial.print(", "); 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; + }