Move frequency estimation to separate module

impl/CMakeLists.txt

@@ -21,6 +21,8 @@ set(sources
 	src/transmission.c
 	src/correlator.h
 	src/correlator.c
+	src/freq_est.h
+	src/freq_est.c
 )
 
 include_directories(

impl/src/freq_est.c

@@ -0,0 +1,73 @@
+#include <liquid/liquid.h>
+
+#include "freq_est.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)
+{
+	float mean_x = (nx-1) / 2.0f;
+	float mean_y = 0.0f;
+	for(unsigned int j = 0; j < nx; j++) {
+		mean_y += y[j];
+	}
+	mean_y /= nx;
+
+	float numerator = 0.0f;
+	float denominator = 0.0f;
+	for(unsigned int j = 0; j < nx; j++) {
+		float delta_index = j - mean_x;
+		numerator += delta_index * (y[j] - mean_y);
+		denominator += delta_index*delta_index;
+	}
+
+	*m = numerator / denominator;
+	//*t = mean_y - (*m) * mean_x;
+	*t = y[0]; // This should work because x always starts at 0.
+}
+
+
+float freq_est_unknown_bpsk(const float complex *recv, size_t n, float *final_phase)
+{
+	float phases[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);
+	}
+
+	liquid_unwrap_phase(phases, n);
+
+	float freq, phi0;
+	linear_regression(n, phases, &freq, &phi0);
+
+	// outputs must be divided by 2 because squaring the symbols doubles their phase.
+	if(final_phase) {
+		*final_phase = (phi0 + n * freq) / 2;
+	}
+
+	return freq/2;
+}
+
+
+float freq_est_known_symbols(const float complex *recv, const float complex *ref, size_t n, float *final_phase)
+{
+	float phases[n];
+
+	// square the symbols and calculate phase
+	for(size_t i = 0; i < n; i++) {
+		phases[i] = cargf(conjf(ref[i]) * recv[i]);
+	}
+
+	liquid_unwrap_phase(phases, n);
+
+	float freq, phi0;
+	linear_regression(n, phases, &freq, &phi0);
+
+	if(final_phase) {
+		*final_phase = phi0 + n * freq;
+	}
+
+	return freq;
+}

impl/src/freq_est.h

@@ -0,0 +1,38 @@
+#ifndef FREQ_EST_H
+#define FREQ_EST_H
+
+#include <complex.h>
+#include <stdlib.h>
+
+/*!
+ * \brief Estimate the frequency of unknown BPSK symbols.
+ *
+ * This function squares all given symbols to remove data ambiguity, calculates
+ * the phase values and then performs linear regression on those to determine
+ * the frequency.
+ *
+ * \param recv        Pointer to the array of received symbols.
+ * \param n           Number of symbols given.
+ * \param final_phase Pointer to a float where the phase correction value is
+ *                    stored. May be NULL if that value is not required.
+ * \returns           The estimated frequency in radians per sample.
+ */
+float freq_est_unknown_bpsk(const float complex *recv, size_t n, float *final_phase);
+
+/*!
+ * \brief Estimate the frequency of known symbols.
+ *
+ * This function removes the data influence by calculating
+ * recv[k]*conj(ref[k]), calculates the phase values and then performs linear
+ * regression on those to determine the frequency.
+ *
+ * \param recv        Pointer to the array of received symbols.
+ * \param ref         Pointer to the array of reference symbols (e.g. a preamble).
+ * \param n           Number of symbols given.
+ * \param final_phase Pointer to a float where the phase correction value is
+ *                    stored. May be NULL if that value is not required.
+ * \returns           The estimated frequency in radians per sample.
+ */
+float freq_est_known_symbols(const float complex *recv, const float complex *ref, size_t n, float *final_phase);
+
+#endif // FREQ_EST_H

impl/src/main.c

@@ -11,6 +11,7 @@
 #include "preamble.h"
 #include "transmission.h"
 #include "correlator.h"
+#include "freq_est.h"
 
 typedef enum {
 	RX_STATE_ACQUISITION,
@@ -109,9 +110,6 @@ int main(void)
 	unsigned int  symsync_out_len = 0;
 
 #define FREQ_EST_L 8
-	float phase_history[FREQ_EST_L];
-	memset(phase_history, 0, sizeof(phase_history));
-
 	// General receiver state
 	rx_state_t rx_state = RX_STATE_ACQUISITION;
 
@@ -170,63 +168,16 @@ int main(void)
 					} else {
 						// preamble not found.
 
-						// for all the output samples produced, run the frequency
-						// estimator. This is an implementation that works with unknown
-						// BPSK symbols and therefore can be used during ramp-up and
-						// preamble.
-
-						if(out_len < FREQ_EST_L) {
-							memmove(phase_history,
-									phase_history + out_len,
-									(FREQ_EST_L-out_len) * sizeof(phase_history[0]));
-						}
-
-						for(unsigned int j = 0; j < out_len; j++) {
-							float complex *psymbol = symsync_out + symsync_out_len + j;
-
-							// square the symbol to remove BPSK ambiguity
-							float phase = cargf((*psymbol) * (*psymbol));
-
-							phase_history[FREQ_EST_L - out_len + j] = phase;
-						}
-
-						// update the frequency estimate
+						// 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(((i/RRC_SPS) % FREQ_EST_L) == 0) {
-							float unwrapped_phase_history[FREQ_EST_L];
-							memcpy(unwrapped_phase_history, phase_history, sizeof(unwrapped_phase_history));
-							liquid_unwrap_phase(unwrapped_phase_history, FREQ_EST_L);
+							float freq_est = freq_est_unknown_bpsk(symsync_out + symsync_out_len - FREQ_EST_L, FREQ_EST_L, NULL);
 
-							// calculate slope of LMS-fitted line
-							float mean_index = (FREQ_EST_L-1) / 2.0f;
-							float mean_phase = 0.0f;
-							for(unsigned int j = 0; j < FREQ_EST_L; j++) {
-								mean_phase += unwrapped_phase_history[j];
-							}
-							mean_phase /= FREQ_EST_L;
-
-							float numerator = 0.0f;
-							float denominator = 0.0f;
-							for(unsigned int j = 0; j < FREQ_EST_L; j++) {
-								float delta_index = j - mean_index;
-								numerator += delta_index * (unwrapped_phase_history[j] - mean_phase);
-								denominator += delta_index*delta_index;
-							}
-
-							float lms_phase_change = numerator / denominator;
-
-							float freq_adjustment = (lms_phase_change / RRC_SPS / 2) * 0.5f;
+							float freq_adjustment = (freq_est / RRC_SPS) * 0.5f;
 							nco_crcf_adjust_frequency(carrier_nco, freq_adjustment);
 
 							printf("Frequency adjustment: %.6f - carrier frequency: %.6f\n", freq_adjustment, nco_crcf_get_frequency(carrier_nco));
-
-							if(i/RRC_SPS == 2*FREQ_EST_L) {
-								float complex tmp[FREQ_EST_L];
-								for(unsigned int j = 0; j < FREQ_EST_L; j++) {
-									tmp[j] = unwrapped_phase_history[j];
-								}
-								dump_array_cf(tmp, FREQ_EST_L, 1.0f, "/tmp/freq_est.cpx");
-								printf("MARK\n");
-							}
 						}
 					}
 					break;