From e42602633ea0feaed7785999cd4148d4388bb62e Mon Sep 17 00:00:00 2001 From: Thomas Kolb Date: Sun, 14 May 2023 15:53:14 +0200 Subject: [PATCH] freq_est: implement L&R-based symbol-independent frequency estimation The RX chain uses this to acquire an initial estimate of the carrier frequency. The estimate is adjusted on every incoming symbol until a preamble is found. --- impl/src/layer1/freq_est.c | 94 ++++++++++++++++++++++++++++++++++---- impl/src/layer1/rx.c | 45 +++++++++--------- 2 files changed, 110 insertions(+), 29 deletions(-) diff --git a/impl/src/layer1/freq_est.c b/impl/src/layer1/freq_est.c index 1897c8c..7011bd4 100644 --- a/impl/src/layer1/freq_est.c +++ b/impl/src/layer1/freq_est.c @@ -2,6 +2,8 @@ #include "freq_est.h" +#include "utils.h" + // Fits a line y' = m*x' + t to the given points (x, y), where x is the // zero-based index in the array. static void linear_regression(size_t nx, const float *y, float *m, float *t) @@ -25,28 +27,104 @@ static void linear_regression(size_t nx, const float *y, float *m, float *t) *t = mean_y - (*m) * mean_x; } +#if 0 +// Calculates an estimation of the true peak frequency in the signal by +// calculating the FFT und interpolating around the bin with the highest value. +static float fft_true_peak(const float complex *recv, size_t n) +{ + float complex *out = fft_malloc(n * sizeof(float complex)); + + fft_run(n, (float complex*)recv, out, LIQUID_FFT_FORWARD, 0); + fft_shift(out, n); // shifts by n/2 + + // search for the maximum-amplitude bin + size_t peak_pos = 0; + float peak_amplitude = 0.0f; + + float amp[n]; + for(size_t i = 0; i < n; i++) { + amp[i] = cabsf(out[i]); + if(amp[i] > peak_amplitude) { + peak_amplitude = amp[i]; + peak_pos = i; + } + } + + dump_array_f(amp, n, 1.0f, "/tmp/fft_amp.f32"); + + // interpolation does not work at the first and last bin + if(peak_pos == 0 || peak_pos == (n-1)) { + return (peak_pos - n/2) / n; + } + + float a1 = amp[peak_pos - 1]; + float a2 = amp[peak_pos]; + float a3 = amp[peak_pos + 1]; + + // find fractional offset to peak_pos by quadratic interpolation. + // Source: https://dspguru.com/dsp/howtos/how-to-interpolate-fft-peak/ + float d = (a3 - a1) / (2 * (2 * a2 - a1 - a3)); + + fft_free(out); + + return ((float)peak_pos + d - (float)(n/2)) / n; +} +#endif + + +// Frequency estimation algorithm by Luise & Reggiannini +float freq_est_lr(const float complex *recv, size_t l0) +{ + const size_t N = l0/2; + + float complex R[N]; + + // calculate R[kappa] + for(size_t kappa = 1; kappa <= N; kappa++) { + R[kappa-1] = 0.0f; + + for(size_t k = 0; k < l0 - kappa; k++) { + R[kappa-1] += recv[k + kappa] * conjf(recv[k]); + } + + R[kappa-1] /= l0 - kappa; + } + + // sum over R[kappa] + float complex R_total = 0.0f; + for(size_t kappa = 1; kappa <= N; kappa++) { + R_total += R[kappa-1]; + } + + // calculate phase angle of summed R + float arg = cargf(R_total); + + // convert to frequency offset in radians per sample + float freq_offset = arg / (N+1) / 2.0f; /* FIXME: not sure where the factor 2 comes from. */ + + return freq_offset; +} + float freq_est_unknown_bpsk(const float complex *recv, size_t n, float *final_phase) { - float phases[n]; + float complex recv_sq[n]; // square the symbols and calculate phase for(size_t i = 0; i < n; i++) { float complex s = recv[i]; - phases[i] = cargf(s * s); + recv_sq[i] = s * s; } - liquid_unwrap_phase(phases, n); - - float freq, phi0; - linear_regression(n, phases, &freq, &phi0); + float freq_est = freq_est_lr(recv_sq, n); + float phi0 = cargf(recv[0]); // outputs must be divided by 2 because squaring the symbols doubles their phase. if(final_phase) { - *final_phase = (phi0 + n * freq) / 2; + *final_phase = (phi0 + n * freq_est) / 2; } - return freq/2; + return freq_est/2; } diff --git a/impl/src/layer1/rx.c b/impl/src/layer1/rx.c index edac5b2..5c7e613 100644 --- a/impl/src/layer1/rx.c +++ b/impl/src/layer1/rx.c @@ -48,7 +48,7 @@ static bool acquire_preamble(layer1_rx_t *rx, const float complex sample) // preamble search float complex corr_out = correlator_step(&rx->preamble_correlator, sample); - if(cabsf(corr_out) > 0.9f * preamble_get_symbol_count()) { + if(cabsf(corr_out) > 0.5f * preamble_get_symbol_count()) { // Preamble found! DEBUG_LOG("Preamble found: %.3f > %.3f\n", cabsf(corr_out), 0.5f * preamble_get_symbol_count()); @@ -80,33 +80,36 @@ static bool acquire_preamble(layer1_rx_t *rx, const float complex sample) } else { // preamble not found. - // Run the frequency estimator in blocks of FREQ_EST_L samples. - // This is an implementation that works with unknown BPSK symbols - // and therefore can be used during ramp-up and preamble. - if(freq_est_history_write_idx == FREQ_EST_L) { - float freq_est = freq_est_unknown_bpsk(freq_est_history, FREQ_EST_L, NULL); + // Run the frequency estimator for every incoming sample overy the last + // FREQ_EST_L samples. This is an implementation that works with unknown + // BPSK symbols and therefore can be used during ramp-up and preamble. - float freq_adjustment = (freq_est / RRC_SPS) * 0.3f; - nco_crcf_adjust_frequency(rx->carrier_coarse_nco, freq_adjustment); - - float actual_freq = nco_crcf_get_frequency(rx->carrier_coarse_nco); - if(actual_freq > MAX_COARSE_FREQ_OFFSET) { - fprintf(stderr, ">"); - nco_crcf_set_frequency(rx->carrier_coarse_nco, MAX_COARSE_FREQ_OFFSET); - } else if(actual_freq < -MAX_COARSE_FREQ_OFFSET) { - fprintf(stderr, "<"); - nco_crcf_set_frequency(rx->carrier_coarse_nco, -MAX_COARSE_FREQ_OFFSET); - } - - DEBUG_LOG("Frequency adjustment: %.6f - carrier frequency: %.6f\n", freq_adjustment, nco_crcf_get_frequency(rx->carrier_coarse_nco)); - - freq_est_history_write_idx = 0; + float complex linearized_history[FREQ_EST_L]; + for(size_t i = 0; i < FREQ_EST_L; i++) { + linearized_history[i] = freq_est_history[(i + freq_est_history_write_idx) % FREQ_EST_L]; } + float freq_est = freq_est_unknown_bpsk(linearized_history, FREQ_EST_L, NULL); + + float freq_adjustment = freq_est / 32.0f; + nco_crcf_adjust_frequency(rx->carrier_coarse_nco, freq_adjustment); + + float actual_freq = nco_crcf_get_frequency(rx->carrier_coarse_nco); + if(actual_freq > MAX_COARSE_FREQ_OFFSET) { + //fprintf(stderr, ">"); + nco_crcf_set_frequency(rx->carrier_coarse_nco, MAX_COARSE_FREQ_OFFSET); + } else if(actual_freq < -MAX_COARSE_FREQ_OFFSET) { + //fprintf(stderr, "<"); + nco_crcf_set_frequency(rx->carrier_coarse_nco, -MAX_COARSE_FREQ_OFFSET); + } + + DEBUG_LOG("Freq. est: %0.6f - Adj: %.6f - carrier frequency: %.6f\n", freq_est, freq_adjustment, nco_crcf_get_frequency(rx->carrier_coarse_nco)); + assert(freq_est_history_write_idx < FREQ_EST_L); freq_est_history[freq_est_history_write_idx] = sample; freq_est_history_write_idx++; + freq_est_history_write_idx %= FREQ_EST_L; return false; // preamble not found }