398 lines
13 KiB
C
398 lines
13 KiB
C
#include <liquid/liquid.h>
|
|
#include <math.h>
|
|
|
|
#include <assert.h>
|
|
|
|
#include "correlator.h"
|
|
#include "preamble.h"
|
|
#include "freq_est.h"
|
|
#include "whitening.h"
|
|
|
|
#include "config.h"
|
|
|
|
#include "rx.h"
|
|
|
|
#include "utils.h"
|
|
|
|
#define HEADER_SIZE_BYTES 4
|
|
#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
|
|
|
|
static void update_nco_pll(nco_crcf nco, float phase_error)
|
|
{
|
|
static const float pll_alpha = 0.03f; // phase adjustment factor
|
|
static const float pll_beta = (pll_alpha * pll_alpha) / 2.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)
|
|
{
|
|
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;
|
|
return true; // preamble found!
|
|
} else {
|
|
// 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);
|
|
|
|
float freq_adjustment = freq_est / RRC_SPS;
|
|
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);
|
|
}
|
|
|
|
//freq_est_history_write_idx = 0;
|
|
|
|
if(freq_est != 0.0f) {
|
|
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
|
|
}
|
|
}
|
|
|
|
|
|
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[1 << 12];
|
|
|
|
float complex samples2dump[sample_count];
|
|
size_t nsamples2dump = 0;
|
|
float complex samples2dump2[sample_count];
|
|
size_t nsamples2dump2 = 0;
|
|
|
|
// 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);
|
|
|
|
// Mix the input signal with the carrier NCO, which oscillates at the
|
|
// frequency coarsly estimated so far.
|
|
float complex mixed_sample;
|
|
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)) {
|
|
// 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\n", sym_demod, phase_error);
|
|
|
|
|
|
update_nco_pll(rx->carrier_fine_nco, phase_error);
|
|
|
|
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));
|
|
|
|
fec_decode(rx->hdr_fec, sizeof(header), header_enc, header);
|
|
|
|
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;
|
|
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;
|
|
|
|
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);
|
|
|
|
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);
|
|
|
|
// 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;
|
|
}
|