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

@@ -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;
 }
 
 

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;
+		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, ">");
+			//fprintf(stderr, ">");
 			nco_crcf_set_frequency(rx->carrier_coarse_nco, MAX_COARSE_FREQ_OFFSET);
 		} else if(actual_freq < -MAX_COARSE_FREQ_OFFSET) {
-				fprintf(stderr, "<");
+			//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;
-		}
+		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
 	}