hamnet70/impl/src/layer1/rx.c

#include <liquid/liquid.h>

#include <assert.h>
#include <math.h>

#include "correlator.h"
#include "preamble.h"
#include "freq_est.h"
#include "whitening.h"

#include "config.h"
#include "options.h"

#include "rx.h"

#include "utils.h"

#define SYMBOL_BUFFER_SIZE 16384

#define HEADER_SIZE_BYTES 5 // including CRC
#define FREQ_EST_L 24

#define AGC_BW_ACQUISITION 5e-2f
#define AGC_BW_TRACKING    1e-4f

#define MAX_COARSE_FREQ_OFFSET 0.20f

#define SHOW_DEBUG_LOG 0

#if SHOW_DEBUG_LOG
#	define DEBUG_LOG(...) fprintf(stderr, "DBG: " __VA_ARGS__)
#else
#	define DEBUG_LOG(...) {}
#endif

#define PLL_BW_HEADER 0.03f
#define PLL_BW_DATA 0.01f

static void update_nco_pll(nco_crcf nco, float phase_error, float bw)
{
	const float pll_alpha = bw;                             // phase adjustment factor
	const float pll_beta  = (pll_alpha * pll_alpha) * 1.0f; // frequency adjustment factor

	nco_crcf_adjust_phase(nco, pll_alpha * phase_error);
	nco_crcf_adjust_frequency(nco, pll_beta * phase_error);

	/*DEBUG_LOG("NCO adjusted: f=%.6f, φ=%.6f\n",
			nco_crcf_get_frequency(nco),
			nco_crcf_get_phase(nco));*/
}


static bool acquire_preamble(layer1_rx_t *rx, const float complex sample, bool do_coarse_freq_est)
{
	static float complex freq_est_history[FREQ_EST_L];
	static unsigned int  freq_est_history_write_idx = 0;

	static uint32_t freq_est_holdoff_samples = 0;

	// preamble search
	float complex corr_out = correlator_step(&rx->preamble_correlator, sample);

	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());

		float phase_offset;
		float mean_frequency_error = correlator_get_mean_frequency_deviation(&rx->preamble_correlator, FREQ_EST_L, &phase_offset);

		DEBUG_LOG("Phase offset:                 %.6f rad\n", phase_offset);
		DEBUG_LOG("Preamble frequency deviation: %.6f rad/symbol\n", mean_frequency_error);

		// Set the fine carrier correction NCO to the values estimated from the preamble
		nco_crcf_set_frequency(rx->carrier_fine_nco, mean_frequency_error);
		nco_crcf_set_phase(rx->carrier_fine_nco, phase_offset);

		float fcoarse = nco_crcf_get_frequency(rx->carrier_coarse_nco);
		float ffine = nco_crcf_get_frequency(rx->carrier_fine_nco);
		DEBUG_LOG("New estimated carrier frequency: %.6f + %.6f/4 = %.6f rad/sample\n", fcoarse, ffine, fcoarse+ffine/4);

		//float complex input_history[preamble_get_symbol_count()];
		//correlator_get_input_history(&rx->preamble_correlator, input_history);

		//DEBUG_LOG("import numpy as np\n");
		//DEBUG_LOG("import matplotlib.pyplot as pp\n");
		//print_complex_array("pre", preamble_get_symbols(), preamble_get_symbol_count());
		//print_complex_array("recv", input_history, preamble_get_symbol_count());
		//DEBUG_LOG("pp.plot(np.angle(recv * pre.conj()))\n");
		//DEBUG_LOG("pp.show()\n");

		// start over with frequency estimation when preamble search restarts.
		freq_est_history_write_idx = 0;
		freq_est_holdoff_samples = FREQ_EST_L;
		return true; // preamble found!
	} else if(do_coarse_freq_est) {
		// preamble not found.
		//DEBUG_LOG("Preamble not found: %.3f < %.3f\n", cabsf(corr_out), 0.5f * preamble_get_symbol_count());

		// 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.

		if(freq_est_holdoff_samples == 0) { //freq_est_history_write_idx == FREQ_EST_L) {
			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_in_rampup(freq_est_history, FREQ_EST_L, NULL);

			// freq_est_in_rampup will return exactly 0.0 if (and probably only if)
			// the sequency quality is not good enough. We skip the adjustment in
			// this case.
			if(freq_est != 0.0f) {
				// apply the frequency adjustment
				float freq_adjustment = freq_est / RRC_SPS / FREQ_EST_L;
				nco_crcf_adjust_frequency(rx->carrier_coarse_nco, freq_adjustment);

				// limit the absolute frequency offset value
				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);
				}

				//freq_est_history_write_idx = 0;

				DEBUG_LOG("Freq. est (x%d): %0.6f - Adj (x1): %.6f - carrier frequency (x1): %.6f\n",
						RRC_SPS, freq_est, freq_adjustment, nco_crcf_get_frequency(rx->carrier_coarse_nco));

				freq_est_holdoff_samples = preamble_get_symbol_count() * 2;
			}
		} else {
			freq_est_holdoff_samples--;
		}

		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
}


