From b4d5293d045d68822ad5b2d0a7a5f392c596a0ba Mon Sep 17 00:00:00 2001 From: jaseg Date: Mon, 9 Mar 2020 10:17:14 +0100 Subject: decoding WIP --- controller/fw/Makefile | 7 +++- controller/fw/src/dsss_demod.c | 64 +++++++++++++++++++++++++++----- controller/fw/src/dsss_demod.h | 33 ++++++++++++---- controller/fw/tools/butter_filter_gen.py | 35 +++++++++++++++-- 4 files changed, 117 insertions(+), 22 deletions(-) (limited to 'controller/fw') diff --git a/controller/fw/Makefile b/controller/fw/Makefile index 3b9cca6..c549e47 100644 --- a/controller/fw/Makefile +++ b/controller/fw/Makefile @@ -52,11 +52,14 @@ FMEAS_SAMPLING_RATE ?= 10.0 DSSS_GOLD_CODE_NBITS ?= 5 DSSS_DECIMATION ?= 10 # TODO maybe auto adjust this based on detection rate? -DSSS_THESHOLD_FACTOR ?= 5.0f +DSSS_THESHOLD_FACTOR ?= 6.0f DSSS_WAVELET_WIDTH ?= 7.3 DSSS_WAVELET_LUT_SIZE ?= 69 DSSS_FILTER_FC ?= 3e-3 DSSS_FILTER_ORDER ?= 12 +DSSS_GROUP_CACHE_SIZE ?= 12 + +PAYLOAD_DATA_BIT ?= 64 CC := $(PREFIX)gcc CXX := $(PREFIX)g++ @@ -102,6 +105,8 @@ COMMON_CFLAGS += -DDSSS_DECIMATION=$(DSSS_DECIMATION) COMMON_CFLAGS += -DDSSS_THESHOLD_FACTOR=$(DSSS_THESHOLD_FACTOR) COMMON_CFLAGS += -DDSSS_WAVELET_WIDTH=$(DSSS_WAVELET_WIDTH) COMMON_CFLAGS += -DDSSS_WAVELET_LUT_SIZE=$(DSSS_WAVELET_LUT_SIZE) +COMMON_CFLAGS += -DDSSS_GROUP_CACHE_SIZE=$(DSSS_GROUP_CACHE_SIZE) +COMMON_CFLAGS += -DPAYLOAD_DATA_BIT=$(PAYLOAD_DATA_BIT) # for musl CFLAGS += -Dhidden= diff --git a/controller/fw/src/dsss_demod.c b/controller/fw/src/dsss_demod.c index cc1c34e..1cad6f8 100644 --- a/controller/fw/src/dsss_demod.c +++ b/controller/fw/src/dsss_demod.c @@ -46,17 +46,16 @@ void debug_print_vector(const char *name, size_t len, const float *data, size_t #endif #ifdef SIMULATION -void dsss_demod_step(struct dsss_demod_state *st, float new_value, size_t sim_pos, int record_channel) { +void dsss_demod_step(struct dsss_demod_state *st, float new_value, uint64_t ts, int record_channel) { bool debug = (record_channel == -1) - && (sim_pos > 1000) - && (sim_pos % DSSS_CORRELATION_LENGTH == DSSS_CORRELATION_LENGTH-1); + && (ts > 1000) + && (ts % DSSS_CORRELATION_LENGTH == DSSS_CORRELATION_LENGTH-1); - if (debug) DEBUG_PRINT("Iteration %zd: signal=%f", sim_pos, new_value); + if (debug) DEBUG_PRINT("Iteration %zd: signal=%f", ts, new_value); #else void dsss_demod_step(struct dsss_demod_state *st, float new_value) { #endif - //const float peak_group_threshold = 0.05 * DSSS_CORRELATION_LENGTH; //const float hole_patching_threshold = 0.01 * DSSS_CORRELATION_LENGTH; st->signal[st->signal_wpos] = new_value; @@ -89,7 +88,7 @@ void dsss_demod_step(struct dsss_demod_state *st, float new_value) { float max_val = st->group.max; int max_ch = st->group.max_ch; - int max_idx = st->group.max_idx + 1; + int max_ts = st->group.max_ts; bool found = false; for (size_t i=0; i fabs(max_val)) { max_val = val; max_ch = i; - max_idx = st->group.len; + max_ts = ts; } } @@ -109,7 +108,7 @@ void dsss_demod_step(struct dsss_demod_state *st, float new_value) { st->group.len++; st->group.max = max_val; st->group.max_ch = max_ch; - st->group.max_idx = max_idx; + st->group.max_ts = max_ts; return; } @@ -120,15 +119,60 @@ void dsss_demod_step(struct dsss_demod_state *st, float new_value) { /* A group ended. Process result. */ if (record_channel == -1) DEBUG_PRINT("GROUP FOUND: %8d len=%3d max=%f ch=%d offx=%d", - sim_pos, st->group.len, st->group.max, st->group.max_ch, st->group.max_idx); + ts, st->group.len, st->group.max, st->group.max_ch, st->group.max_ts); /* reset grouping state */ st->group.len = 0; - st->group.max_idx = 0; + st->group.max_ts = 0; st->group.max_ch = 0; st->group.max = 0.0f; } +float score_group(const struct group *g, uint64_t ts) { + return fabs(g->max); /* Possibly at time penalty 1/(ts-max_ts) later */ +} + +ssize_t group_cache_insertion_index(const struct group *g, const struct group *cache, size_t cache_size, uint64_t ts) { + float min_score = INFINITY; + ssize_t min_idx = -1; + for (size_t i=0; i group_cache_expiration) + return i; + + /* Otherwise check weakest entry */ + float score = score_group(&cache[i]); + if (score < min_score) { + min_idx = i; + min_score = score; + } + } + + /* Return weakest group if weaker than candidate */ + if (min_score < score_group(g)) + return min_idx; + + return -1; +} + +void group_received(struct dsss_demod_state *st, uint64_t ts) { + /* TODO make these constants configurable from Makefile */ + const uint64_t group_cache_expiration = DSSS_CORRELATION_LENGTH * DSSS_GROUP_CACHE_SIZE; + + /* Insert into group cache if space is available or there is a weaker entry to replace */ + ssize_t found = group_cache_insertion_index(&st->group, st->group_cache, DSSS_GROUP_CACHE_SIZE); + if (!found) + return; /* Nothing changed */ + st->group_cache[found] = st->group; + + float mean_phase = 0.0; + for (size_t i=0; igroup_cache[i].max_ts) % DSSS_CORRELATION_LENGTH; + mean_phase /= DSSS_GROUP_CACHE_SIZE; + + +} + float run_iir(const float x, const int order, const struct iir_biquad q[order], struct iir_biquad_state st[order]) { float intermediate = x; for (int i=0; i<(order+1)/2; i++) diff --git a/controller/fw/src/dsss_demod.h b/controller/fw/src/dsss_demod.h index b6947bc..30ec3f7 100644 --- a/controller/fw/src/dsss_demod.h +++ b/controller/fw/src/dsss_demod.h @@ -5,6 +5,8 @@ #define DSSS_GOLD_CODE_COUNT ((1<