/* CP detection algorithm taken from:
* "SSS Detection Method for Initial Cell Search in 3GPP LTE FDD/TDD Dual Mode Receiver"
* by Jung-In Kim et al.
*/
srslte_cp_t srslte_sync_detect_cp(srslte_sync_t *q, const cf_t *input, uint32_t peak_pos)
{
float R_norm=0, R_ext=0, C_norm=0, C_ext=0;
float M_norm=0, M_ext=0;
uint32_t cp_norm_len = SRSLTE_CP_LEN_NORM(7, q->fft_size);
uint32_t cp_ext_len = SRSLTE_CP_LEN_EXT(q->fft_size);
uint32_t nof_symbols = peak_pos/(q->fft_size+cp_ext_len);
if (nof_symbols > 3) {
nof_symbols = 3;
}
if (nof_symbols > 0) {
const cf_t *input_cp_norm = &input[peak_pos-nof_symbols*(q->fft_size+cp_norm_len)];
const cf_t *input_cp_ext = &input[peak_pos-nof_symbols*(q->fft_size+cp_ext_len)];
for (int i=0;i<nof_symbols;i++) {
R_norm += crealf(srslte_vec_dot_prod_conj_ccc(&input_cp_norm[q->fft_size], input_cp_norm, cp_norm_len));
C_norm += cp_norm_len * srslte_vec_avg_power_cf(input_cp_norm, cp_norm_len);
input_cp_norm += q->fft_size+cp_norm_len;
}
if (C_norm > 0) {
M_norm = R_norm/C_norm;
}
q->M_norm_avg = SRSLTE_VEC_EMA(M_norm/nof_symbols, q->M_norm_avg, CP_EMA_ALPHA);
for (int i=0;i<nof_symbols;i++) {
R_ext += crealf(srslte_vec_dot_prod_conj_ccc(&input_cp_ext[q->fft_size], input_cp_ext, cp_ext_len));
C_ext += cp_ext_len * srslte_vec_avg_power_cf(input_cp_ext, cp_ext_len);
input_cp_ext += q->fft_size+cp_ext_len;
}
if (C_ext > 0) {
M_ext = R_ext/C_ext;
}
q->M_ext_avg = SRSLTE_VEC_EMA(M_ext/nof_symbols, q->M_ext_avg, CP_EMA_ALPHA);
if (q->M_norm_avg > q->M_ext_avg) {
return SRSLTE_CP_NORM;
} else if (q->M_norm_avg < q->M_ext_avg) {
return SRSLTE_CP_EXT;
} else {
if (R_norm > R_ext) {
return SRSLTE_CP_NORM;
} else {
return SRSLTE_CP_EXT;
}
}
} else {
return SRSLTE_CP_NORM;
}
}
float srslte_vec_avg_power_cf(cf_t *x, uint32_t len) {
return crealf(srslte_vec_dot_prod_conj_ccc(x,x,len)) / len;
}
void srslte_agc_process(srslte_agc_t *q, cf_t *signal, uint32_t len) {
float gain_db = 10*log10(q->gain);
float gain_uhd_db = 1.0;
//float gain_uhd = 1.0;
float y = 0;
// Apply current gain to input signal
if (!q->uhd_handler) {
srslte_vec_sc_prod_cfc(signal, q->gain, signal, len);
} else {
if (q->gain < 1) {
q->gain = 1.0;
}
if (isinf(gain_db) || isnan(gain_db)) {
q->gain = 10.0;
} else {
gain_uhd_db = q->set_gain_callback(q->uhd_handler, gain_db);
q->gain = pow(10, gain_uhd_db/10);
}
}
float *t;
switch(q->mode) {
case SRSLTE_AGC_MODE_ENERGY:
y = sqrtf(crealf(srslte_vec_dot_prod_conj_ccc(signal, signal, len))/len);
break;
case SRSLTE_AGC_MODE_PEAK_AMPLITUDE:
t = (float*) signal;
y = t[srslte_vec_max_fi(t, 2*len)];// take only positive max to avoid abs() (should be similar)
break;
default:
fprintf(stderr, "Unsupported AGC mode\n");
return;
}
if (q->nof_frames > 0) {
q->y_tmp[q->frame_cnt++] = y;
if (q->frame_cnt == q->nof_frames) {
q->frame_cnt = 0;
switch(q->mode) {
case SRSLTE_AGC_MODE_ENERGY:
y = srslte_vec_acc_ff(q->y_tmp, q->nof_frames)/q->nof_frames;
break;
case SRSLTE_AGC_MODE_PEAK_AMPLITUDE:
y = q->y_tmp[srslte_vec_max_fi(q->y_tmp, q->nof_frames)];
break;
default:
fprintf(stderr, "Unsupported AGC mode\n");
return;
}
}
}
double gg = 1.0;
if (q->isfirst) {
q->y_out = y;
q->isfirst = false;
} else {
if (q->frame_cnt == 0) {
q->y_out = (1-q->bandwidth) * q->y_out + q->bandwidth * y;
if (!q->lock) {
gg = expf(-0.5*q->bandwidth*logf(q->y_out/q->target));
q->gain *= gg;
}
INFO("AGC gain: %.2f (%.2f) y_out=%.3f, y=%.3f target=%.1f gg=%.2f\n", gain_db, gain_uhd_db, q->y_out, y, q->target, gg);
}
}
}
