/* Returns the CFO estimation given a PSS received sequence
*
* Source: An Efficient CFO Estimation Algorithm for the Downlink of 3GPP-LTE
* Feng Wang and Yu Zhu
*/
float srslte_pss_synch_cfo_compute(srslte_pss_synch_t* q, cf_t *pss_recv) {
cf_t y0, y1, yr;
y0 = srslte_vec_dot_prod_ccc(q->pss_signal_time[q->N_id_2], pss_recv, q->fft_size/2);
y1 = srslte_vec_dot_prod_ccc(&q->pss_signal_time[q->N_id_2][q->fft_size/2], &pss_recv[q->fft_size/2], q->fft_size/2);
yr = conjf(y0) * y1;
return atan2f(__imag__ yr, __real__ yr) / M_PI;
}
uint32_t srslte_conv_cc(cf_t *input, cf_t *filter, cf_t *output, uint32_t input_len, uint32_t filter_len) {
uint32_t i;
uint32_t M = filter_len;
uint32_t N = input_len;
for (i=0;i<M;i++) {
output[i]=srslte_vec_dot_prod_ccc(&input[i],&filter[i],i);
}
for (;i<M+N-1;i++) {
output[i] = srslte_vec_dot_prod_ccc(&input[i-M], filter, M);
}
return M+N-1;
}
/* Returns the CFO estimation given a PSS received sequence
*
* Source: An Efficient CFO Estimation Algorithm for the Downlink of 3GPP-LTE
* Feng Wang and Yu Zhu
*/
float srslte_pss_cfo_compute(srslte_pss_t* q, const cf_t *pss_recv) {
cf_t y0, y1;
const cf_t *pss_ptr = pss_recv;
if (q->filter_pss_enable) {
srslte_pss_filter(q, pss_recv, q->tmp_fft);
pss_ptr = (const cf_t*) q->tmp_fft;
}
y0 = srslte_vec_dot_prod_ccc(q->pss_signal_time[q->N_id_2], pss_ptr, q->fft_size/2);
y1 = srslte_vec_dot_prod_ccc(&q->pss_signal_time[q->N_id_2][q->fft_size/2], &pss_ptr[q->fft_size/2], q->fft_size/2);
return carg(conjf(y0) * y1)/M_PI;
}
/* Centered convolution. Returns the same number of input elements. Equivalent to conv(x,h,'same') in matlab.
* y(n)=sum_i x(n+i-M/2)*h(i) for n=1..N with N input samples and M filter len
*/
uint32_t srslte_conv_same_cc(cf_t *input, cf_t *filter, cf_t *output, uint32_t input_len, uint32_t filter_len) {
uint32_t i;
uint32_t M = filter_len;
uint32_t N = input_len;
for (i=0;i<M/2;i++) {
output[i]=srslte_vec_dot_prod_ccc(&input[i],&filter[M/2-i],M-M/2+i);
}
for (;i<N-M/2;i++) {
output[i]=srslte_vec_dot_prod_ccc(&input[i-M/2],filter,M);
}
for (;i<N;i++) {
output[i]=srslte_vec_dot_prod_ccc(&input[i-M/2],filter,N-i+M/2);
}
return N;
}
/** Performs time-domain PSS correlation.
* Returns the index of the PSS correlation peak in a subframe.
* The frame starts at corr_peak_pos-subframe_size/2.
* The value of the correlation is stored in corr_peak_value.
*
* Input buffer must be subframe_size long.
*/
int srslte_pss_synch_find_pss(srslte_pss_synch_t *q, cf_t *input, float *corr_peak_value)
{
int ret = SRSLTE_ERROR_INVALID_INPUTS;
if (q != NULL &&
input != NULL)
{
uint32_t corr_peak_pos;
uint32_t conv_output_len;
if (!srslte_N_id_2_isvalid(q->N_id_2)) {
fprintf(stderr, "Error finding PSS peak, Must set N_id_2 first\n");
return SRSLTE_ERROR;
}
/* Correlate input with PSS sequence */
if (q->frame_size >= q->fft_size) {
#ifdef CONVOLUTION_FFT
memcpy(q->tmp_input, input, q->frame_size * sizeof(cf_t));
conv_output_len = srslte_conv_fft_cc_run(&q->conv_fft, q->tmp_input, q->pss_signal_time[q->N_id_2], q->conv_output);
#else
conv_output_len = srslte_conv_cc(input, q->pss_signal_time[q->N_id_2], q->conv_output, q->frame_size, q->fft_size);
#endif
} else {
for (int i=0;i<q->frame_size;i++) {
q->conv_output[i] = srslte_vec_dot_prod_ccc(q->pss_signal_time[q->N_id_2], &input[i], q->fft_size);
}
conv_output_len = q->frame_size;
}
#ifdef SRSLTE_PSS_ABS_SQUARE
srslte_vec_abs_square_cf(q->conv_output, q->conv_output_abs, conv_output_len-1);
#else
srslte_vec_abs_cf(q->conv_output, q->conv_output_abs, conv_output_len-1);
#endif
srslte_vec_sc_prod_fff(q->conv_output_abs, q->ema_alpha, q->conv_output_abs, conv_output_len-1);
srslte_vec_sc_prod_fff(q->conv_output_avg, 1-q->ema_alpha, q->conv_output_avg, conv_output_len-1);
srslte_vec_sum_fff(q->conv_output_abs, q->conv_output_avg, q->conv_output_avg, conv_output_len-1);
/* Find maximum of the absolute value of the correlation */
corr_peak_pos = srslte_vec_max_fi(q->conv_output_avg, conv_output_len-1);
// save absolute value
q->peak_value = q->conv_output_avg[corr_peak_pos];
#ifdef SRSLTE_PSS_RETURN_PSR
// Find second side lobe
// Find end of peak lobe to the right
int pl_ub = corr_peak_pos+1;
while(q->conv_output_avg[pl_ub+1] <= q->conv_output_avg[pl_ub] && pl_ub < conv_output_len) {
pl_ub ++;
}
// Find end of peak lobe to the left
int pl_lb;
if (corr_peak_pos > 2) {
pl_lb = corr_peak_pos-1;
while(q->conv_output_avg[pl_lb-1] <= q->conv_output_avg[pl_lb] && pl_lb > 1) {
pl_lb --;
}
} else {
pl_lb = 0;
}
int sl_distance_right = conv_output_len-1-pl_ub;
if (sl_distance_right < 0) {
sl_distance_right = 0;
}
int sl_distance_left = pl_lb;
int sl_right = pl_ub+srslte_vec_max_fi(&q->conv_output_avg[pl_ub], sl_distance_right);
int sl_left = srslte_vec_max_fi(q->conv_output_avg, sl_distance_left);
float side_lobe_value = SRSLTE_MAX(q->conv_output_avg[sl_right], q->conv_output_avg[sl_left]);
if (corr_peak_value) {
*corr_peak_value = q->conv_output_avg[corr_peak_pos]/side_lobe_value;
DEBUG("peak_pos=%2d, pl_ub=%2d, pl_lb=%2d, sl_right: %2d, sl_left: %2d, PSR: %.2f/%.2f=%.2f\n", corr_peak_pos, pl_ub, pl_lb,
sl_right,sl_left, q->conv_output_avg[corr_peak_pos], side_lobe_value,*corr_peak_value);
}
#else
if (corr_peak_value) {
*corr_peak_value = q->conv_output_avg[corr_peak_pos];
}
#endif
if (q->frame_size >= q->fft_size) {
ret = (int) corr_peak_pos;
} else {
ret = (int) corr_peak_pos + q->fft_size;
}
}
return ret;
}
/** Performs time-domain PSS correlation.
* Returns the index of the PSS correlation peak in a subframe.
* The frame starts at corr_peak_pos-subframe_size/2.
* The value of the correlation is stored in corr_peak_value.
*
* Input buffer must be subframe_size long.
*/
int srslte_pss_find_pss(srslte_pss_t *q, const cf_t *input, float *corr_peak_value)
{
int ret = SRSLTE_ERROR_INVALID_INPUTS;
if (q != NULL &&
input != NULL)
{
uint32_t corr_peak_pos;
uint32_t conv_output_len;
if (!srslte_N_id_2_isvalid(q->N_id_2)) {
ERROR("Error finding PSS peak, Must set N_id_2 first\n");
return SRSLTE_ERROR;
}
/* Correlate input with PSS sequence
*
* We do not reverse time-domain PSS signal because it's conjugate is symmetric.
* The conjugate operation on pss_signal_time has been done in srslte_pss_init_N_id_2
* This is why we can use FFT-based convolution
*/
if (q->frame_size >= q->fft_size) {
#ifdef CONVOLUTION_FFT
memcpy(q->tmp_input, input, (q->frame_size * q->decimate) * sizeof(cf_t));
if(q->decimate > 1) {
srslte_filt_decim_cc_execute(&(q->filter), q->tmp_input, q->filter.downsampled_input, q->filter.filter_output , (q->frame_size * q->decimate));
conv_output_len = srslte_conv_fft_cc_run_opt(&q->conv_fft, q->filter.filter_output,q->pss_signal_freq_full[q->N_id_2], q->conv_output);
} else {
conv_output_len = srslte_conv_fft_cc_run_opt(&q->conv_fft, q->tmp_input, q->pss_signal_freq_full[q->N_id_2], q->conv_output);
}
#else
conv_output_len = srslte_conv_cc(input, q->pss_signal_time[q->N_id_2], q->conv_output, q->frame_size, q->fft_size);
#endif
} else {
for (int i=0;i<q->frame_size;i++) {
q->conv_output[i] = srslte_vec_dot_prod_ccc(q->pss_signal_time[q->N_id_2], &input[i], q->fft_size);
}
conv_output_len = q->frame_size;
}
// Compute modulus square
srslte_vec_abs_square_cf(q->conv_output, q->conv_output_abs, conv_output_len-1);
// If enabled, average the absolute value from previous calls
if (q->ema_alpha < 1.0 && q->ema_alpha > 0.0) {
srslte_vec_sc_prod_fff(q->conv_output_abs, q->ema_alpha, q->conv_output_abs, conv_output_len-1);
srslte_vec_sc_prod_fff(q->conv_output_avg, 1-q->ema_alpha, q->conv_output_avg, conv_output_len-1);
srslte_vec_sum_fff(q->conv_output_abs, q->conv_output_avg, q->conv_output_avg, conv_output_len-1);
} else {
memcpy(q->conv_output_avg, q->conv_output_abs, sizeof(float)*(conv_output_len-1));
}
/* Find maximum of the absolute value of the correlation */
corr_peak_pos = srslte_vec_max_fi(q->conv_output_avg, conv_output_len-1);
// save absolute value
q->peak_value = q->conv_output_avg[corr_peak_pos];
#ifdef SRSLTE_PSS_RETURN_PSR
if (corr_peak_value) {
*corr_peak_value = compute_peak_sidelobe(q, corr_peak_pos, conv_output_len);
}
#else
if (corr_peak_value) {
*corr_peak_value = q->conv_output_avg[corr_peak_pos];
}
#endif
if(q->decimate >1) {
int decimation_correction = (q->filter.num_taps - 2);
corr_peak_pos = corr_peak_pos - decimation_correction;
corr_peak_pos = corr_peak_pos*q->decimate;
}
if (q->frame_size >= q->fft_size) {
ret = (int) corr_peak_pos;
} else {
ret = (int) corr_peak_pos + q->fft_size;
}
}
return ret;
}