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.
This commit is contained in:
parent
3b7628882c
commit
e42602633e
|
@ -2,6 +2,8 @@
|
||||||
|
|
||||||
#include "freq_est.h"
|
#include "freq_est.h"
|
||||||
|
|
||||||
|
#include "utils.h"
|
||||||
|
|
||||||
// Fits a line y' = m*x' + t to the given points (x, y), where x is the
|
// Fits a line y' = m*x' + t to the given points (x, y), where x is the
|
||||||
// zero-based index in the array.
|
// zero-based index in the array.
|
||||||
static void linear_regression(size_t nx, const float *y, float *m, float *t)
|
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;
|
*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 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
|
// square the symbols and calculate phase
|
||||||
for(size_t i = 0; i < n; i++) {
|
for(size_t i = 0; i < n; i++) {
|
||||||
float complex s = recv[i];
|
float complex s = recv[i];
|
||||||
phases[i] = cargf(s * s);
|
recv_sq[i] = s * s;
|
||||||
}
|
}
|
||||||
|
|
||||||
liquid_unwrap_phase(phases, n);
|
float freq_est = freq_est_lr(recv_sq, n);
|
||||||
|
float phi0 = cargf(recv[0]);
|
||||||
float freq, phi0;
|
|
||||||
linear_regression(n, phases, &freq, &phi0);
|
|
||||||
|
|
||||||
// outputs must be divided by 2 because squaring the symbols doubles their phase.
|
// outputs must be divided by 2 because squaring the symbols doubles their phase.
|
||||||
if(final_phase) {
|
if(final_phase) {
|
||||||
*final_phase = (phi0 + n * freq) / 2;
|
*final_phase = (phi0 + n * freq_est) / 2;
|
||||||
}
|
}
|
||||||
|
|
||||||
return freq/2;
|
return freq_est/2;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
|
@ -48,7 +48,7 @@ static bool acquire_preamble(layer1_rx_t *rx, const float complex sample)
|
||||||
// preamble search
|
// preamble search
|
||||||
float complex corr_out = correlator_step(&rx->preamble_correlator, sample);
|
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!
|
// Preamble found!
|
||||||
DEBUG_LOG("Preamble found: %.3f > %.3f\n", cabsf(corr_out), 0.5f * preamble_get_symbol_count());
|
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 {
|
} else {
|
||||||
// preamble not found.
|
// preamble not found.
|
||||||
|
|
||||||
// Run the frequency estimator in blocks of FREQ_EST_L samples.
|
// Run the frequency estimator for every incoming sample overy the last
|
||||||
// This is an implementation that works with unknown BPSK symbols
|
// FREQ_EST_L samples. This is an implementation that works with unknown
|
||||||
// and therefore can be used during ramp-up and preamble.
|
// 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);
|
|
||||||
|
|
||||||
float freq_adjustment = (freq_est / RRC_SPS) * 0.3f;
|
float complex linearized_history[FREQ_EST_L];
|
||||||
nco_crcf_adjust_frequency(rx->carrier_coarse_nco, freq_adjustment);
|
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 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 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);
|
assert(freq_est_history_write_idx < FREQ_EST_L);
|
||||||
|
|
||||||
freq_est_history[freq_est_history_write_idx] = sample;
|
freq_est_history[freq_est_history_write_idx] = sample;
|
||||||
freq_est_history_write_idx++;
|
freq_est_history_write_idx++;
|
||||||
|
freq_est_history_write_idx %= FREQ_EST_L;
|
||||||
|
|
||||||
return false; // preamble not found
|
return false; // preamble not found
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in a new issue