First shot at frequency synchronization

Supports only the BPSK part so far (ramp-up, preamble).

impl/src/config.h

@@ -17,7 +17,7 @@
 #define RRC_DELAY       7   // delay in symbols
 #define RRC_BETA        0.2f
 
-#define TRANSMISSION_RAMP_UP_LEN     32  // symbols
+#define TRANSMISSION_RAMP_UP_LEN    128  // symbols
 #define TRANSMISSION_RAMP_DOWN_LEN   32  // symbols
 
 #endif // CONFIG_H

impl/src/main.c

@@ -1,6 +1,7 @@
 #include <stdio.h>
 #include <string.h>
 #include <assert.h>
+#include <math.h>
 #include <liquid/liquid.h>
 
 #include "utils.h"
@@ -19,9 +20,9 @@ int main(void)
 
 	channel_cccf channel = channel_cccf_create();
 
-	float snr = 20.0f;
+	float snr = 50.0f;
 	channel_cccf_add_awgn(channel, -snr, snr);
-	channel_cccf_add_carrier_offset(channel, 0.01f, 1.00f);
+	channel_cccf_add_carrier_offset(channel, 0.10f, 1.00f);
 
 
 	packet_mod_ctx_t pmod;
@@ -77,12 +78,93 @@ int main(void)
 	channel_cccf_execute_block(channel, whole_burst, burst_len, msg_received);
 	dump_array_cf(msg_received, burst_len, 1.0f, "/tmp/rx.cpx");
 
+	// create NCO for carrier frequency compensation
+	nco_crcf carrier_nco = nco_crcf_create(LIQUID_NCO);
+	nco_crcf_set_frequency(carrier_nco, 0.00f);
+	nco_crcf_set_phase(carrier_nco, 0.0f);
+
 	// create symbol synchronizer
 	symsync_crcf symsync = symsync_crcf_create_rnyquist(LIQUID_FIRFILT_RRC, RRC_SPS, RRC_DELAY, RRC_BETA, 32);
 
 	float complex symsync_out[burst_len];
-	unsigned int  symsync_out_len;
-	symsync_crcf_execute(symsync, msg_received, burst_len, symsync_out, &symsync_out_len);
+	unsigned int  symsync_out_len = 0;
+
+#define FREQ_EST_L 16
+	float phase_history[FREQ_EST_L];
+	memset(phase_history, 0, sizeof(phase_history));
+
+	for(unsigned int i = 0; i < burst_len; i++) {
+		// Mix the input signal with the carrier NCO, which oscillates at the
+		// frequency estimated so far.
+		float complex mixed_sample;
+		nco_crcf_step(carrier_nco);
+		nco_crcf_mix_down(carrier_nco, msg_received[i], &mixed_sample);
+
+		// run the timing synchronizer (works even with shifted frequency
+		unsigned int out_len;
+		symsync_crcf_execute(symsync, &mixed_sample, 1, symsync_out + symsync_out_len, &out_len);
+
+		if(out_len != 0) {
+			// 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
+			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);
+
+			// 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;
+
+			nco_crcf_adjust_frequency(carrier_nco, lms_phase_change / RRC_SPS / 32);
+
+			printf("Phase change: %.6f - carrier frequency: %.6f\n", lms_phase_change, 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");
+			}
+		}
+
+		symsync_out_len += out_len;
+	}
+
 	dump_array_cf(symsync_out, symsync_out_len, 1.0f, "/tmp/rx.cpx");
 
 #if 0
@@ -124,6 +206,10 @@ int main(void)
 	printf("%u bit errors detected.\n", bit_errors);
 #endif
 
+	nco_crcf_destroy(carrier_nco);
+
+	symsync_crcf_destroy(symsync);
+
 	fec_destroy(q);
 
 	modem_destroy(demod);