typedef enum squelch_state_t {
	SQUELCH_CLOSED,
	SQUELCH_OPEN,
	SQUELCH_JUST_OPENED,
};

static enum squelch_state_t update_and_check_squelch(layer1_rx_t *rx, unsigned int sample_idx)
{
	float level = agc_crcf_get_rssi(rx->agc);
	enum squelch_state_t result = SQUELCH_CLOSED;

	if((sample_idx & 0xFF) == 0) { // every 256 samples
		if(level < rx->noise_floor_level) {
			rx->noise_floor_level = level;
		} else {
			level += 1e-4f; // slowly increase the measured noise floor level to compensate for drift
		}
		agc_crcf_squelch_set_threshold(rx->agc, rx->noise_floor_level + 10.0f); // in dB
	}

	switch(agc_crcf_squelch_get_status(rx->agc)) {
		case LIQUID_AGC_SQUELCH_RISE:
			DEBUG_LOG("Squelch disabled at #%u RSSI = %.3f dB [thr: %.3f dB]\n", sample_idx, level, agc_crcf_squelch_get_threshold(rx->agc));
			result = SQUELCH_JUST_OPENED;
			break;

		case LIQUID_AGC_SQUELCH_SIGNALHI:
			result = SQUELCH_OPEN;
			break;

		case LIQUID_AGC_SQUELCH_FALL:
			DEBUG_LOG("Squelch enabled at #%u RSSI = %.3f dB [thr: %.3f dB]\n", sample_idx, level, agc_crcf_squelch_get_threshold(rx->agc));
			// fall through
		case LIQUID_AGC_SQUELCH_SIGNALLO:
		case LIQUID_AGC_SQUELCH_ENABLED:
		case LIQUID_AGC_SQUELCH_TIMEOUT:
			result = SQUELCH_CLOSED;
			break;
	}

	return result;
}


