/* * RadioFMDiscriminator_F32 * 25 April 2022 Bob Larkin * With much credit to: * Chip Audette (OpenAudio) Feb 2017 * Building from AudioFilterFIR from Teensy Audio Library * (AudioFilterFIR credited to Pete (El Supremo)) * and of course, to PJRC for the Teensy and Teensy Audio Library * * Copyright (c) 2022 Bob Larkin * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal * in the Software without restriction, including without limitation the rights * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * copies of the Software, and to permit persons to whom the Software is * furnished to do so, subject to the following conditions: * * The above copyright notice and this permission notice shall be included in all * copies or substantial portions of the Software. * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * 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. */ /* This consists of a single input at some frequency, such as 10 to 20 kHz and * an output, such as 0 to 5 kHz. The output level is linearly dependent on the * frequency of the input sine wave frequency, i.e., it is an FM discriminator. * * NOTE: Due to the sample frequencies we are working with, like 44.1 kHz, this * discriminator cannot handle full FM broadcast bandwidths. It is suitable for * NBFM as used in communications, marine radio, ham radio, etc. * * The output can be FIR filtered using default parameters, * or using coefficients from an array. A separate single pole de-emphasis filer * is included that again can be programmed. * * Internally, the discriminator uses a pair of single pole BPF that * * Status: * * * Functions: * frequency(float fCenter ) sets the center frequency in Hz, default 15000. * * filterOut(float *firCoeffs, uint nFIR, float Kdem) sets output filtering where: * float32_t* firCoeffs is an array of coefficients * uint nFIR is the number of coefficients * float32_t Kdem is the de-emphasis frequency factor, where * Kdem = 1/(0.5+(tau*fsample)) and tau is the de-emphasis * time constant, typically 0.0005 second and fsample is * the sample frequency, typically 44117. * * 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). * * Time: For T4.0, 45 microseconds for a block of 128 data points. * */ #ifndef _radioFMDiscriminator_f32_h #define _radioFMDiscriminator_f32_h //#include "mathDSP_F32.h" #include "AudioStream_F32.h" //#include "arm_math.h" #define LPF_NONE 0 #define LPF_FIR 1 #define LPF_IIR 2 #define TEST_TIME_FM 0 class RadioFMDiscriminator_F32 : public AudioStream_F32 { //GUI: inputs:1, outputs:2 //this line used for automatic generation of GUI node //GUI: shortName: FMDiscriminator public: // Default block size and sample rate: RadioFMDiscriminator_F32(void) : AudioStream_F32(1, inputQueueArray_f32) { } // Option of AudioSettings_F32 change to block size and/or sample rate: RadioFMDiscriminator_F32(const AudioSettings_F32 &settings) : AudioStream_F32(1, inputQueueArray_f32) { sampleRate_Hz = settings.sample_rate_Hz; block_size = settings.audio_block_samples; } // This sets the parameters of the discriminator. The output LPF, if any, // must precede this function. void initializeFMDiscriminator(float32_t _f1, float32_t _f2, float32_t _q1, float32_t _q2) { f1 = _f1; f2 = _f2; q1 = _q1; q2 = _q2; // Design the 2 single pole filters: setBandpass(coeff_f1BPF, f1, q1); setBandpass(coeff_f2BPF, f2, q2); // Initialize BiQuad instances for BPF's (ARM DSP Math Library) // https://www.keil.com/pack/doc/CMSIS/DSP/html/group__BiquadCascadeDF1.html // arm_biquad_cascade_df1_init_f32(&biquad_inst, numStagesUsed, &coeff32[0], &StateF32[0]) arm_biquad_cascade_df1_init_f32(&f1BPF_inst, 1, &coeff_f1BPF[0], &state_f1BPF[0]); arm_biquad_cascade_df1_init_f32(&f2BPF_inst, 1, &coeff_f2BPF[0], &state_f2BPF[0]); /* The FIR instance setup call * void arm_fir_init_f32( * arm_fir_instance_f32* S, points to instance of floating-point FIR filter struct * 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. */ if (fir_Out_Coeffs && outputFilterType == LPF_FIR) { arm_fir_init_f32(&FMDet_Out_inst, nFIR_Out, &fir_Out_Coeffs[0], &State_FIR_Out[0], (uint32_t)block_size); } else { ; } // Initialize squelch Input BPF BiQuad instance arm_biquad_cascade_df1_init_f32(&iirSqIn_inst, 2, pCfSq, &stateSqIn[0]); } // Provide for changing to user FIR for discriminator output, (and user de-emphasis) // This should precede setting discriminator parameters void filterOutFIR(float32_t *_fir_Out_Coeffs, int _nFIR_Out, float32_t *_State_FIR_Out, float32_t _Kdem) { if(_fir_Out_Coeffs==NULL) { outputFilterType = LPF_NONE; return; } if( _Kdem<0.0001 || _Kdem>1.0 ) { return; } outputFilterType = LPF_FIR; fir_Out_Coeffs = _fir_Out_Coeffs; nFIR_Out = _nFIR_Out; State_FIR_Out = _State_FIR_Out; Kdem = _Kdem; OneMinusKdem = 1.0f - Kdem; } // This should precede setting discriminator parameters, if used void filterOutIIR(float32_t _frequency, float32_t _q, float32_t _Kdem) { if( _frequency < 0.0001f) { outputFilterType = LPF_NONE; return; } outputFilterType = LPF_IIR; setLowpass(coeff_outLPF, _frequency, _q); arm_biquad_cascade_df1_init_f32(&outLPF_inst, 1, &coeff_outLPF[0], &state_outLPF[0]); if( _Kdem<0.0001 || _Kdem>1.0 ) { return; } Kdem = _Kdem; OneMinusKdem = 1.0f - Kdem; } // Provide for changing to user supplied BiQuad for Squelch input. // This should precede setting discriminator parameters, if used void setSquelchFilter(float* _sqCoeffs) { if( _sqCoeffs==NULL) pCfSq = coeffSqIn; // Default filter else pCfSq = _sqCoeffs; } // 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); } // This should precede setting discriminator parameters, if used void setSampleRate_Hz(float32_t _sampleRate_Hz) { sampleRate_Hz = _sampleRate_Hz; } virtual void update(void); private: // One input data pointer audio_block_f32_t *inputQueueArray_f32[1]; float32_t sampleRate_Hz = AUDIO_SAMPLE_RATE_EXACT; uint16_t block_size = AUDIO_BLOCK_SAMPLES; /* A pair of single pole BPF for the discriminator: * 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. */ float f1, q1, f2, q2; arm_biquad_casd_df1_inst_f32 f1BPF_inst; float coeff_f1BPF[5]; float state_f1BPF[4]; arm_biquad_casd_df1_inst_f32 f2BPF_inst; float coeff_f2BPF[5]; float state_f2BPF[4]; // De-emphasis constant float32_t Kdem = 0.045334f; float32_t OneMinusKdem = 0.954666f; // Save last data point for next update of de-emphasis filter float32_t dLast = -1.0f; // The output FIR LPF (optional) int outputFilterType = LPF_NONE; // ARM CMSIS FIR filter instances and State vectors arm_fir_instance_f32 FMDet_Out_inst; float32_t *State_FIR_Out; // 128+nFIR_Out uint16_t nFIR_Out; float32_t* fir_Out_Coeffs = NULL; float32_t discrOut = 0.0f; // Output IIR Biquad alternative arm_biquad_casd_df1_inst_f32 outLPF_inst; float coeff_outLPF[5]; float state_outLPF[4]; 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 #endif #if 0 /* Info Only, an example FIR filter, include this in INO to use. * 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 */ 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.0001837353f, -0.0008401274f, -0.0008908477f }; #endif // Unity gain BPF Biquad, CMSIS format (not Matlab) void setBandpass(float32_t* pCoeff, float32_t frequency, float32_t q) { float32_t w0 = 2.0f*3.141592654f*frequency/sampleRate_Hz; float32_t alpha = sin(w0)/(2.0f*q); float32_t scale = 1.0f/(1.0f + alpha); /* b0 */ *(pCoeff+0) = alpha*scale; /* b1 */ *(pCoeff+1) = 0.0f; /* b2 */ *(pCoeff+2) = (-alpha)*scale; /* a1 */ *(pCoeff+3) = -(-2.0f*cos(w0))*scale; /* a2 */ *(pCoeff+4) = -(1.0f - alpha)*scale; } // Unity gain LPF, CMSIS format void setLowpass(float32_t* pCoeff, float32_t frequency, float32_t q) { float32_t w0 = frequency*(2.0f*3.141592654f / sampleRate_Hz); float32_t alpha = sin(w0) / ((double)q*2.0f); float32_t cosW0 = cos(w0); float32_t scale = 1.0f/(1.0f+alpha); // which is equal to 1.0f / a0 /* b0 */ *(pCoeff+0) = ((1.0f - cosW0) / 2.0f)*scale; /* b1 */ *(pCoeff+1) = (1.0f - cosW0)*scale; /* b2 */ *(pCoeff+2) = *(pCoeff+0); /* a1 */ *(pCoeff+3) = -(-2.0f*cosW0)*scale; /* a2 */ *(pCoeff+4) = -(1.0f - alpha)*scale; } }; #endif