mirror
/
Wolf-LITE
kopia lustrzana https://github.com/UU5JPP/Wolf-LITE
1
0
Forkuj 0
Kod Zgłoszenia Projekty Wydania Wiki Aktywność
Wolf-LITE/STM32/Core/Src/audio_processor.c

891 wiersze
37 KiB
C
Czysty Wina Historia

#include "audio_processor.h"
#include "wm8731.h"
#include "audio_filters.h"
#include "agc.h"
#include "settings.h"
#include "usbd_audio_if.h"
#include "auto_notch.h"
#include "trx_manager.h"
#include "cw.h"
// Public variables
volatile uint32_t AUDIOPROC_samples = 0; // audio samples processed in the processor
IRAM1 int32_t Processor_AudioBuffer_A[AUDIO_BUFFER_SIZE] = {0}; // buffer A of the audio processor
IRAM1 int32_t Processor_AudioBuffer_B[AUDIO_BUFFER_SIZE] = {0}; // buffer B of the audio processor
volatile uint_fast8_t Processor_AudioBuffer_ReadyBuffer = 0; // which buffer is currently in use, A or B
volatile bool Processor_NeedRXBuffer = false; // codec needs data from processor for RX
volatile bool Processor_NeedTXBuffer = false; // codec needs data from processor for TX
IRAM1 float32_t FPGA_Audio_Buffer_RX_Q_tmp[FPGA_RX_IQ_BUFFER_HALF_SIZE] = {0}; // copy of the working part of the FPGA buffers for processing
IRAM1 float32_t FPGA_Audio_Buffer_RX_I_tmp[FPGA_RX_IQ_BUFFER_HALF_SIZE] = {0};
IRAM1 float32_t FPGA_Audio_Buffer_TX_Q_tmp[FPGA_TX_IQ_BUFFER_HALF_SIZE] = {0};
IRAM1 float32_t FPGA_Audio_Buffer_TX_I_tmp[FPGA_TX_IQ_BUFFER_HALF_SIZE] = {0};
volatile float32_t Processor_RX_Power_value; // RX signal magnitude
volatile float32_t Processor_selected_RFpower_amplitude = 0.0f; // target TX signal amplitude
volatile float32_t Processor_TX_MAX_amplitude_OUT; // TX uplift after ALC
// Private variables
static uint32_t two_signal_gen_position = 0; // signal position in a two-signal generator
static float32_t ALC_need_gain = 1.0f; // current gain of ALC and audio compressor
static float32_t ALC_need_gain_target = 1.0f; // Target Gain Of ALC And Audio Compressor
static float32_t DFM_RX_lpf_prev = 0, DFM_RX_hpf_prev_a = 0, DFM_RX_hpf_prev_b = 0; // used in FM detection and low / high pass processing
static float32_t DFM_RX_i_prev = 0, DFM_RX_q_prev = 0; // used in FM detection and low / high pass processing
static uint_fast8_t DFM_RX_fm_sql_count = 0; // used for squelch processing and debouncing tone detection, respectively
static float32_t DFM_RX_fm_sql_avg = 0.0f; // average SQL in FM
static bool DFM_RX_Squelched = false;
static float32_t current_if_gain = 0.0f;
static float32_t volume_gain = 0.0f;
// Prototypes
static void doRX_HILBERT(uint16_t size); // Hilbert filter for phase shift of signals
static void doRX_LPF_IQ(uint16_t size); // Low-pass filter for I and Q
static void doRX_LPF_I(uint16_t size); // LPF filter for I
static void doRX_GAUSS_I(uint16_t size); // Gauss filter for I
static void doRX_HPF_I(uint16_t size); // HPF filter for I
static void doRX_AGC(uint16_t size, uint_fast8_t mode); // automatic gain control
static void doRX_NOTCH(uint16_t size); // notch filter
static void doRX_SMETER(uint16_t size); // s-meter
static void doRX_COPYCHANNEL(uint16_t size); // copy I to Q channel
static void DemodulateFM(uint16_t size); // FM demodulator
static void ModulateFM(uint16_t size); // FM modulator
static void doRX_EQ(uint16_t size); // receiver equalizer
static void doMIC_EQ(uint16_t size); // microphone equalizer
static void doRX_IFGain(uint16_t size); //IF gain
// initialize audio processor
void initAudioProcessor(void)
{
InitAudioFilters();
}
// start audio processor for RX
void processRxAudio(void)
{
if (!Processor_NeedRXBuffer)
return;
VFO *current_vfo = CurrentVFO();
AUDIOPROC_samples++;
uint_fast16_t FPGA_Audio_Buffer_Index_tmp = FPGA_Audio_RXBuffer_Index;
if (FPGA_Audio_Buffer_Index_tmp == 0)
FPGA_Audio_Buffer_Index_tmp = FPGA_RX_IQ_BUFFER_SIZE;
else
FPGA_Audio_Buffer_Index_tmp--;
// copy buffer from FPGA
readFromCircleBuffer32((uint32_t *)&FPGA_Audio_Buffer_RX_Q[0], (uint32_t *)&FPGA_Audio_Buffer_RX_Q_tmp[0], FPGA_Audio_Buffer_Index_tmp, FPGA_RX_IQ_BUFFER_SIZE, FPGA_RX_IQ_BUFFER_HALF_SIZE);
readFromCircleBuffer32((uint32_t *)&FPGA_Audio_Buffer_RX_I[0], (uint32_t *)&FPGA_Audio_Buffer_RX_I_tmp[0], FPGA_Audio_Buffer_Index_tmp, FPGA_RX_IQ_BUFFER_SIZE, FPGA_RX_IQ_BUFFER_HALF_SIZE);
//Process DC corrector filter
if (current_vfo->Mode != TRX_MODE_AM && current_vfo->Mode != TRX_MODE_NFM && current_vfo->Mode != TRX_MODE_WFM)
{
dc_filter(FPGA_Audio_Buffer_RX_I_tmp, FPGA_RX_IQ_BUFFER_HALF_SIZE, DC_FILTER_RX_I);
dc_filter(FPGA_Audio_Buffer_RX_Q_tmp, FPGA_RX_IQ_BUFFER_HALF_SIZE, DC_FILTER_RX_Q);
}
//IQ Phase corrector https://github.com/df8oe/UHSDR/wiki/IQ---correction-and-mirror-frequencies
if(AUDIOPROC_samples == 1)
{
float32_t teta1_new = 0;
float32_t teta3_new = 0;
static float32_t teta1 = 0;
static float32_t teta3 = 0;
for (uint16_t i = 0; i < FPGA_RX_IQ_BUFFER_HALF_SIZE; i++)
{
teta1_new += FPGA_Audio_Buffer_RX_Q_tmp[i] * (FPGA_Audio_Buffer_RX_I_tmp[i] < 0.0f ? -1.0f : 1.0f);
teta3_new += FPGA_Audio_Buffer_RX_Q_tmp[i] * (FPGA_Audio_Buffer_RX_Q_tmp[i] < 0.0f ? -1.0f : 1.0f);
}
teta1_new = teta1_new / (float32_t)FPGA_RX_IQ_BUFFER_HALF_SIZE;
teta3_new = teta3_new / (float32_t)FPGA_RX_IQ_BUFFER_HALF_SIZE;
teta1 = 0.003f * teta1_new + 0.997f * teta1;
teta3 = 0.003f * teta3_new + 0.997f * teta3;
if (teta3 > 0.0f)
TRX_IQ_phase_error = asinf(teta1 / teta3);
}
if(current_vfo->Mode != TRX_MODE_IQ)
doRX_IFGain(FPGA_RX_IQ_BUFFER_HALF_SIZE);
switch (current_vfo->Mode) // first receiver
{
case TRX_MODE_LSB:
case TRX_MODE_CW_L:
doRX_HILBERT(FPGA_RX_IQ_BUFFER_HALF_SIZE);
arm_sub_f32(FPGA_Audio_Buffer_RX_I_tmp, FPGA_Audio_Buffer_RX_Q_tmp, FPGA_Audio_Buffer_RX_I_tmp, FPGA_RX_IQ_BUFFER_HALF_SIZE); // difference of I and Q - LSB
doRX_HPF_I(FPGA_RX_IQ_BUFFER_HALF_SIZE);
doRX_LPF_I(FPGA_RX_IQ_BUFFER_HALF_SIZE);
doRX_GAUSS_I(FPGA_RX_IQ_BUFFER_HALF_SIZE);
doRX_NOTCH(FPGA_RX_IQ_BUFFER_HALF_SIZE);
doRX_SMETER(FPGA_RX_IQ_BUFFER_HALF_SIZE);
doRX_AGC(FPGA_RX_IQ_BUFFER_HALF_SIZE, current_vfo->Mode);
doRX_COPYCHANNEL(FPGA_RX_IQ_BUFFER_HALF_SIZE);
break;
case TRX_MODE_DIGI_L:
doRX_HILBERT(FPGA_RX_IQ_BUFFER_HALF_SIZE);
arm_sub_f32(FPGA_Audio_Buffer_RX_I_tmp, FPGA_Audio_Buffer_RX_Q_tmp, FPGA_Audio_Buffer_RX_I_tmp, FPGA_RX_IQ_BUFFER_HALF_SIZE); // difference of I and Q - LSB
doRX_LPF_I(FPGA_RX_IQ_BUFFER_HALF_SIZE);
doRX_SMETER(FPGA_RX_IQ_BUFFER_HALF_SIZE);
doRX_AGC(FPGA_RX_IQ_BUFFER_HALF_SIZE, current_vfo->Mode);
doRX_COPYCHANNEL(FPGA_RX_IQ_BUFFER_HALF_SIZE);
break;
case TRX_MODE_USB:
case TRX_MODE_CW_U:
doRX_HILBERT(FPGA_RX_IQ_BUFFER_HALF_SIZE);
arm_add_f32(FPGA_Audio_Buffer_RX_I_tmp, FPGA_Audio_Buffer_RX_Q_tmp, FPGA_Audio_Buffer_RX_I_tmp, FPGA_RX_IQ_BUFFER_HALF_SIZE); // sum of I and Q - USB
doRX_HPF_I(FPGA_RX_IQ_BUFFER_HALF_SIZE);
doRX_LPF_I(FPGA_RX_IQ_BUFFER_HALF_SIZE);
doRX_GAUSS_I(FPGA_RX_IQ_BUFFER_HALF_SIZE);
doRX_NOTCH(FPGA_RX_IQ_BUFFER_HALF_SIZE);
doRX_SMETER(FPGA_RX_IQ_BUFFER_HALF_SIZE);
doRX_AGC(FPGA_RX_IQ_BUFFER_HALF_SIZE, current_vfo->Mode);
doRX_COPYCHANNEL(FPGA_RX_IQ_BUFFER_HALF_SIZE);
break;
case TRX_MODE_DIGI_U:
doRX_HILBERT(FPGA_RX_IQ_BUFFER_HALF_SIZE);
arm_add_f32(FPGA_Audio_Buffer_RX_I_tmp, FPGA_Audio_Buffer_RX_Q_tmp, FPGA_Audio_Buffer_RX_I_tmp, FPGA_RX_IQ_BUFFER_HALF_SIZE); // sum of I and Q - USB
doRX_LPF_I(FPGA_RX_IQ_BUFFER_HALF_SIZE);
doRX_SMETER(FPGA_RX_IQ_BUFFER_HALF_SIZE);
doRX_AGC(FPGA_RX_IQ_BUFFER_HALF_SIZE, current_vfo->Mode);
doRX_COPYCHANNEL(FPGA_RX_IQ_BUFFER_HALF_SIZE);
break;
case TRX_MODE_AM:
doRX_LPF_IQ(FPGA_RX_IQ_BUFFER_HALF_SIZE);
arm_mult_f32(FPGA_Audio_Buffer_RX_I_tmp, FPGA_Audio_Buffer_RX_I_tmp, FPGA_Audio_Buffer_RX_I_tmp, FPGA_RX_IQ_BUFFER_HALF_SIZE);
arm_mult_f32(FPGA_Audio_Buffer_RX_Q_tmp, FPGA_Audio_Buffer_RX_Q_tmp, FPGA_Audio_Buffer_RX_Q_tmp, FPGA_RX_IQ_BUFFER_HALF_SIZE);
arm_add_f32(FPGA_Audio_Buffer_RX_I_tmp, FPGA_Audio_Buffer_RX_Q_tmp, FPGA_Audio_Buffer_RX_I_tmp, FPGA_RX_IQ_BUFFER_HALF_SIZE);
//arm_vsqrt_f32(FPGA_Audio_Buffer_RX_I_tmp, FPGA_Audio_Buffer_RX_I_tmp, FPGA_AUDIO_BUFFER_HALF_SIZE);
for (uint_fast16_t i = 0; i < FPGA_RX_IQ_BUFFER_HALF_SIZE; i++)
arm_sqrt_f32(FPGA_Audio_Buffer_RX_I_tmp[i], &FPGA_Audio_Buffer_RX_I_tmp[i]);
arm_scale_f32(FPGA_Audio_Buffer_RX_I_tmp, 0.5f, FPGA_Audio_Buffer_RX_I_tmp, FPGA_RX_IQ_BUFFER_HALF_SIZE);
doRX_NOTCH(FPGA_RX_IQ_BUFFER_HALF_SIZE);
doRX_SMETER(FPGA_RX_IQ_BUFFER_HALF_SIZE);
doRX_AGC(FPGA_RX_IQ_BUFFER_HALF_SIZE, current_vfo->Mode);
doRX_COPYCHANNEL(FPGA_RX_IQ_BUFFER_HALF_SIZE);
break;
case TRX_MODE_NFM:
doRX_LPF_IQ(FPGA_RX_IQ_BUFFER_HALF_SIZE);
case TRX_MODE_WFM:
doRX_SMETER(FPGA_RX_IQ_BUFFER_HALF_SIZE);
DemodulateFM(FPGA_RX_IQ_BUFFER_HALF_SIZE);
//end decimate
doRX_AGC(FPGA_RX_IQ_BUFFER_HALF_SIZE, current_vfo->Mode);
doRX_COPYCHANNEL(FPGA_RX_IQ_BUFFER_HALF_SIZE);
break;
case TRX_MODE_IQ:
default:
doRX_SMETER(FPGA_RX_IQ_BUFFER_HALF_SIZE);
break;
}
//Prepare data to DMA
int32_t *Processor_AudioBuffer_current;
if (Processor_AudioBuffer_ReadyBuffer == 0)
Processor_AudioBuffer_current = &Processor_AudioBuffer_B[0];
else
Processor_AudioBuffer_current = &Processor_AudioBuffer_A[0];
// receiver equalizer
if (current_vfo->Mode != TRX_MODE_DIGI_L && current_vfo->Mode != TRX_MODE_DIGI_U && current_vfo->Mode != TRX_MODE_IQ)
doRX_EQ(FPGA_RX_IQ_BUFFER_HALF_SIZE);
// muting
if (TRX_Mute)
arm_scale_f32(FPGA_Audio_Buffer_RX_I_tmp, 0.0f, FPGA_Audio_Buffer_RX_I_tmp, FPGA_RX_IQ_BUFFER_HALF_SIZE);
// create buffers for transmission to the codec
for (uint_fast16_t i = 0; i < FPGA_RX_IQ_BUFFER_HALF_SIZE; i++)
{
arm_float_to_q31(&FPGA_Audio_Buffer_RX_I_tmp[i], &Processor_AudioBuffer_current[i * 2], 1); //left channel
if (current_vfo->Mode == TRX_MODE_IQ)
arm_float_to_q31(&FPGA_Audio_Buffer_RX_Q_tmp[i], &Processor_AudioBuffer_current[i * 2 + 1], 1); //right channel
else
Processor_AudioBuffer_current[i * 2 + 1] = Processor_AudioBuffer_current[i * 2]; //right channel
}
if (Processor_AudioBuffer_ReadyBuffer == 0)
Processor_AudioBuffer_ReadyBuffer = 1;
else
Processor_AudioBuffer_ReadyBuffer = 0;
//Send to USB Audio
if (USB_AUDIO_need_rx_buffer && TRX_Inited)
{
uint8_t *USB_AUDIO_rx_buffer_current;
if (!USB_AUDIO_current_rx_buffer)
USB_AUDIO_rx_buffer_current = &USB_AUDIO_rx_buffer_a[0];
else
USB_AUDIO_rx_buffer_current = &USB_AUDIO_rx_buffer_b[0];
//drop LSB 32b->16b
for (uint_fast16_t i = 0; i < (USB_AUDIO_RX_BUFFER_SIZE / BYTES_IN_SAMPLE_AUDIO_OUT_PACKET); i++)
{
USB_AUDIO_rx_buffer_current[i * BYTES_IN_SAMPLE_AUDIO_OUT_PACKET + 0] = (Processor_AudioBuffer_current[i] >> 16) & 0xFF;
USB_AUDIO_rx_buffer_current[i * BYTES_IN_SAMPLE_AUDIO_OUT_PACKET + 1] = (Processor_AudioBuffer_current[i] >> 24) & 0xFF;
//USB_AUDIO_rx_buffer_current[i * BYTES_IN_SAMPLE_AUDIO_OUT_PACKET + 2] = (Processor_AudioBuffer_current[i] >> 24) & 0xFF;
}
USB_AUDIO_need_rx_buffer = false;
}
//OUT Volume
float32_t volume_gain_new = volume2rate((float32_t)TRX.Volume / 100.0f);
volume_gain = 0.9f * volume_gain + 0.1f * volume_gain_new;
for (uint_fast16_t i = 0; i < AUDIO_BUFFER_SIZE; i++)
{
Processor_AudioBuffer_current[i] = (int32_t)((float32_t)Processor_AudioBuffer_current[i] * volume_gain);
Processor_AudioBuffer_current[i] = convertToSPIBigEndian(Processor_AudioBuffer_current[i]); //for 32bit audio
}
//Beep signal
if(WM8731_Beeping)
{
float32_t signal = 0;
int32_t out = 0;
bool halted = false;
float32_t amplitude = volume2rate((float32_t)TRX.Volume / 100.0f) * 0.1f;
for(uint32_t pos = 0; pos < AUDIO_BUFFER_HALF_SIZE; pos++)
{
signal = generateSin(amplitude, pos, TRX_SAMPLERATE, 1500);
arm_float_to_q31(&signal, &out, 1);
Processor_AudioBuffer_current[pos * 2] = convertToSPIBigEndian(out); //left channel
Processor_AudioBuffer_current[pos * 2 + 1] = Processor_AudioBuffer_current[pos * 2]; //right channel
}
}
//Mute codec
if(WM8731_Muting)
{
for(uint32_t pos = 0; pos < AUDIO_BUFFER_HALF_SIZE; pos++)
{
Processor_AudioBuffer_current[pos * 2] = 0; //left channel
Processor_AudioBuffer_current[pos * 2 + 1] = 0; //right channel
}
}
//Send to Codec DMA
if (TRX_Inited)
{
if (WM8731_DMA_state) //complete
{
HAL_DMA_Start_IT(&hdma_memtomem_dma2_stream1, (uint32_t)&Processor_AudioBuffer_current[0], (uint32_t)&CODEC_Audio_Buffer_RX[AUDIO_BUFFER_SIZE], CODEC_AUDIO_BUFFER_HALF_SIZE); //*2 -> left_right
//HAL_DMA_PollForTransfer(&hdma_memtomem_dma2_stream1, HAL_DMA_FULL_TRANSFER, HAL_MAX_DELAY);
}
else //half
{
HAL_DMA_Start_IT(&hdma_memtomem_dma2_stream2, (uint32_t)&Processor_AudioBuffer_current[0], (uint32_t)&CODEC_Audio_Buffer_RX[0], CODEC_AUDIO_BUFFER_HALF_SIZE); //*2 -> left_right
//HAL_DMA_PollForTransfer(&hdma_memtomem_dma2_stream2, HAL_DMA_FULL_TRANSFER, HAL_MAX_DELAY);
}
}
Processor_NeedRXBuffer = false;
}
// start audio processor for TX
void processTxAudio(void)
{
if (!Processor_NeedTXBuffer)
return;
//sync fpga to audio-codec
/*uint32_t dma_index = CODEC_AUDIO_BUFFER_SIZE * 2 - (uint16_t)__HAL_DMA_GET_COUNTER(hi2s3.hdmatx);
if(!WM8731_DMA_state && dma_index > (CODEC_AUDIO_BUFFER_SIZE * 2 - 100))
return;
if(WM8731_DMA_state && dma_index > (CODEC_AUDIO_BUFFER_SIZE - 100))
return;*/
static bool old_WM8731_DMA_state = false;
if(WM8731_DMA_state == old_WM8731_DMA_state)
return;
old_WM8731_DMA_state = WM8731_DMA_state;
bool start_WM8731_DMA_state = old_WM8731_DMA_state;
VFO *current_vfo = CurrentVFO();
AUDIOPROC_samples++;
uint_fast8_t mode = current_vfo->Mode;
// get the amplitude for the selected power and range
Processor_selected_RFpower_amplitude = log10f_fast(((float32_t)TRX.RF_Power * 0.9f + 10.0f) / 10.0f) * TRX_MAX_TX_Amplitude;
if (Processor_selected_RFpower_amplitude < 0.0f)
Processor_selected_RFpower_amplitude = 0.0f;
if (mode == TRX_MODE_LOOPBACK && !TRX_Tune)
Processor_selected_RFpower_amplitude = 0.5f;
// zero beats
if ((TRX_Tune && !TRX.TWO_SIGNAL_TUNE) || (TRX_Tune && (mode == TRX_MODE_CW_L || mode == TRX_MODE_CW_U)))
Processor_selected_RFpower_amplitude = Processor_selected_RFpower_amplitude * 1.0f;
if (TRX.InputType_USB) //USB AUDIO
{
uint32_t buffer_index = USB_AUDIO_GetTXBufferIndex_FS() / BYTES_IN_SAMPLE_AUDIO_OUT_PACKET; //buffer 8bit, data 16 bit
if ((buffer_index % BYTES_IN_SAMPLE_AUDIO_OUT_PACKET) == 1)
buffer_index -= (buffer_index % BYTES_IN_SAMPLE_AUDIO_OUT_PACKET);
readHalfFromCircleUSBBuffer16Bit(&USB_AUDIO_tx_buffer[0], &Processor_AudioBuffer_A[0], buffer_index, (USB_AUDIO_TX_BUFFER_SIZE / BYTES_IN_SAMPLE_AUDIO_OUT_PACKET));
}
else //AUDIO CODEC AUDIO
{
uint32_t dma_index = CODEC_AUDIO_BUFFER_SIZE - (uint16_t)__HAL_DMA_GET_COUNTER(hi2s3.hdmarx) / 2;
//uint32_t dma_index = (uint16_t)__HAL_DMA_GET_COUNTER(hi2s3.hdmarx);
if ((dma_index % 2) == 1)
dma_index--;
//sendToDebug_uint32(CODEC_AUDIO_BUFFER_SIZE, false);
readFromCircleBuffer32((uint32_t *)&CODEC_Audio_Buffer_TX[0], (uint32_t *)&Processor_AudioBuffer_A[0], dma_index, CODEC_AUDIO_BUFFER_SIZE, AUDIO_BUFFER_SIZE);
}
//One-signal zero-tune generator
if (TRX_Tune && !TRX.TWO_SIGNAL_TUNE)
{
for (uint_fast16_t i = 0; i < AUDIO_BUFFER_HALF_SIZE; i++)
{
FPGA_Audio_Buffer_TX_I_tmp[i] = (Processor_selected_RFpower_amplitude / 100.0f * TUNE_POWER);
FPGA_Audio_Buffer_TX_Q_tmp[i] = 0.0f;
}
}
//Two-signal tune generator
if (TRX_Tune && TRX.TWO_SIGNAL_TUNE)
{
for (uint_fast16_t i = 0; i < AUDIO_BUFFER_HALF_SIZE; i++)
{
float32_t point = generateSin((Processor_selected_RFpower_amplitude / 100.0f * TUNE_POWER) / 2.0f, two_signal_gen_position, TRX_SAMPLERATE, 1000);
point += generateSin((Processor_selected_RFpower_amplitude / 100.0f * TUNE_POWER) / 2.0f, two_signal_gen_position, TRX_SAMPLERATE, 2000);
two_signal_gen_position++;
if (two_signal_gen_position >= TRX_SAMPLERATE)
two_signal_gen_position = 0;
FPGA_Audio_Buffer_TX_I_tmp[i] = point;
FPGA_Audio_Buffer_TX_Q_tmp[i] = point;
}
//hilbert fir
// + 45 deg to Q data
arm_fir_f32(&FIR_TX_Hilbert_Q, FPGA_Audio_Buffer_TX_I_tmp, FPGA_Audio_Buffer_TX_I_tmp, AUDIO_BUFFER_HALF_SIZE);
// - 45 deg to I data
arm_fir_f32(&FIR_TX_Hilbert_I, FPGA_Audio_Buffer_TX_Q_tmp, FPGA_Audio_Buffer_TX_Q_tmp, AUDIO_BUFFER_HALF_SIZE);
}
//FM tone generator
if (TRX_Tune && (mode == TRX_MODE_NFM || mode == TRX_MODE_WFM))
{
static uint32_t tone_counter = 100;
tone_counter++;
if(tone_counter >= 400)
tone_counter = 0;
for (uint_fast16_t i = 0; i < AUDIO_BUFFER_HALF_SIZE; i++)
{
float32_t point = 0.0f;
if(tone_counter > 300)
point = generateSin(1.0f, two_signal_gen_position, TRX_SAMPLERATE, 3500);
else if(tone_counter > 200)
point = generateSin(1.0f, two_signal_gen_position, TRX_SAMPLERATE, 2000);
else if(tone_counter > 100)
point = generateSin(1.0f, two_signal_gen_position, TRX_SAMPLERATE, 1000);
two_signal_gen_position++;
if (two_signal_gen_position >= TRX_SAMPLERATE)
two_signal_gen_position = 0;
FPGA_Audio_Buffer_TX_I_tmp[i] = point;
FPGA_Audio_Buffer_TX_Q_tmp[i] = point;
}
ModulateFM(AUDIO_BUFFER_HALF_SIZE);
}
if (!TRX_Tune)
{
//Copy and convert buffer
for (uint_fast16_t i = 0; i < AUDIO_BUFFER_HALF_SIZE; i++)
{
//FPGA_Audio_Buffer_TX_I_tmp[i] = (float32_t)Processor_AudioBuffer_A[i * 2] / 2147483648.0f;
//FPGA_Audio_Buffer_TX_Q_tmp[i] = (float32_t)Processor_AudioBuffer_A[i * 2 + 1] / 2147483648.0f;
FPGA_Audio_Buffer_TX_I_tmp[i] = (float32_t)convertToSPIBigEndian(Processor_AudioBuffer_A[i * 2]) / 2147483648.0f;
FPGA_Audio_Buffer_TX_Q_tmp[i] = (float32_t)convertToSPIBigEndian(Processor_AudioBuffer_A[i * 2 + 1]) / 2147483648.0f;
}
//sendToDebug_float32(FPGA_Audio_Buffer_TX_I_tmp[0],false);
if (TRX.InputType_MIC)
{
//Mic Gain
arm_scale_f32(FPGA_Audio_Buffer_TX_I_tmp, TRX.MIC_GAIN, FPGA_Audio_Buffer_TX_I_tmp, AUDIO_BUFFER_HALF_SIZE);
arm_scale_f32(FPGA_Audio_Buffer_TX_Q_tmp, TRX.MIC_GAIN, FPGA_Audio_Buffer_TX_Q_tmp, AUDIO_BUFFER_HALF_SIZE);
//Mic Equalizer
if (mode != TRX_MODE_DIGI_L && mode != TRX_MODE_DIGI_U && mode != TRX_MODE_IQ)
doMIC_EQ(AUDIO_BUFFER_HALF_SIZE);
}
//Process DC corrector filter
dc_filter(FPGA_Audio_Buffer_TX_I_tmp, AUDIO_BUFFER_HALF_SIZE, DC_FILTER_TX_I);
dc_filter(FPGA_Audio_Buffer_TX_Q_tmp, AUDIO_BUFFER_HALF_SIZE, DC_FILTER_TX_Q);
}
if (mode != TRX_MODE_IQ && !TRX_Tune)
{
//IIR HPF
if (current_vfo->HPF_Filter_Width > 0)
arm_biquad_cascade_df2T_f32(&IIR_TX_HPF_I, FPGA_Audio_Buffer_TX_I_tmp, FPGA_Audio_Buffer_TX_I_tmp, AUDIO_BUFFER_HALF_SIZE);
//IIR LPF
if (current_vfo->TX_LPF_Filter_Width > 0)
arm_biquad_cascade_df2T_f32(&IIR_TX_LPF_I, FPGA_Audio_Buffer_TX_I_tmp, FPGA_Audio_Buffer_TX_I_tmp, AUDIO_BUFFER_HALF_SIZE);
memcpy(&FPGA_Audio_Buffer_TX_Q_tmp[0], &FPGA_Audio_Buffer_TX_I_tmp[0], AUDIO_BUFFER_HALF_SIZE * 4); //double left and right channel
switch (mode)
{
case TRX_MODE_CW_L:
case TRX_MODE_CW_U:
for (uint_fast16_t i = 0; i < AUDIO_BUFFER_HALF_SIZE; i++)
{
FPGA_Audio_Buffer_TX_I_tmp[i] = CW_GenerateSignal(Processor_selected_RFpower_amplitude);
FPGA_Audio_Buffer_TX_Q_tmp[i] = 0.0f;
}
break;
case TRX_MODE_USB:
case TRX_MODE_DIGI_U:
//hilbert fir
// + 45 deg to Q data
arm_fir_f32(&FIR_TX_Hilbert_Q, FPGA_Audio_Buffer_TX_I_tmp, FPGA_Audio_Buffer_TX_I_tmp, AUDIO_BUFFER_HALF_SIZE);
// - 45 deg to I data
arm_fir_f32(&FIR_TX_Hilbert_I, FPGA_Audio_Buffer_TX_Q_tmp, FPGA_Audio_Buffer_TX_Q_tmp, AUDIO_BUFFER_HALF_SIZE);
break;
case TRX_MODE_LSB:
case TRX_MODE_DIGI_L:
//hilbert fir
// + 45 deg to I data
arm_fir_f32(&FIR_TX_Hilbert_I, FPGA_Audio_Buffer_TX_I_tmp, FPGA_Audio_Buffer_TX_I_tmp, AUDIO_BUFFER_HALF_SIZE);
// - 45 deg to Q data
arm_fir_f32(&FIR_TX_Hilbert_Q, FPGA_Audio_Buffer_TX_Q_tmp, FPGA_Audio_Buffer_TX_Q_tmp, AUDIO_BUFFER_HALF_SIZE);
break;
case TRX_MODE_AM:
// + 45 deg to I data
arm_fir_f32(&FIR_TX_Hilbert_I, FPGA_Audio_Buffer_TX_I_tmp, FPGA_Audio_Buffer_TX_I_tmp, AUDIO_BUFFER_HALF_SIZE);
// - 45 deg to Q data
arm_fir_f32(&FIR_TX_Hilbert_Q, FPGA_Audio_Buffer_TX_Q_tmp, FPGA_Audio_Buffer_TX_Q_tmp, AUDIO_BUFFER_HALF_SIZE);
for (size_t i = 0; i < AUDIO_BUFFER_HALF_SIZE; i++)
{
float32_t i_am = ((FPGA_Audio_Buffer_TX_I_tmp[i] - FPGA_Audio_Buffer_TX_Q_tmp[i]) + (Processor_selected_RFpower_amplitude)) / 2.0f;
float32_t q_am = ((FPGA_Audio_Buffer_TX_Q_tmp[i] - FPGA_Audio_Buffer_TX_I_tmp[i]) - (Processor_selected_RFpower_amplitude)) / 2.0f;
FPGA_Audio_Buffer_TX_I_tmp[i] = i_am;
FPGA_Audio_Buffer_TX_Q_tmp[i] = q_am;
}
break;
case TRX_MODE_NFM:
case TRX_MODE_WFM:
ModulateFM(AUDIO_BUFFER_HALF_SIZE);
break;
case TRX_MODE_LOOPBACK:
break;
default:
break;
}
}
// RF PowerControl (Audio Level Control) Compressor
// looking for a maximum in amplitude
float32_t ampl_max_i = 0.0f;
float32_t ampl_max_q = 0.0f;
float32_t ampl_min_i = 0.0f;
float32_t ampl_min_q = 0.0f;
uint32_t tmp_index;
arm_max_f32(FPGA_Audio_Buffer_TX_I_tmp, AUDIO_BUFFER_HALF_SIZE, &ampl_max_i, &tmp_index);
arm_max_f32(FPGA_Audio_Buffer_TX_Q_tmp, AUDIO_BUFFER_HALF_SIZE, &ampl_max_q, &tmp_index);
arm_min_f32(FPGA_Audio_Buffer_TX_I_tmp, AUDIO_BUFFER_HALF_SIZE, &ampl_min_i, &tmp_index);
arm_min_f32(FPGA_Audio_Buffer_TX_Q_tmp, AUDIO_BUFFER_HALF_SIZE, &ampl_min_q, &tmp_index);
float32_t Processor_TX_MAX_amplitude_IN = ampl_max_i;
if (ampl_max_q > Processor_TX_MAX_amplitude_IN)
Processor_TX_MAX_amplitude_IN = ampl_max_q;
if ((-ampl_min_i) > Processor_TX_MAX_amplitude_IN)
Processor_TX_MAX_amplitude_IN = -ampl_min_i;
if ((-ampl_min_q) > Processor_TX_MAX_amplitude_IN)
Processor_TX_MAX_amplitude_IN = -ampl_min_q;
// calculate the target gain
if (Processor_TX_MAX_amplitude_IN > 0.0f)
{
ALC_need_gain_target = (Processor_selected_RFpower_amplitude * 0.99f) / Processor_TX_MAX_amplitude_IN;
// move the gain one step
if (fabsf(ALC_need_gain_target - ALC_need_gain) > 0.00001f) // hysteresis
{
if (ALC_need_gain_target > ALC_need_gain)
{
if (mode == TRX_MODE_DIGI_L || mode == TRX_MODE_DIGI_U) // FAST AGC
ALC_need_gain = (ALC_need_gain * (1.0f - (float32_t)TRX.TX_AGC_speed / 30.0f)) + (ALC_need_gain_target * ((float32_t)TRX.TX_AGC_speed / 30.0f));
else // SLOW AGC
ALC_need_gain = (ALC_need_gain * (1.0f - (float32_t)TRX.TX_AGC_speed / 1000.0f)) + (ALC_need_gain_target * ((float32_t)TRX.TX_AGC_speed / 1000.0f));
}
}
//just in case
if (ALC_need_gain < 0.0f)
ALC_need_gain = 0.0f;
// overload (clipping), sharply reduce the gain
if ((ALC_need_gain * Processor_TX_MAX_amplitude_IN) > (Processor_selected_RFpower_amplitude * 1.0f))
{
ALC_need_gain = ALC_need_gain_target;
// sendToDebug_str ("MIC_CLIP");
}
if (ALC_need_gain > TX_AGC_MAXGAIN)
ALC_need_gain = TX_AGC_MAXGAIN;
// noise threshold
if (Processor_TX_MAX_amplitude_IN < TX_AGC_NOISEGATE) // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> TX_AGC_NOISEGATE == 0.00001f
ALC_need_gain = 0.0f;
}
// disable gain for some types of mod
if ((ALC_need_gain > 1.0f) && (mode == TRX_MODE_LOOPBACK))
ALC_need_gain = 1.0f;
if (mode == TRX_MODE_CW_L || mode == TRX_MODE_CW_U || mode == TRX_MODE_NFM || mode == TRX_MODE_WFM)
ALC_need_gain = 1.0f;
if (TRX_Tune)
ALC_need_gain = 1.0f;
// apply gain
arm_scale_f32(FPGA_Audio_Buffer_TX_I_tmp, ALC_need_gain, FPGA_Audio_Buffer_TX_I_tmp, AUDIO_BUFFER_HALF_SIZE);
arm_scale_f32(FPGA_Audio_Buffer_TX_Q_tmp, ALC_need_gain, FPGA_Audio_Buffer_TX_Q_tmp, AUDIO_BUFFER_HALF_SIZE);
Processor_TX_MAX_amplitude_OUT = Processor_TX_MAX_amplitude_IN * ALC_need_gain;
if (Processor_selected_RFpower_amplitude > 0.0f)
TRX_ALC = Processor_TX_MAX_amplitude_OUT / Processor_selected_RFpower_amplitude;
else
TRX_ALC = 0.0f;
//RF PowerControl (Audio Level Control) Compressor END
//Send TX data to FFT
float32_t* FFTInput_I_current = FFT_buff_current ? (float32_t*)&FFTInput_I_B : (float32_t*)&FFTInput_I_A;
float32_t* FFTInput_Q_current = FFT_buff_current ? (float32_t*)&FFTInput_Q_B : (float32_t*)&FFTInput_Q_A;
for (uint_fast16_t i = 0; i < AUDIO_BUFFER_HALF_SIZE; i++)
{
FFTInput_I_current[FFT_buff_index] = FPGA_Audio_Buffer_TX_I_tmp[i];
FFTInput_Q_current[FFT_buff_index] = FPGA_Audio_Buffer_TX_Q_tmp[i];
FFT_buff_index++;
if (FFT_buff_index >= FFT_SIZE)
{
FFT_buff_index = 0;
FFT_new_buffer_ready = true;
FFT_buff_current = !FFT_buff_current;
}
}
//Loopback mode
if (mode == TRX_MODE_LOOPBACK && !TRX_Tune)
{
//OUT Volume
float32_t volume_gain_tx = volume2rate((float32_t)TRX.Volume / 100.0f);
arm_scale_f32(FPGA_Audio_Buffer_TX_I_tmp, volume_gain_tx, FPGA_Audio_Buffer_TX_I_tmp, AUDIO_BUFFER_HALF_SIZE);
for (uint_fast16_t i = 0; i < AUDIO_BUFFER_HALF_SIZE; i++)
{
arm_float_to_q31(&FPGA_Audio_Buffer_TX_I_tmp[i], &Processor_AudioBuffer_A[i * 2], 1);
Processor_AudioBuffer_A[i * 2] = convertToSPIBigEndian(Processor_AudioBuffer_A[i * 2]); //left channel
Processor_AudioBuffer_A[i * 2 + 1] = Processor_AudioBuffer_A[i * 2]; //right channel
}
if (WM8731_DMA_state) //compleate
{
HAL_DMA_Start(&hdma_memtomem_dma2_stream1, (uint32_t)&Processor_AudioBuffer_A[0], (uint32_t)&CODEC_Audio_Buffer_RX[AUDIO_BUFFER_SIZE], AUDIO_BUFFER_SIZE);
HAL_DMA_PollForTransfer(&hdma_memtomem_dma2_stream1, HAL_DMA_FULL_TRANSFER, HAL_MAX_DELAY);
}
else //half
{
HAL_DMA_Start(&hdma_memtomem_dma2_stream2, (uint32_t)&Processor_AudioBuffer_A[0], (uint32_t)&CODEC_Audio_Buffer_RX[0], AUDIO_BUFFER_SIZE);
HAL_DMA_PollForTransfer(&hdma_memtomem_dma2_stream2, HAL_DMA_FULL_TRANSFER, HAL_MAX_DELAY);
}
}
else
{
//CW SelfHear
if (TRX.CW_SelfHear && (TRX.CW_KEYER || CW_key_serial || CW_key_dot_hard || CW_key_dash_hard) && (mode == TRX_MODE_CW_L || mode == TRX_MODE_CW_U) && !TRX_Tune)
{
static float32_t cwgen_index = 0;
const float32_t SELFHEAR_Volume = 100.0f;
float32_t amplitude = volume2rate((float32_t)TRX.Volume / 100.0f) * volume2rate(SELFHEAR_Volume / 100.0f);
for (uint_fast16_t i = 0; i < AUDIO_BUFFER_HALF_SIZE; i++)
{
const float32_t CW_Pitch_freq = 700;
float32_t point = generateSinF(amplitude * FPGA_Audio_Buffer_TX_I_tmp[i], &cwgen_index, TRX_SAMPLERATE, CW_Pitch_freq);
int32_t sample = 0;
arm_float_to_q31(&point, &sample, 1);
int32_t data = convertToSPIBigEndian(sample);
if (start_WM8731_DMA_state)
{
CODEC_Audio_Buffer_RX[AUDIO_BUFFER_SIZE + i * 2] = data;
CODEC_Audio_Buffer_RX[AUDIO_BUFFER_SIZE + i * 2 + 1] = data;
}
else
{
CODEC_Audio_Buffer_RX[i * 2] = data;
CODEC_Audio_Buffer_RX[i * 2 + 1] = data;
}
}
}
else if (TRX.CW_SelfHear && mode != TRX_MODE_DIGI_L && mode != TRX_MODE_DIGI_U && mode != TRX_MODE_LOOPBACK)
{
memset(CODEC_Audio_Buffer_RX, 0x00, sizeof CODEC_Audio_Buffer_RX);
}
//
if (FPGA_Audio_Buffer_State) //Send to FPGA DMA
{
//sendToDebug_float32(FPGA_Audio_SendBuffer_I[0],false);
HAL_DMA_Start(&hdma_memtomem_dma2_stream1, (uint32_t)&FPGA_Audio_Buffer_TX_I_tmp[0], (uint32_t)&FPGA_Audio_SendBuffer_I[AUDIO_BUFFER_HALF_SIZE], AUDIO_BUFFER_HALF_SIZE);
HAL_DMA_PollForTransfer(&hdma_memtomem_dma2_stream1, HAL_DMA_FULL_TRANSFER, HAL_MAX_DELAY);
HAL_DMA_Start(&hdma_memtomem_dma2_stream1, (uint32_t)&FPGA_Audio_Buffer_TX_Q_tmp[0], (uint32_t)&FPGA_Audio_SendBuffer_Q[AUDIO_BUFFER_HALF_SIZE], AUDIO_BUFFER_HALF_SIZE);
HAL_DMA_PollForTransfer(&hdma_memtomem_dma2_stream1, HAL_DMA_FULL_TRANSFER, HAL_MAX_DELAY);
}
else
{
HAL_DMA_Start(&hdma_memtomem_dma2_stream2, (uint32_t)&FPGA_Audio_Buffer_TX_I_tmp[0], (uint32_t)&FPGA_Audio_SendBuffer_I[0], AUDIO_BUFFER_HALF_SIZE);
HAL_DMA_PollForTransfer(&hdma_memtomem_dma2_stream2, HAL_DMA_FULL_TRANSFER, HAL_MAX_DELAY);
HAL_DMA_Start(&hdma_memtomem_dma2_stream2, (uint32_t)&FPGA_Audio_Buffer_TX_Q_tmp[0], (uint32_t)&FPGA_Audio_SendBuffer_Q[0], AUDIO_BUFFER_HALF_SIZE);
HAL_DMA_PollForTransfer(&hdma_memtomem_dma2_stream2, HAL_DMA_FULL_TRANSFER, HAL_MAX_DELAY);
}
}
Processor_NeedTXBuffer = false;
Processor_NeedRXBuffer = false;
USB_AUDIO_need_rx_buffer = false;
}
// Hilbert filter for phase shift of signals
static void doRX_HILBERT(uint16_t size)
{
arm_fir_f32(&FIR_RX_Hilbert_I, FPGA_Audio_Buffer_RX_I_tmp, FPGA_Audio_Buffer_RX_I_tmp, size);
arm_fir_f32(&FIR_RX_Hilbert_Q, FPGA_Audio_Buffer_RX_Q_tmp, FPGA_Audio_Buffer_RX_Q_tmp, size);
}
// Low-pass filter for I and Q
static void doRX_LPF_IQ(uint16_t size)
{
if (CurrentVFO()->RX_LPF_Filter_Width > 0)
{
arm_biquad_cascade_df2T_f32(&IIR_RX_LPF_I, FPGA_Audio_Buffer_RX_I_tmp, FPGA_Audio_Buffer_RX_I_tmp, size);
arm_biquad_cascade_df2T_f32(&IIR_RX_LPF_Q, FPGA_Audio_Buffer_RX_Q_tmp, FPGA_Audio_Buffer_RX_Q_tmp, size);
}
}
// LPF filter for I
static void doRX_LPF_I(uint16_t size)
{
if (CurrentVFO()->RX_LPF_Filter_Width > 0)
{
arm_biquad_cascade_df2T_f32(&IIR_RX_LPF_I, FPGA_Audio_Buffer_RX_I_tmp, FPGA_Audio_Buffer_RX_I_tmp, size);
}
}
// Gauss filter for I
static void doRX_GAUSS_I(uint16_t size)
{
if (!TRX.CW_GaussFilter)
return;
if (CurrentVFO()->Mode == TRX_MODE_CW_L || CurrentVFO()->Mode == TRX_MODE_CW_U)
{
arm_scale_f32(FPGA_Audio_Buffer_RX_I_tmp,8.0,FPGA_Audio_Buffer_RX_I_tmp,size);
arm_biquad_cascade_df2T_f32(&IIR_RX_GAUSS, FPGA_Audio_Buffer_RX_I_tmp, FPGA_Audio_Buffer_RX_I_tmp, size);
}
}
// HPF filter for I
static void doRX_HPF_I(uint16_t size)
{
if (CurrentVFO()->HPF_Filter_Width > 0)
{
arm_biquad_cascade_df2T_f32(&IIR_RX_HPF_I, FPGA_Audio_Buffer_RX_I_tmp, FPGA_Audio_Buffer_RX_I_tmp, size);
}
}
// notch filter
static void doRX_NOTCH(uint16_t size)
{
if (CurrentVFO()->AutoNotchFilter) // automatic filter
{
for (uint32_t block = 0; block < (size / AUTO_NOTCH_BLOCK_SIZE); block++)
processAutoNotchReduction(FPGA_Audio_Buffer_RX_I_tmp + (block * AUTO_NOTCH_BLOCK_SIZE));
}
}
// RX Equalizer
static void doRX_EQ(uint16_t size)
{
if (TRX.RX_EQ_LOW != 0)
arm_biquad_cascade_df2T_f32(&EQ_RX_LOW_FILTER, FPGA_Audio_Buffer_RX_I_tmp, FPGA_Audio_Buffer_RX_I_tmp, size);
if (TRX.RX_EQ_MID != 0)
arm_biquad_cascade_df2T_f32(&EQ_RX_MID_FILTER, FPGA_Audio_Buffer_RX_I_tmp, FPGA_Audio_Buffer_RX_I_tmp, size);
if (TRX.RX_EQ_HIG != 0)
arm_biquad_cascade_df2T_f32(&EQ_RX_HIG_FILTER, FPGA_Audio_Buffer_RX_I_tmp, FPGA_Audio_Buffer_RX_I_tmp, size);
}
// Equalizer microphone
static void doMIC_EQ(uint16_t size)
{
if (TRX.MIC_EQ_LOW != 0)
arm_biquad_cascade_df2T_f32(&EQ_MIC_LOW_FILTER, FPGA_Audio_Buffer_TX_I_tmp, FPGA_Audio_Buffer_TX_I_tmp, size);
if (TRX.MIC_EQ_MID != 0)
arm_biquad_cascade_df2T_f32(&EQ_MIC_MID_FILTER, FPGA_Audio_Buffer_TX_I_tmp, FPGA_Audio_Buffer_TX_I_tmp, size);
if (TRX.MIC_EQ_HIG != 0)
arm_biquad_cascade_df2T_f32(&EQ_MIC_HIG_FILTER, FPGA_Audio_Buffer_TX_I_tmp, FPGA_Audio_Buffer_TX_I_tmp, size);
}
// automatic gain control
static void doRX_AGC(uint16_t size, uint_fast8_t mode)
{
DoRxAGC(FPGA_Audio_Buffer_RX_I_tmp, size, mode);
}
// s-meter
static void doRX_SMETER(uint16_t size)
{
if(Processor_RX_Power_value != 0)
return;
// Prepare data to calculate s-meter
float32_t i = 0;
arm_rms_f32(FPGA_Audio_Buffer_RX_I_tmp, size, &i);
if(current_if_gain > 0)
i *= 1.0f / current_if_gain;
Processor_RX_Power_value = i;
}
// copy I to Q channel
static void doRX_COPYCHANNEL(uint16_t size)
{
// Double channel I-> Q
dma_memcpy32((uint32_t *)&FPGA_Audio_Buffer_RX_Q_tmp[0], (uint32_t *)&FPGA_Audio_Buffer_RX_I_tmp[0], size);
}
// FM demodulator
static void DemodulateFM(uint16_t size)
{
float32_t *lpf_prev = &DFM_RX_lpf_prev;
float32_t *hpf_prev_a = &DFM_RX_hpf_prev_a;
float32_t *hpf_prev_b = &DFM_RX_hpf_prev_b;
float32_t *i_prev = &DFM_RX_i_prev;
float32_t *q_prev = &DFM_RX_q_prev;
uint_fast8_t *fm_sql_count = &DFM_RX_fm_sql_count;
float32_t *FPGA_Audio_Buffer_I_tmp = &FPGA_Audio_Buffer_RX_I_tmp[0];
float32_t *FPGA_Audio_Buffer_Q_tmp = &FPGA_Audio_Buffer_RX_Q_tmp[0];
float32_t *fm_sql_avg = &DFM_RX_fm_sql_avg;
arm_biquad_cascade_df2T_instance_f32 *iir_filter_inst = &IIR_RX_Squelch_HPF;
bool *squelched = &DFM_RX_Squelched;
float32_t angle, x, y, a, b;
static float32_t squelch_buf[FPGA_RX_IQ_BUFFER_HALF_SIZE];
for (uint_fast16_t i = 0; i < size; i++)
{
// first, calculate "x" and "y" for the arctan2, comparing the vectors of present data with previous data
y = (FPGA_Audio_Buffer_Q_tmp[i] * *i_prev) - (FPGA_Audio_Buffer_I_tmp[i] * *q_prev);
x = (FPGA_Audio_Buffer_I_tmp[i] * *i_prev) + (FPGA_Audio_Buffer_Q_tmp[i] * *q_prev);
angle = atan2f(y, x);
// we now have our audio in "angle"
squelch_buf[i] = angle; // save audio in "d" buffer for squelch noise filtering/detection - done later
a = *lpf_prev + (FM_RX_LPF_ALPHA * (angle - *lpf_prev)); //
*lpf_prev = a; // save "[n-1]" sample for next iteration
*q_prev = FPGA_Audio_Buffer_Q_tmp[i]; // save "previous" value of each channel to allow detection of the change of angle in next go-around
*i_prev = FPGA_Audio_Buffer_I_tmp[i];
if ((!*squelched) || (!TRX.FM_SQL_threshold)) // high-pass audio only if we are un-squelched (to save processor time)
{
if (CurrentVFO()->Mode == TRX_MODE_WFM)
{
FPGA_Audio_Buffer_I_tmp[i] = (float32_t)(angle / PI) * 0.1f; //second way
}
else
{
b = FM_RX_HPF_ALPHA * (*hpf_prev_b + a - *hpf_prev_a); // do differentiation
*hpf_prev_a = a; // save "[n-1]" samples for next iteration
*hpf_prev_b = b;
FPGA_Audio_Buffer_I_tmp[i] = b * 0.3f; // save demodulated and filtered audio in main audio processing buffer
}
}
else if (*squelched) // were we squelched or tone NOT detected?
FPGA_Audio_Buffer_I_tmp[i] = 0; // do not filter receive audio - fill buffer with zeroes to mute it
}
// *** Squelch Processing ***
arm_biquad_cascade_df2T_f32(iir_filter_inst, squelch_buf, squelch_buf, size); // Do IIR high-pass filter on audio so we may detect squelch noise energy
*fm_sql_avg = ((1.0f - FM_RX_SQL_SMOOTHING) * *fm_sql_avg) + (FM_RX_SQL_SMOOTHING * sqrtf(fabsf(squelch_buf[0]))); // IIR filter squelch energy magnitude: We need look at only one representative sample
*fm_sql_count = *fm_sql_count + 1; // bump count that controls how often the squelch threshold is checked
if (*fm_sql_count >= FM_SQUELCH_PROC_DECIMATION)
*fm_sql_count = 0; // enforce the count limit
// Determine if the (averaged) energy in "ads.fm_sql_avg" is above or below the squelch threshold
if (*fm_sql_count == 0) // do the squelch threshold calculation much less often than we are called to process this audio
{
if (*fm_sql_avg > 0.7f) // limit maximum noise value in averaging to keep it from going out into the weeds under no-signal conditions (higher = noisier)
*fm_sql_avg = 0.7f;
b = *fm_sql_avg * 10.0f; // scale noise amplitude to range of squelch setting
// Now evaluate noise power with respect to squelch setting
if (!TRX.FM_SQL_threshold) // is squelch set to zero?
*squelched = false; // yes, the we are un-squelched
else if (*squelched) // are we squelched?
{
if (b <= (float)((10 - TRX.FM_SQL_threshold) - FM_SQUELCH_HYSTERESIS)) // yes - is average above threshold plus hysteresis?
*squelched = false; // yes, open the squelch
}
else // is the squelch open (e.g. passing audio)?
{
if ((10.0f - TRX.FM_SQL_threshold) > FM_SQUELCH_HYSTERESIS) // is setting higher than hysteresis?
{
if (b > (float)((10 - TRX.FM_SQL_threshold) + FM_SQUELCH_HYSTERESIS)) // yes - is average below threshold minus hysteresis?
*squelched = true; // yes, close the squelch
}
else // setting is lower than hysteresis so we can't use it!
{
if (b > (10.0f - (float)TRX.FM_SQL_threshold)) // yes - is average below threshold?
*squelched = true; // yes, close the squelch
}
}
}
}
// FM modulator
static void ModulateFM(uint16_t size)
{
static float32_t modulation = (float32_t)TRX_SAMPLERATE;
static float32_t hpf_prev_a = 0;
static float32_t hpf_prev_b = 0;
static float32_t sin_data = 0;
static float32_t fm_mod_accum = 0;
static float32_t modulation_index = 15000.0f;
if (CurrentVFO()->TX_LPF_Filter_Width == 5000)
modulation_index = 4000.0f;
if (CurrentVFO()->TX_LPF_Filter_Width == 6000)
modulation_index = 6000.0f;
if (CurrentVFO()->TX_LPF_Filter_Width == 7000)
modulation_index = 8000.0f;
if (CurrentVFO()->TX_LPF_Filter_Width == 8000)
modulation_index = 11000.0f;
if (CurrentVFO()->TX_LPF_Filter_Width == 9000)
modulation_index = 13000.0f;
if (CurrentVFO()->TX_LPF_Filter_Width == 10000)
modulation_index = 15000.0f;
if (CurrentVFO()->TX_LPF_Filter_Width == 15000)
modulation_index = 30000.0f;
if (CurrentVFO()->TX_LPF_Filter_Width == 20000)
modulation_index = 40000.0f;
if (CurrentVFO()->TX_LPF_Filter_Width == 0)
modulation_index = 45000.0f;
// Do differentiating high-pass filter to provide 6dB/octave pre-emphasis - which also removes any DC component!
float32_t ampl = (Processor_selected_RFpower_amplitude / 100.0f * TUNE_POWER);
for (uint_fast16_t i = 0; i < size; i++)
{
hpf_prev_b = FM_TX_HPF_ALPHA * (hpf_prev_b +FPGA_Audio_Buffer_TX_I_tmp[i] - hpf_prev_a); // do differentiation
hpf_prev_a = FPGA_Audio_Buffer_TX_I_tmp[i]; // save "[n-1] samples for next iteration
fm_mod_accum = (1.0f - 0.999f) * fm_mod_accum + 0.999f * hpf_prev_b; // save differentiated data in audio buffer // change frequency using scaled audio
while(fm_mod_accum > modulation) fm_mod_accum -= modulation; // limit range
while(fm_mod_accum < -modulation) fm_mod_accum += modulation; // limit range
sin_data = ((fm_mod_accum * modulation_index) / modulation) * PI;
FPGA_Audio_Buffer_TX_I_tmp[i] = ampl * arm_sin_f32(sin_data);
FPGA_Audio_Buffer_TX_Q_tmp[i] = ampl * arm_cos_f32(sin_data);
}
}
// Apply IF Gain IF Gain
static void doRX_IFGain(uint16_t size)
{
float32_t if_gain = db2rateV(TRX.IF_Gain);
//apply gain
arm_scale_f32(FPGA_Audio_Buffer_RX_I_tmp, if_gain, FPGA_Audio_Buffer_RX_I_tmp, size);
arm_scale_f32(FPGA_Audio_Buffer_RX_Q_tmp, if_gain, FPGA_Audio_Buffer_RX_Q_tmp, size);
current_if_gain = if_gain;
}
Reference in New Issue View Git Blame Kopiuj bezpośredni odnośnik