result_t layer1_rx_process(layer1_rx_t *rx, const float complex *samples, size_t sample_count)
{
	static size_t symbol_counter = 0;
	static uint8_t symbols_int[SYMBOL_BUFFER_SIZE];

	float complex samples2dump[sample_count];
	size_t nsamples2dump = 0;
	float complex samples2dump2[sample_count];
	size_t nsamples2dump2 = 0;

	// cache configuration flags
	bool is_central_node = options_is_flag_set(OPTIONS_FLAG_IS_CENTRAL_NODE);

	// filter the input
	float complex filtered_samples[sample_count];
	firfilt_crcf_execute_block(rx->channel_filter, (float complex *)samples, sample_count, filtered_samples);

	DEBUG_LOG("\nagc: %f\n", agc_crcf_get_gain(rx->agc));

	for(unsigned int i = 0; i < sample_count; i++) {
		rx_state_t last_state = rx->state;

		// Apply the AGC.
		float complex agc_out;
		agc_crcf_execute(rx->agc, filtered_samples[i], &agc_out);

		switch(update_and_check_squelch(rx, i)) {
			case SQUELCH_CLOSED:
				// ignore this sample
				continue;

			case SQUELCH_JUST_OPENED:
				symsync_crcf_reset(rx->symsync);
				// fall through
			case SQUELCH_OPEN:
				// sample processing continues
				break;
		}


		// Mix the input signal with the carrier NCO, which oscillates at the
		// frequency coarsly estimated so far.
		float complex mixed_sample;

		if(is_central_node) {
			// central nodes do not do coarse frequency tracking, as the clients
			// shall lock onto the central’s carrier frequency and therefore not much
			// frequency deviation is expected.
			mixed_sample = agc_out;
		} else {
			nco_crcf_step(rx->carrier_coarse_nco);
			nco_crcf_mix_down(rx->carrier_coarse_nco, agc_out, &mixed_sample);
		}

		samples2dump[nsamples2dump++] = mixed_sample;

		// run the timing synchronizer (works even with shifted frequency)
		unsigned int out_len;
		float complex symsync_out;
		symsync_crcf_execute(rx->symsync, &mixed_sample, 1, &symsync_out, &out_len);

		if(out_len != 0) {
			switch(rx->state) {
				// Try to acquire packets by synchronizing the frequency
				// (symbol-independent search) and correlating the preamble.
				case RX_STATE_ACQUISITION:
					// reset the acquisition periodically if the preamble is not found.
					symbol_counter++;
					if(symbol_counter == 30 * SYMBOL_RATE) {
						symbol_counter = 0;
						nco_crcf_set_frequency(rx->carrier_coarse_nco, 0.0f);
						nco_crcf_set_phase(rx->carrier_coarse_nco, 0.0f);
					}

					if(acquire_preamble(rx, symsync_out, !is_central_node)) {
						// Preamble found and frequency corrected!
						rx->callback(RX_EVT_PREAMBLE_FOUND, NULL, 0);

						// switch AGC to tracking mode
						agc_crcf_set_bandwidth(rx->agc, AGC_BW_TRACKING);

						// go on with decoding the header
						rx->state = RX_STATE_HEADER;
						symbol_counter = 0;
					}
					break;

				case RX_STATE_HEADER:
					nco_crcf_step(rx->carrier_fine_nco);
					nco_crcf_mix_down(rx->carrier_fine_nco, symsync_out, &mixed_sample);

					if(symbol_counter < rx->hdr_len_symbols) {
						unsigned int sym_demod;
						modem_demodulate(rx->hdr_demod, mixed_sample, &sym_demod);

						float phase_error = modem_get_demodulator_phase_error(rx->hdr_demod);
						DEBUG_LOG("Sym: %d; Phase error: %f %s\n", sym_demod, phase_error,
								(fabs(phase_error) > 0.3) ? "!!!" : "");


						update_nco_pll(rx->carrier_fine_nco, phase_error, PLL_BW_HEADER);

						symbols_int[symbol_counter] = sym_demod;
						symbol_counter++;
					}

					if(symbol_counter == rx->hdr_len_symbols) {
						unsigned int nsyms;
						uint8_t header_enc[rx->hdr_len_enc_bytes];
						uint8_t header[HEADER_SIZE_BYTES];

						ERR_CHECK_LIQUID(liquid_repack_bytes(
									symbols_int, modem_get_bps(rx->hdr_demod), rx->hdr_len_symbols,
									header_enc, 8, sizeof(header_enc), &nsyms));

						if(fec_decode(rx->hdr_fec, sizeof(header), header_enc, header) != LIQUID_OK) {
							DEBUG_LOG("Header decoding failed!\n", rx->modcod);
							rx->state = RX_STATE_ACQUISITION;
							rx->callback(RX_EVT_HEADER_ERROR, NULL, 0);
							break;
						}

						// CRC check
						if(crc_check_key(LIQUID_CRC_8, header, HEADER_SIZE_BYTES) != LIQUID_OK) {
							uint8_t expected_crc = crc_generate_key(LIQUID_CRC_8, header, 4);
							DEBUG_LOG("Header CRC check failed! Expected: 0x%02x, received: 0x%02x\n",
									expected_crc, header[4]);
							rx->state = RX_STATE_ACQUISITION;
							rx->callback(RX_EVT_HEADER_ERROR, NULL, 0);
							break;
						}

						rx->payload_len_bytes = (((uint16_t)header[0] << 8) | header[1]) & 0x07FF;
						rx->payload_crc       = ((uint16_t)header[2] << 8) | header[3];
						rx->modcod            = header[0] >> 3;

						// check the received information
						if(!MODCOD_IS_VALID(rx->modcod)) {
							DEBUG_LOG("Decoded MODCOD %d is invalid!\n", rx->modcod);
							rx->state = RX_STATE_ACQUISITION;
							rx->callback(RX_EVT_HEADER_ERROR, NULL, 0);
							break;
						}

						// check the payload length
						if(rx->payload_len_bytes == 0) {
							DEBUG_LOG("Packet length %u is not supported.\n", rx->payload_len_bytes);
							rx->state = RX_STATE_ACQUISITION;
							rx->callback(RX_EVT_HEADER_ERROR, NULL, 0);
							break;
						}

						rx->payload_demod = modcod_create_modem(rx->modcod);
						uint16_t payload_bps = modem_get_bps(rx->payload_demod);

						rx->payload_len_enc_bytes = fec_get_enc_msg_length(PAYLOAD_CHANNEL_CODE, rx->payload_len_bytes);
						rx->payload_len_symbols = (rx->payload_len_enc_bytes * 8 + payload_bps - 1) / payload_bps;

						if(rx->payload_len_symbols > SYMBOL_BUFFER_SIZE) {
							DEBUG_LOG("Symbol count %u is too lange for buffer. Ignoring packet.\n", rx->payload_len_symbols);
							rx->state = RX_STATE_ACQUISITION;
							rx->callback(RX_EVT_HEADER_ERROR, NULL, 0);
							break;
						}

						assert(rx->payload_len_symbols < sizeof(symbols_int)/sizeof(symbols_int[0]));

						DEBUG_LOG("=== DECODED HEADER ===\n");
						DEBUG_LOG("Payload length: %u symbols, %u bytes encoded, %u bytes decoded\n", rx->payload_len_symbols, rx->payload_len_enc_bytes, rx->payload_len_bytes);
						DEBUG_LOG("CRC16: 0x%04x\n", rx->payload_crc);
						DEBUG_LOG("======================\n");

						//dump_array_cf(symbols_cpx, symbol_counter, 1.0f, "/tmp/hdr.cpx");

						rx->state = RX_STATE_DATA;
						symbol_counter = 0;
					}
					break;

				case RX_STATE_DATA:
					nco_crcf_step(rx->carrier_fine_nco);
					nco_crcf_mix_down(rx->carrier_fine_nco, symsync_out, &mixed_sample);

					samples2dump2[nsamples2dump2++] = mixed_sample;
					if(symbol_counter < rx->payload_len_symbols) {
						unsigned int sym_demod;
						modem_demodulate(rx->payload_demod, mixed_sample, &sym_demod);

						float phase_error = modem_get_demodulator_phase_error(rx->payload_demod);
						//DEBUG_LOG("Sym: %d; Phase error: %f\n", sym_demod, phase_error);

						update_nco_pll(rx->carrier_fine_nco, phase_error, PLL_BW_DATA);

						symbols_int[symbol_counter] = sym_demod;
						symbol_counter++;
					}

					if(symbol_counter == rx->payload_len_symbols) {
						unsigned int nsyms;
						unsigned char payload_enc[rx->payload_len_enc_bytes];
						unsigned char payload[rx->payload_len_bytes];

						ERR_CHECK_LIQUID(liquid_repack_bytes(
									symbols_int, modem_get_bps(rx->payload_demod), rx->payload_len_symbols,
									payload_enc, 8, sizeof(payload_enc), &nsyms));

						fec payload_fec = modcod_create_fec(rx->modcod);
						fec_decode(payload_fec, rx->payload_len_bytes, payload_enc, payload);
						fec_destroy(payload_fec);

						// de-whiten the data
						whitening_apply_in_place(payload, rx->payload_len_bytes);

						int valid = crc_validate_message(PAYLOAD_CRC_SCHEME, payload, rx->payload_len_bytes, rx->payload_crc);

						payload[rx->payload_len_bytes] = '\0';

						//dump_array_cf(symbols_cpx, symbol_counter, 1.0f, "/tmp/payload.cpx");
						if(valid) {
							rx->callback(RX_EVT_PACKET_RECEIVED, payload, rx->payload_len_bytes);
						} else {
							rx->callback(RX_EVT_CHECKSUM_ERROR, payload, rx->payload_len_bytes);
						}

						modem_destroy(rx->payload_demod);
						rx->state = RX_STATE_ACQUISITION;
					}
					break;
			}
		}

		if(rx->state != last_state) {
			if(rx->state == RX_STATE_ACQUISITION) {
				// ensure the AGC is in acquisition mode again when
				// RX_STATE_ACQUISITION is entered.
				agc_crcf_set_bandwidth(rx->agc, AGC_BW_ACQUISITION);
			}
		}
	}

	assert(nsamples2dump <= (sizeof(samples2dump) / sizeof(samples2dump[0])));

	dump_array_cf(samples2dump, nsamples2dump, 1.0f/(RRC_SPS * SYMBOL_RATE), "/tmp/rx_dbg.cpx64");
	dump_array_cf(samples2dump2, nsamples2dump2, 1.0f/(SYMBOL_RATE), "/tmp/rx_dbg2.cpx64");

	return OK;
}


