RadioFMDetector_F32.h

/*
 * RadioFMDetector_F32
 * 22 March 2020     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) 2020 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., 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.
 *
 * 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 detector uses a pair of mixers (multipliers) that generate the
 * in-phase and quadrature inputs to a atan2 type of phase detector.  These
 * mixers have two output signals at the difference (desired) and sum (undesired)
 * frequencies.  The high frequency sum signal can be filtered (For a 15 kHz center,
 * with an input band of 10 to 20 kHz the sum signal will be from 25 to 35 kHz that
 * wraps around the 22 kHz half-sample point to produce 19 to 9 kHz. This needs to
 * 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
 *            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.
 *
 * 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).
 *
 *   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, 430 microseconds, or
 *                  3.4 microseconds per data point.
 *                  For T4.0, 97 microseconds, or 0.76 microseconds per data point.
 *
 * Error checking:  See functions setSampleRate_Hz() and returnInitializeFMError()
 *                  above.
 */

#ifndef _radioFMDetector_f32_h
#define _radioFMDetector_f32_h

#include "mathDSP_F32.h"
#include "AudioStream_F32.h"
#include "arm_math.h"

#define MAX_FIR_IQ_COEFFS  100
#define MAX_FIR_OUT_COEFFS 120

#define TEST_TIME_FM 0

class RadioFMDetector_F32 : public AudioStream_F32 {
//GUI: inputs:1, outputs:2  //this line used for automatic generation of GUI node
//GUI: shortName: FMDetector
public:
    // Default block size and sample rate:
    RadioFMDetector_F32(void) :  AudioStream_F32(1, inputQueueArray_f32) {
       initializeFM();
    }
    // Option of AudioSettings_F32 change to block size and/or sample rate:
    RadioFMDetector_F32(const AudioSettings_F32 &settings) : AudioStream_F32(1, inputQueueArray_f32) {
        sampleRate_Hz = settings.sample_rate_Hz;
        block_size = settings.audio_block_samples;
        initializeFM();
    }

    // Provide for changing input center frequency, in Hz
    void frequency(float32_t _fCenter) {
        fCenter = _fCenter;
        phaseIncrement = 512.0f * fCenter / sampleRate_Hz;
    }

    // Provide for user FIR for I and Q signals to user supplied array
    void filterIQ(float32_t* _fir_IQ_Coeffs, int _nFIR_IQ) {
        if( fir_IQ_Coeffs==NULL || nFIR_IQ<4 || nFIR_IQ>MAX_FIR_IQ_COEFFS ) {
            initializeFMErrors |= 1;
            return;
        }
        fir_IQ_Coeffs = _fir_IQ_Coeffs;
        nFIR_IQ = _nFIR_IQ;
        initializeFM();
    }

    // Provide for changing to user FIR for detector output, (and user de-emphasis)
    void filterOut(float32_t *_fir_Out_Coeffs, int _nFIR_Out, float32_t _Kdem) {
        if( _fir_Out_Coeffs==NULL || _nFIR_Out<4 || _nFIR_Out>MAX_FIR_OUT_COEFFS) {
            initializeFMErrors |= 2;
            return;
        }
        if( _Kdem<0.0001 || _Kdem>1.0 ) {
            initializeFMErrors |= 4;
            return;
        }
        fir_Out_Coeffs = _fir_Out_Coeffs;
        nFIR_Out = _nFIR_Out;
        Kdem = _Kdem;
        OneMinusKdem = 1.0f - Kdem;
        initializeFM();
    }

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

    void update(void);

private:
    // One input data pointer
    audio_block_f32_t *inputQueueArray_f32[1];
    float32_t fCenter = 15000.0f;
    float32_t phaseS = 0.0f;
    float32_t phaseS_C = 128.00f;
    float32_t phaseIncrement = 512.0f*15000.0f/AUDIO_SAMPLE_RATE_EXACT;
    float32_t sampleRate_Hz = AUDIO_SAMPLE_RATE_EXACT;
    uint16_t block_size = AUDIO_BLOCK_SAMPLES;
    // De-emphasis constant
    float32_t   Kdem =         0.045334f;
    float32_t   OneMinusKdem = 0.954666f;
    // Save last data point of atan2 for differentiator
    float32_t diffLast = 0.0f;
    // Save last data point for next update of de-emphasis filter
    float32_t dLast = 0.0f;
    // Control error printing in update(), normally off
    uint16_t errorPrintFM = 0;
    // Monitor constructor errors
    uint16_t initializeFMErrors = 0;
    uint16_t nFIR_IQ = 29;
    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
#endif
    // ARM CMSIS FIR filter instances and State vectors, sized for max, max
    arm_fir_instance_f32 FMDet_I_inst;
    float32_t State_I_F32[AUDIO_BLOCK_SAMPLES + MAX_FIR_IQ_COEFFS];  // 228

    arm_fir_instance_f32 FMDet_Q_inst;
    float32_t State_Q_F32[AUDIO_BLOCK_SAMPLES + MAX_FIR_IQ_COEFFS];  // 248

    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) {
/*          the instance setup call
 *          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;

        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
     */
    float32_t fir_IQ29[29] = {
    -0.000970689f, -0.004690292f, -0.008256345f, -0.007565650f,
     0.001524420f,  0.015435011f,  0.021920240f,  0.008211937f,
    -0.024286413f, -0.052184700f, -0.040532507f,  0.031248107f,
     0.146902412f,  0.255179564f,  0.299445269f,  0.255179564f,
     0.146902412f,  0.031248107f, -0.040532507f, -0.052184700f,
    -0.024286413f,  0.008211937f,  0.021920240f,  0.015435011f,
     0.001524420f, -0.007565650f, -0.008256345f, -0.004690292f,
    -0.000970689f};

    /* 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