2022-04-18 16:53:10 +00:00
|
|
|
|
#include "stm32f4xx_hal.h"
|
|
|
|
|
#include "agc.h"
|
|
|
|
|
#include "settings.h"
|
|
|
|
|
#include "audio_filters.h"
|
|
|
|
|
|
|
|
|
|
//Private variables
|
|
|
|
|
static float32_t AGC_RX_need_gain_db = 0.0f;
|
|
|
|
|
static float32_t AGC_TX_need_gain_db = 0.0f;
|
|
|
|
|
static float32_t AGC_RX_need_gain_db_old = 0.0f;
|
|
|
|
|
static float32_t AGC_TX_need_gain_db_old = 0.0f;
|
|
|
|
|
static uint32_t AGC_RX_last_agc_peak_time = 0.0f;
|
|
|
|
|
static float32_t AGC_RX_agcBuffer_kw[AUDIO_BUFFER_HALF_SIZE] = {0};
|
|
|
|
|
IRAM2 static float32_t AGC_RX_ringbuffer[AGC_RINGBUFFER_TAPS_SIZE * AUDIO_BUFFER_HALF_SIZE] = {0};
|
|
|
|
|
static float32_t AGC_TX_ringbuffer_i[AGC_RINGBUFFER_TAPS_SIZE * AUDIO_BUFFER_HALF_SIZE] = {0};
|
|
|
|
|
|
|
|
|
|
//Run AGC on data block
|
|
|
|
|
void DoRxAGC(float32_t *agcBuffer, uint_fast16_t blockSize, uint_fast8_t mode)
|
|
|
|
|
{
|
|
|
|
|
// higher speed in settings - higher speed of AGC processing
|
|
|
|
|
// float32_t *AGC_need_gain_db = &AGC_RX_need_gain_db;
|
|
|
|
|
// float32_t *AGC_need_gain_db_old = &AGC_RX_need_gain_db_old;
|
|
|
|
|
uint32_t *last_agc_peak_time = &AGC_RX_last_agc_peak_time;
|
|
|
|
|
float32_t RX_AGC_STEPSIZE_UP = 0.0f;
|
|
|
|
|
float32_t RX_AGC_STEPSIZE_DOWN = 0.0f;
|
|
|
|
|
if (mode == TRX_MODE_CW_L || mode == TRX_MODE_CW_U)
|
|
|
|
|
{
|
|
|
|
|
RX_AGC_STEPSIZE_UP = 200.0f / (float32_t)TRX.RX_AGC_CW_speed;
|
|
|
|
|
RX_AGC_STEPSIZE_DOWN = 20.0f / (float32_t)TRX.RX_AGC_CW_speed;
|
|
|
|
|
}
|
|
|
|
|
else
|
|
|
|
|
{
|
2022-05-08 12:16:50 +00:00
|
|
|
|
RX_AGC_STEPSIZE_UP = 1000.0f / (float32_t)TRX.RX_AGC_SSB_speed;
|
2022-04-18 16:53:10 +00:00
|
|
|
|
RX_AGC_STEPSIZE_DOWN = 20.0f / (float32_t)TRX.RX_AGC_SSB_speed;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
//do k-weighting (for LKFS)
|
|
|
|
|
arm_biquad_cascade_df2T_f32(&AGC_RX_KW_HSHELF_FILTER, agcBuffer, AGC_RX_agcBuffer_kw, blockSize);
|
|
|
|
|
arm_biquad_cascade_df2T_f32(&AGC_RX_KW_HPASS_FILTER, agcBuffer, AGC_RX_agcBuffer_kw, blockSize);
|
|
|
|
|
|
|
|
|
|
//do ring buffer
|
|
|
|
|
static uint32_t ring_position = 0;
|
|
|
|
|
//save new data to ring buffer
|
|
|
|
|
memcpy(&AGC_RX_ringbuffer[ring_position * blockSize], agcBuffer, sizeof(float32_t) * blockSize);
|
|
|
|
|
//move ring buffer index
|
|
|
|
|
ring_position++;
|
|
|
|
|
if (ring_position >= AGC_RINGBUFFER_TAPS_SIZE)
|
|
|
|
|
ring_position = 0;
|
|
|
|
|
//get old data to process
|
|
|
|
|
memcpy(agcBuffer, &AGC_RX_ringbuffer[ring_position * blockSize], sizeof(float32_t) * blockSize);
|
|
|
|
|
|
|
|
|
|
//calculate the magnitude in dBFS
|
|
|
|
|
float32_t AGC_RX_magnitude = 0;
|
|
|
|
|
arm_rms_f32(AGC_RX_agcBuffer_kw, blockSize, &AGC_RX_magnitude);
|
|
|
|
|
if (AGC_RX_magnitude == 0.0f)
|
|
|
|
|
AGC_RX_magnitude = 0.001f;
|
|
|
|
|
float32_t full_scale_rate = AGC_RX_magnitude / FLOAT_FULL_SCALE_POW;
|
|
|
|
|
float32_t AGC_RX_dbFS = rate2dbV(full_scale_rate);
|
|
|
|
|
|
|
|
|
|
//move the gain one step
|
|
|
|
|
// if (!WM8731_Muting)
|
|
|
|
|
// {
|
|
|
|
|
// float32_t diff = ((float32_t)TRX.AGC_GAIN_TARGET - (AGC_RX_dbFS + AGC_RX_need_gain_db));
|
|
|
|
|
// if (diff > 0)
|
|
|
|
|
// AGC_RX_need_gain_db += diff / RX_AGC_STEPSIZE_UP;
|
|
|
|
|
// else
|
|
|
|
|
// AGC_RX_need_gain_db += diff / RX_AGC_STEPSIZE_DOWN;
|
|
|
|
|
|
|
|
|
|
// //overload (clipping), sharply reduce the gain
|
|
|
|
|
// if ((AGC_RX_dbFS + AGC_RX_need_gain_db) > ((float32_t)TRX.AGC_GAIN_TARGET + AGC_CLIPPING))
|
|
|
|
|
// {
|
|
|
|
|
// AGC_RX_need_gain_db = (float32_t)TRX.AGC_GAIN_TARGET - AGC_RX_dbFS;
|
|
|
|
|
// //sendToDebug_float32(diff,false);
|
|
|
|
|
// }
|
|
|
|
|
// }
|
|
|
|
|
|
|
|
|
|
if (!WM8731_Muting)
|
|
|
|
|
{
|
|
|
|
|
float32_t diff = ((float32_t)TRX.AGC_GAIN_TARGET - (AGC_RX_dbFS + AGC_RX_need_gain_db));
|
|
|
|
|
if (diff > 0)
|
|
|
|
|
{
|
|
|
|
|
if((HAL_GetTick() - *last_agc_peak_time) > TRX.RX_AGC_Hold)
|
|
|
|
|
AGC_RX_need_gain_db += diff / RX_AGC_STEPSIZE_UP;
|
|
|
|
|
}
|
|
|
|
|
else
|
|
|
|
|
{
|
|
|
|
|
AGC_RX_need_gain_db += diff / RX_AGC_STEPSIZE_DOWN;
|
|
|
|
|
*last_agc_peak_time = HAL_GetTick();
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
//overload (clipping), sharply reduce the gain
|
|
|
|
|
if ((AGC_RX_dbFS + AGC_RX_need_gain_db) > ((float32_t)TRX.AGC_GAIN_TARGET + AGC_CLIPPING))
|
|
|
|
|
{
|
|
|
|
|
AGC_RX_need_gain_db = (float32_t)TRX.AGC_GAIN_TARGET - AGC_RX_dbFS;
|
|
|
|
|
//sendToDebug_float32(diff,false);
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
//noise threshold, below it - do not amplify
|
|
|
|
|
// if (AGC_RX_dbFS < TRX.AGC_THRESHOLD)
|
|
|
|
|
// AGC_RX_need_gain_db = 1.0f;
|
|
|
|
|
|
|
|
|
|
//sendToDebug_float32(AGC_RX_dbFS, false);
|
|
|
|
|
|
|
|
|
|
//AGC off, not adjustable
|
|
|
|
|
if (!CurrentVFO()->AGC)
|
|
|
|
|
AGC_RX_need_gain_db = 1.0f;
|
|
|
|
|
|
|
|
|
|
//Muting if need
|
|
|
|
|
if (WM8731_Muting)
|
|
|
|
|
AGC_RX_need_gain_db = -200.0f;
|
|
|
|
|
|
|
|
|
|
//gain limitation
|
2022-05-08 12:16:50 +00:00
|
|
|
|
if (AGC_RX_need_gain_db > (float32_t) TRX.RX_AGC_Max_gain) // AGC_MAX_GAIN;
|
|
|
|
|
AGC_RX_need_gain_db = (float32_t) TRX.RX_AGC_Max_gain; // AGC_MAX_GAIN;
|
2022-04-18 16:53:10 +00:00
|
|
|
|
|
|
|
|
|
//apply gain
|
|
|
|
|
//sendToDebug_float32(AGC_RX_need_gain_db, false);
|
|
|
|
|
if (fabsf(AGC_RX_need_gain_db_old - AGC_RX_need_gain_db) > 0.0f) //gain changed
|
|
|
|
|
{
|
|
|
|
|
float32_t gainApplyStep = 0;
|
|
|
|
|
if (AGC_RX_need_gain_db_old > AGC_RX_need_gain_db)
|
|
|
|
|
gainApplyStep = -(AGC_RX_need_gain_db_old - AGC_RX_need_gain_db) / (float32_t)blockSize;
|
|
|
|
|
if (AGC_RX_need_gain_db_old < AGC_RX_need_gain_db)
|
|
|
|
|
gainApplyStep = (AGC_RX_need_gain_db - AGC_RX_need_gain_db_old) / (float32_t)blockSize;
|
|
|
|
|
float32_t val_prev = 0.0f;
|
|
|
|
|
bool zero_cross = false;
|
|
|
|
|
for (uint_fast16_t i = 0; i < blockSize; i++)
|
|
|
|
|
{
|
|
|
|
|
if (val_prev < 0.0f && agcBuffer[i] > 0.0f)
|
|
|
|
|
zero_cross = true;
|
|
|
|
|
else if (val_prev > 0.0f && agcBuffer[i] < 0.0f)
|
|
|
|
|
zero_cross = true;
|
|
|
|
|
if (zero_cross)
|
|
|
|
|
AGC_RX_need_gain_db_old += gainApplyStep;
|
|
|
|
|
|
|
|
|
|
agcBuffer[i] = agcBuffer[i] * db2rateV(AGC_RX_need_gain_db_old);
|
|
|
|
|
val_prev = agcBuffer[i];
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
else //gain did not change, apply gain across all samples
|
|
|
|
|
{
|
|
|
|
|
arm_scale_f32(agcBuffer, db2rateV(AGC_RX_need_gain_db), agcBuffer, blockSize);
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
//Run TX AGC on data block
|
|
|
|
|
void DoTxAGC(float32_t *agcBuffer_i, uint_fast16_t blockSize, float32_t target, uint_fast8_t mode)
|
|
|
|
|
{
|
|
|
|
|
float32_t *AGC_need_gain_db = &AGC_TX_need_gain_db;
|
|
|
|
|
float32_t *AGC_need_gain_db_old = &AGC_TX_need_gain_db_old;
|
|
|
|
|
float32_t *agc_ringbuffer_i = (float32_t *)&AGC_TX_ringbuffer_i;
|
|
|
|
|
|
|
|
|
|
//higher speed in settings - higher speed of AGC processing
|
|
|
|
|
float32_t TX_AGC_STEPSIZE_UP = 0.0f;
|
|
|
|
|
float32_t TX_AGC_STEPSIZE_DOWN = 0.0f;
|
|
|
|
|
switch(mode)
|
|
|
|
|
{
|
|
|
|
|
case TRX_MODE_LSB:
|
|
|
|
|
case TRX_MODE_USB:
|
|
|
|
|
case TRX_MODE_LOOPBACK:
|
|
|
|
|
default:
|
2022-05-08 12:16:50 +00:00
|
|
|
|
TX_AGC_STEPSIZE_UP = 200.0f / (float32_t)TRX.TX_Compressor_speed_SSB; // <20><> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> TRX.TX_Compressor_speed_SSB 3.0f
|
2022-04-18 16:53:10 +00:00
|
|
|
|
TX_AGC_STEPSIZE_DOWN = 20.0f / (float32_t)TRX.TX_Compressor_speed_SSB; // <20><> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> TRX.TX_Compressor_speed_SSB 3.0f
|
|
|
|
|
break;
|
|
|
|
|
|
|
|
|
|
case TRX_MODE_NFM:
|
|
|
|
|
case TRX_MODE_WFM:
|
|
|
|
|
case TRX_MODE_AM:
|
2022-05-08 12:16:50 +00:00
|
|
|
|
TX_AGC_STEPSIZE_UP = 200.0f / (float32_t)TRX.TX_Compressor_speed_AMFM; // <20><> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> TRX.TX_Compressor_speed_AMFM 3.0f
|
2022-04-18 16:53:10 +00:00
|
|
|
|
TX_AGC_STEPSIZE_DOWN = 20.0f / (float32_t)TRX.TX_Compressor_speed_AMFM; // <20><> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> TRX.TX_Compressor_speed_AMFM 3.0f
|
|
|
|
|
break;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
//do ring buffer
|
|
|
|
|
static uint32_t ring_position = 0;
|
|
|
|
|
//save new data to ring buffer
|
|
|
|
|
memcpy(&agc_ringbuffer_i[ring_position * blockSize], agcBuffer_i, sizeof(float32_t) * blockSize);
|
|
|
|
|
//move ring buffer index
|
|
|
|
|
ring_position++;
|
|
|
|
|
if (ring_position >= AGC_RINGBUFFER_TAPS_SIZE)
|
|
|
|
|
ring_position = 0;
|
|
|
|
|
//get old data to process
|
|
|
|
|
memcpy(agcBuffer_i, &agc_ringbuffer_i[ring_position * blockSize], sizeof(float32_t) * blockSize);
|
|
|
|
|
|
|
|
|
|
//calculate the magnitude
|
|
|
|
|
float32_t AGC_TX_I_magnitude = 0;
|
|
|
|
|
float32_t ampl_max_i = 0.0f;
|
|
|
|
|
float32_t ampl_min_i = 0.0f;
|
|
|
|
|
uint32_t tmp_index;
|
|
|
|
|
arm_max_no_idx_f32(agcBuffer_i, blockSize, &l_max_i);
|
|
|
|
|
arm_min_f32(agcBuffer_i, blockSize, &l_min_i, &tmp_index);
|
|
|
|
|
if (ampl_max_i > -ampl_min_i)
|
|
|
|
|
AGC_TX_I_magnitude = ampl_max_i;
|
|
|
|
|
else
|
|
|
|
|
AGC_TX_I_magnitude = -ampl_min_i;
|
|
|
|
|
|
|
|
|
|
if (AGC_TX_I_magnitude == 0.0f)
|
|
|
|
|
AGC_TX_I_magnitude = 0.001f;
|
|
|
|
|
float32_t AGC_TX_dbFS = rate2dbV(AGC_TX_I_magnitude);
|
|
|
|
|
|
|
|
|
|
//move the gain one step
|
|
|
|
|
float32_t diff = (target - (AGC_TX_dbFS + *AGC_need_gain_db));
|
|
|
|
|
if (diff > 0)
|
|
|
|
|
*AGC_need_gain_db += diff / TX_AGC_STEPSIZE_UP;
|
|
|
|
|
else
|
|
|
|
|
*AGC_need_gain_db += diff / TX_AGC_STEPSIZE_DOWN;
|
|
|
|
|
|
|
|
|
|
//overload (clipping), sharply reduce the gain
|
|
|
|
|
if ((AGC_TX_dbFS + *AGC_need_gain_db) > target)
|
|
|
|
|
{
|
|
|
|
|
*AGC_need_gain_db = target - AGC_TX_dbFS;
|
|
|
|
|
//sendToDebug_float32(diff,false);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
//noise threshold, below it - do not amplify
|
|
|
|
|
/*if (AGC_RX_dbFS < AGC_NOISE_GATE)
|
|
|
|
|
*AGC_need_gain_db = 1.0f;*/
|
|
|
|
|
|
|
|
|
|
//Muting if need
|
|
|
|
|
if (target == 0.0f)
|
|
|
|
|
*AGC_need_gain_db = -200.0f;
|
|
|
|
|
|
|
|
|
|
//gain limitation
|
|
|
|
|
switch(mode)
|
|
|
|
|
{
|
|
|
|
|
case TRX_MODE_LSB:
|
|
|
|
|
case TRX_MODE_USB:
|
|
|
|
|
case TRX_MODE_LOOPBACK:
|
|
|
|
|
default:
|
|
|
|
|
if (*AGC_need_gain_db > TRX.TX_Compressor_maxgain_SSB) // TX_Compressor_maxgain_SSB <20><><EFBFBD><EFBFBD> <20><> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 10.0f
|
2022-05-08 12:16:50 +00:00
|
|
|
|
*AGC_need_gain_db = TRX.TX_Compressor_maxgain_SSB; // TX_Compressor_maxgain_SSB <20><><EFBFBD><EFBFBD> <20><> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 10.0f
|
2022-04-18 16:53:10 +00:00
|
|
|
|
break;
|
|
|
|
|
|
|
|
|
|
case TRX_MODE_NFM:
|
|
|
|
|
case TRX_MODE_WFM:
|
|
|
|
|
case TRX_MODE_AM:
|
|
|
|
|
if (*AGC_need_gain_db > TRX.TX_Compressor_maxgain_AMFM) // TX_Compressor_maxgain_AMFM
|
2022-05-08 12:16:50 +00:00
|
|
|
|
*AGC_need_gain_db = TRX.TX_Compressor_maxgain_AMFM; // TX_Compressor_maxgain_AMFM
|
2022-04-18 16:53:10 +00:00
|
|
|
|
break;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
//apply gain
|
|
|
|
|
if (fabsf(*AGC_need_gain_db_old - *AGC_need_gain_db) > 0.0f) //gain changed
|
|
|
|
|
{
|
|
|
|
|
float32_t gainApplyStep = 0;
|
|
|
|
|
if (*AGC_need_gain_db_old > *AGC_need_gain_db)
|
|
|
|
|
gainApplyStep = -(*AGC_need_gain_db_old - *AGC_need_gain_db) / (float32_t)blockSize;
|
|
|
|
|
if (*AGC_need_gain_db_old < *AGC_need_gain_db)
|
|
|
|
|
gainApplyStep = (*AGC_need_gain_db - *AGC_need_gain_db_old) / (float32_t)blockSize;
|
|
|
|
|
float32_t val_prev = 0.0f;
|
|
|
|
|
bool zero_cross = false;
|
|
|
|
|
for (uint_fast16_t i = 0; i < blockSize; i++)
|
|
|
|
|
{
|
|
|
|
|
if (val_prev < 0.0f && agcBuffer_i[i] > 0.0f)
|
|
|
|
|
zero_cross = true;
|
|
|
|
|
else if (val_prev > 0.0f && agcBuffer_i[i] < 0.0f)
|
|
|
|
|
zero_cross = true;
|
|
|
|
|
if (zero_cross)
|
|
|
|
|
*AGC_need_gain_db_old += gainApplyStep;
|
|
|
|
|
|
|
|
|
|
agcBuffer_i[i] = agcBuffer_i[i] * db2rateV(*AGC_need_gain_db_old);
|
|
|
|
|
val_prev = agcBuffer_i[i];
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
else //gain did not change, apply gain across all samples
|
|
|
|
|
{
|
|
|
|
|
arm_scale_f32(agcBuffer_i, db2rateV(*AGC_need_gain_db), agcBuffer_i, blockSize);
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void ResetAGC(void)
|
|
|
|
|
{
|
|
|
|
|
AGC_RX_need_gain_db = 0.0f;
|
|
|
|
|
memset(AGC_RX_ringbuffer, 0x00, sizeof(AGC_RX_ringbuffer));
|
|
|
|
|
}
|