result_t layer1_rx_init(layer1_rx_t *rx, rx_callback_t callback)
{
	rx->callback = callback;

	rx->state = RX_STATE_ACQUISITION;

	// create channel FIR filter to remove out-of-band interferers
	rx->channel_filter = firfilt_crcf_create_kaiser(21, 0.28f * RRC_SPS / 4, 60.0f, 0.0f);

	// create the AGC
	rx->agc = agc_crcf_create();
	agc_crcf_set_bandwidth(rx->agc, AGC_BW_ACQUISITION);

	// set up squelch
	rx->noise_floor_level = 0.0f;
	agc_crcf_squelch_set_threshold(rx->agc, rx->noise_floor_level + 3.0f); // in dB
	agc_crcf_squelch_enable(rx->agc);

	// create NCOs for carrier frequency compensation
	rx->carrier_coarse_nco = nco_crcf_create(LIQUID_NCO);
	nco_crcf_set_frequency(rx->carrier_coarse_nco, 0.00f);
	nco_crcf_set_phase(rx->carrier_coarse_nco, 0.0f);

	rx->carrier_fine_nco = nco_crcf_create(LIQUID_NCO);
	nco_crcf_set_frequency(rx->carrier_fine_nco, 0.00f);
	nco_crcf_set_phase(rx->carrier_fine_nco, 0.0f);

	// forward error correction objects
	rx->hdr_fec     = fec_create(HEADER_CHANNEL_CODE, NULL);

	// demodulators
	rx->hdr_demod     = modem_create(HEADER_MODULATION);

	// symbol timing synchronizer
	rx->symsync = symsync_crcf_create_rnyquist(LIQUID_FIRFILT_RRC, RRC_SPS, RRC_DELAY, RRC_BETA, 32);

	// Correlator for preamble search
	correlator_init(&rx->preamble_correlator, preamble_get_symbols(), preamble_get_symbol_count());

	// precalculate encoded header length in symbols
	unsigned int hdr_bps = modem_get_bps(rx->hdr_demod);
	rx->hdr_len_enc_bytes = fec_get_enc_msg_length(HEADER_CHANNEL_CODE, HEADER_SIZE_BYTES);
	rx->hdr_len_symbols = (rx->hdr_len_enc_bytes * 8 + hdr_bps - 1) / hdr_bps;

	return OK;
}


result_t layer1_rx_shutdown(layer1_rx_t *rx)
{
	firfilt_crcf_destroy(rx->channel_filter);

	agc_crcf_destroy(rx->agc);

	nco_crcf_destroy(rx->carrier_coarse_nco);
	nco_crcf_destroy(rx->carrier_fine_nco);

	fec_destroy(rx->hdr_fec);

	modem_destroy(rx->hdr_demod);

	symsync_crcf_destroy(rx->symsync);

	correlator_free(&rx->preamble_correlator);

	return OK;
}


bool layer1_rx_is_busy(const layer1_rx_t *rx)
{
	return rx->state != RX_STATE_ACQUISITION;
}


float layer1_rx_get_coarse_carrier_frequency(const layer1_rx_t *rx)
{
	return nco_crcf_get_frequency(rx->carrier_coarse_nco);
}