static float cfo_cp_estimate(srslte_sync_t *q, const cf_t *input)
{
uint32_t cp_offset = 0;
cp_offset = srslte_cp_synch(&q->cp_synch, input, q->max_offset, q->cfo_cp_nsymbols, SRSLTE_CP_LEN_NORM(1,q->fft_size));
cf_t cp_corr_max = srslte_cp_synch_corr_output(&q->cp_synch, cp_offset);
float cfo = -carg(cp_corr_max) / M_PI / 2;
return cfo;
}
/** Finds the PSS sequence previously defined by a call to srslte_sync_set_N_id_2()
* around the position find_offset in the buffer input.
* Returns 1 if the correlation peak exceeds the threshold set by srslte_sync_set_threshold()
* or 0 otherwise. Returns a negative number on error (if N_id_2 has not been set)
*
* The maximum of the correlation peak is always stored in *peak_position
*/
int srslte_sync_find(srslte_sync_t *q, cf_t *input, uint32_t find_offset, uint32_t *peak_position)
{
int ret = SRSLTE_ERROR_INVALID_INPUTS;
if (q != NULL &&
input != NULL &&
srslte_N_id_2_isvalid(q->N_id_2) &&
fft_size_isvalid(q->fft_size))
{
int peak_pos;
ret = SRSLTE_SUCCESS;
if (peak_position) {
*peak_position = 0;
}
/* Estimate CFO using CP */
if (q->enable_cfo_corr) {
uint32_t cp_offset = srslte_cp_synch(&q->cp_synch, input, q->nof_symbols, q->nof_symbols, SRSLTE_CP_LEN_NORM(1,q->fft_size));
cf_t cp_corr_max = srslte_cp_synch_corr_output(&q->cp_synch, cp_offset);
float cfo = -carg(cp_corr_max) / M_PI / 2;
/* compute cumulative moving average CFO */
INFO("cp_offset_pos=%d, abs=%f, cfo=%f, mean_cfo=%f, nof_symb=%d\n",
cp_offset, cabs(cp_corr_max), cfo, q->mean_cfo, q->nof_symbols);
if (q->mean_cfo) {
q->mean_cfo = SRSLTE_VEC_EMA(cfo, q->mean_cfo, q->cfo_ema_alpha);
} else {
q->mean_cfo = cfo;
}
/* Correct CFO with the averaged CFO estimation */
srslte_cfo_correct(&q->cfocorr, input, input, -q->mean_cfo / q->fft_size);
}
if (q->find_cfo_i && q->enable_cfo_corr) {
float peak_value;
float max_peak_value = -99;
peak_pos = 0;
srslte_pss_synch_t *pss_obj[3] = {&q->pss_i[0], &q->pss, &q->pss_i[1]};
for (int cfo_i=0;cfo_i<3;cfo_i++) {
srslte_pss_synch_set_N_id_2(pss_obj[cfo_i], q->N_id_2);
int p = srslte_pss_synch_find_pss(pss_obj[cfo_i], &input[find_offset], &peak_value);
if (peak_value > max_peak_value) {
max_peak_value = peak_value;
peak_pos = p;
q->peak_value = peak_value;
q->cfo_i = cfo_i-1;
}
}
if (q->cfo_i != 0) {
srslte_vec_prod_ccc(input, q->cfo_i_corr[q->cfo_i<0?0:1], input, q->frame_size);
INFO("Compensating cfo_i=%d\n", q->cfo_i);
}
} else {
srslte_pss_synch_set_N_id_2(&q->pss, q->N_id_2);
peak_pos = srslte_pss_synch_find_pss(&q->pss, &input[find_offset], &q->peak_value);
if (peak_pos < 0) {
fprintf(stderr, "Error calling finding PSS sequence\n");
return SRSLTE_ERROR;
}
}
q->mean_peak_value = SRSLTE_VEC_EMA(q->peak_value, q->mean_peak_value, MEANPEAK_EMA_ALPHA);
if (peak_position) {
*peak_position = (uint32_t) peak_pos;
}
/* If peak is over threshold, compute CFO and SSS */
if (q->peak_value >= q->threshold) {
// Try to detect SSS
if (q->sss_en) {
// Set an invalid N_id_1 indicating SSS is yet to be detected
q->N_id_1 = 1000;
if (sync_sss(q, input, find_offset + peak_pos, q->cp) < 0) {
DEBUG("No space for SSS processing. Frame starts at %d\n", peak_pos);
}
}
if (q->detect_cp) {
if (peak_pos + find_offset >= 2*(q->fft_size + SRSLTE_CP_LEN_EXT(q->fft_size))) {
srslte_sync_set_cp(q, srslte_sync_detect_cp(q, input, peak_pos + find_offset));
} else {
DEBUG("Not enough room to detect CP length. Peak position: %d\n", peak_pos);
}
}
// Return 1 (peak detected) even if we couldn't estimate CFO and SSS
ret = 1;
} else {
ret = 0;
}
DEBUG("SYNC ret=%d N_id_2=%d find_offset=%d pos=%d peak=%.2f threshold=%.2f sf_idx=%d, CFO=%.3f KHz\n",
ret, q->N_id_2, find_offset, peak_pos, q->peak_value, q->threshold, q->sf_idx, 15*(q->cfo_i+q->mean_cfo));
} else if (srslte_N_id_2_isvalid(q->N_id_2)) {
fprintf(stderr, "Must call srslte_sync_set_N_id_2() first!\n");
}
return ret;
}