You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
 
 
OpenAudio_ArduinoLibrary/RadioFMDiscriminator_F32.cpp

155 lines
5.6 KiB

/*
* RadioFMDetector_F32.cpp
*
* 25 April 2022
* Bob Larkin, in support of the library:
* Chip Audette, OpenAudio, Apr 2017
* -------------------
*
* 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.
*
* See RadioFMDiscriminator_F32.h for usage details
*/
#include "RadioFMDiscriminator_F32.h"
// ==== UPDATE ====
void RadioFMDiscriminator_F32::update(void) {
audio_block_f32_t *blockIn, *blockA=NULL, *blockB=NULL;
int i;
static float32_t discr0, disc1, disc2;
static float32_t sumNoise;
static float saveIn = 0.0f; // for squelch
static float saveOut = 0.0f;
#if TEST_TIME_FM
if (iitt++ >1000000) iitt = -10;
uint32_t t1, t2;
t1 = tElapse;
#endif
// Get input to FM Discriminator block
blockIn = AudioStream_F32::receiveWritable_f32(0);
if (!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 two blocks for both intermediate data and outputs
blockA = AudioStream_F32::allocate_f32();
if (!blockA){ // Didn't have any
//if(errorPrintFM) Serial.println("FMDET-ERR: No Output Memory");
AudioStream_F32::release(blockIn);
return;
}
blockB = AudioStream_F32::allocate_f32();
if (!blockB) // Didn't have any
{
//if(errorPrintFM) Serial.println("FMDET-ERR: No Output Memory");
AudioStream_F32::release(blockIn);
AudioStream_F32::release(blockA);
return;
}
// Limiter
for(int k=0; k<128; k++)
blockIn->data[k] = (blockIn->data[k]>0.0f) ? 1.0f : -1.0f;
// Two BPF at f1 and f2
arm_biquad_cascade_df1_f32(&f1BPF_inst, blockIn->data,
blockA->data, blockIn->length);
arm_biquad_cascade_df1_f32(&f2BPF_inst, blockIn->data,
blockB->data, blockIn->length);
if ( isnan(discrOut) ) discrOut=0.0f;
// Find difference in responses and average
for (i=0; i < block_size; i++)
{
// Find maximum absolute amplitudes (full-wave rectifiers)
disc1 = blockA->data[i];
disc1 = disc1>0.0 ? disc1 : -disc1;
disc2 = blockB->data[i];
disc2 = disc2>0.0 ? disc2 : -disc2;
discr0 = disc2 - disc1; // Sample-by-sample discriminator output
// Low pass filtering, RC type
discrOut = 0.999f*discr0 + 0.001f*discrOut; // REMOVE?????
// Data point is now discrOut. Apply single pole de-emphasis LPF, in place
// Set kDem = 1 to stop filtering
// dLast = Kdem * discrOut + OneMinusKdem * dLast; //<<<<<<<<FIX <<<<<<
// blockA->data[i] = dLast; // and save to an array
blockA->data[i] = discrOut;
//Serial.print(disc1, 6); Serial.print(" dLow dHigh "); Serial.println(disc2, 6);
//Serial.println(discrOut, 6);
}
// Do output FIR filter. Data now in blockA. Filter out goes to blockB.
if(outputFilterType == LPF_FIR)
arm_fir_f32(&FMDet_Out_inst, blockA->data, blockB->data, (uint32_t)blockIn->length);
else if(outputFilterType == LPF_IIR)
arm_biquad_cascade_df1_f32(&outLPF_inst, blockA->data, blockB->data, blockIn->length);
else
for(int k=0; k<blockIn->length; k++)
blockB->data[k] = blockA->data[k];
// The un-squelch controlled audio for tone decoding, etc., always goes to output 0
AudioStream_F32::transmit(blockB, 0);
// Squelch picks the audio from before the output filter and does a 4-pole BiQuad BPF
// blockA->data still has the data we need. Borrow blockIn for squelch filter out.
arm_biquad_cascade_df1_f32(&iirSqIn_inst, blockA->data, blockIn->data, blockIn->length);
// Update the Squelch full-wave envelope detector and single pole LPF
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(blockB, 1);
else
{
for(int kk=0; kk<128; kk++)
blockA->data[kk] = 0.0f;
AudioStream_F32::transmit(blockA, 1);
}
AudioStream_F32::release(blockA); // Give back the blocks
AudioStream_F32::release(blockB);
AudioStream_F32::release(blockIn);
#if TEST_TIME_FM
t2 = tElapse;
if(iitt++ < 0) {Serial.print("At end of FM Det "); Serial.println (t2 - t1); }
t1 = tElapse;
#endif
}