diff options
Diffstat (limited to 'controller/fw')
| -rw-r--r-- | controller/fw/Makefile | 4 | ||||
| -rw-r--r-- | controller/fw/src/dsss_demod.c | 182 | ||||
| -rw-r--r-- | controller/fw/src/dsss_demod.h | 23 | ||||
| -rw-r--r-- | controller/fw/tools/dsss_demod_test.c | 12 |
4 files changed, 175 insertions, 46 deletions
diff --git a/controller/fw/Makefile b/controller/fw/Makefile index c549e47..fac9169 100644 --- a/controller/fw/Makefile +++ b/controller/fw/Makefile @@ -57,9 +57,9 @@ 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 +TRANSMISSION_SYMBOLS ?= 32 CC := $(PREFIX)gcc CXX := $(PREFIX)g++ @@ -105,8 +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) +COMMON_CFLAGS += -DTRANSMISSION_SYMBOLS=$(TRANSMISSION_SYMBOLS) # for musl CFLAGS += -Dhidden= diff --git a/controller/fw/src/dsss_demod.c b/controller/fw/src/dsss_demod.c index 1cad6f8..32d6104 100644 --- a/controller/fw/src/dsss_demod.c +++ b/controller/fw/src/dsss_demod.c @@ -2,6 +2,8 @@ #include <unistd.h> #include <stdbool.h> #include <math.h> +#include <stdlib.h> +#include <assert.h> #include <arm_math.h> @@ -18,11 +20,15 @@ extern const float * const dsss_cwt_wavelet_table; struct iir_biquad cwt_filter_bq[DSSS_FILTER_CLEN] = {DSSS_FILTER_COEFF}; -float gold_correlate_step(const size_t ncode, const float a[DSSS_CORRELATION_LENGTH], size_t offx, bool debug); -float cwt_convolve_step(const float v[DSSS_WAVELET_LUT_SIZE], size_t offx); -float run_iir(const float x, const int order, const struct iir_biquad q[order], struct iir_biquad_state st[order]); -float run_biquad(float x, const struct iir_biquad *const q, struct iir_biquad_state *const restrict st); void debug_print_vector(const char *name, size_t len, const float *data, size_t stride, bool index, bool debug); +static float gold_correlate_step(const size_t ncode, const float a[DSSS_CORRELATION_LENGTH], size_t offx, bool debug); +static float cwt_convolve_step(const float v[DSSS_WAVELET_LUT_SIZE], size_t offx); +static float run_iir(const float x, const int order, const struct iir_biquad q[order], struct iir_biquad_state st[order]); +static float run_biquad(float x, const struct iir_biquad *const q, struct iir_biquad_state *const restrict st); +static void matcher_init(struct matcher_state states[static DSSS_MATCHER_CACHE_SIZE]); +static void matcher_tick(struct matcher_state states[static DSSS_MATCHER_CACHE_SIZE], + uint64_t ts, int peak_ch, float peak_ampl); +static void group_received(struct dsss_demod_state *st, uint64_t ts); #ifdef SIMULATION void debug_print_vector(const char *name, size_t len, const float *data, size_t stride, bool index, bool debug) { @@ -45,11 +51,19 @@ void debug_print_vector(const char *name, size_t len, const float *data, size_t void debug_print_vector(const char *name, size_t len, const float *data, size_t stride, bool index, bool debug) {} #endif +void dsss_demod_init(struct dsss_demod_state *st) { + memset(st, 0, sizeof(*st)); + matcher_init(st->matcher_cache); +} + #ifdef SIMULATION void dsss_demod_step(struct dsss_demod_state *st, float new_value, uint64_t ts, int record_channel) { + bool debug = false; + /* bool debug = (record_channel == -1) && (ts > 1000) && (ts % DSSS_CORRELATION_LENGTH == DSSS_CORRELATION_LENGTH-1); + */ if (debug) DEBUG_PRINT("Iteration %zd: signal=%f", ts, new_value); #else @@ -93,16 +107,19 @@ void dsss_demod_step(struct dsss_demod_state *st, float new_value) { for (size_t i=0; i<DSSS_GOLD_CODE_COUNT; i++) { float val = cwt[i] / avg[i]; - if (fabs(val) > DSSS_THESHOLD_FACTOR) - found = true; - if (fabs(val) > fabs(max_val)) { max_val = val; max_ch = i; max_ts = ts; + + if (fabs(val) > DSSS_THESHOLD_FACTOR) + found = true; } } + /* FIXME: skipped sample handling here */ + matcher_tick(st->matcher_cache, ts, max_ch, max_val); + if (found) { /* Continue ongoing group */ st->group.len++; @@ -120,6 +137,7 @@ void dsss_demod_step(struct dsss_demod_state *st, float new_value) { if (record_channel == -1) DEBUG_PRINT("GROUP FOUND: %8d len=%3d max=%f ch=%d offx=%d", ts, st->group.len, st->group.max, st->group.max_ch, st->group.max_ts); + group_received(st, ts); /* reset grouping state */ st->group.len = 0; @@ -128,49 +146,139 @@ void dsss_demod_step(struct dsss_demod_state *st, float new_value) { 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 */ +/* Map a sequence match to a data symbol. This maps the sequence's index number to the 2nd to n+2nd bit of the result, + * and maps the polarity of detection to the LSb. 5-bit example: + * + * [0, S, S, S, S, S, S, P] ; S ^= symbol index (0 - 2^n+1), P ^= symbol polarity + * + * Symbol polarity is preserved from transmitter to receiver. The symbol index is n+1 bit instead of n bit since we have + * 2^n+1 symbols to express, one too many for an n-bit index. + */ +uint8_t decode_peak(int peak_ch, float peak_ampl) { + return (peak_ch<<1) | (peak_ampl > 0); } -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<cache_size; i++) { - /* If we find an empty or expired entry, use that */ - if (cache[i].max_ts == 0 || ts - cache[i].max_ts > group_cache_expiration) - return i; +void matcher_init(struct matcher_state states[static DSSS_MATCHER_CACHE_SIZE]) { + for (size_t i=0; i<DSSS_MATCHER_CACHE_SIZE; i++) + states[i].last_phase = -1; /* mark as inactive */ +} - /* Otherwise check weakest entry */ - float score = score_group(&cache[i]); - if (score < min_score) { - min_idx = i; - min_score = score; +/* TODO make these constants configurable from Makefile */ +const int group_phase_tolerance = (int)(DSSS_CORRELATION_LENGTH * 0.10); + +void matcher_tick(struct matcher_state states[static DSSS_MATCHER_CACHE_SIZE], uint64_t ts, int peak_ch, float peak_ampl) { + /* TODO make these constants configurable from Makefile */ + const float skip_sampling_depreciation = 0.2f; /* 0.0 -> no depreciation, 1.0 -> complete disregard */ + const float score_depreciation = 0.1f; /* 0.0 -> no depreciation, 1.0 -> complete disregard */ + const uint64_t current_phase = ts % DSSS_CORRELATION_LENGTH; + + for (size_t i=0; i<DSSS_MATCHER_CACHE_SIZE; i++) { + if (states[i].last_phase == -1) + continue; /* Inactive entry */ + + if (current_phase == states[i].last_phase) { + /* Skip sampling */ + float score = fabs(peak_ampl) * (1.0f - skip_sampling_depreciation); + if (score > states[i].candidate_score) { + /* We win, update candidate */ + states[i].candidate_score = score; + states[i].candidate_phase = current_phase; + states[i].candidate_data = decode_peak(peak_ch, peak_ampl); + } + } + + /* Note of caution on group_phase_tolerance: Group detection has some latency since a group is only considered + * "detected" after signal levels have fallen back below the detection threshold. This means we only get to + * process a group a couple ticks after its peak. We have to make sure the window is still open at this point. + * This means we have to match against group_phase_tolerance should a little bit loosely. + */ + if (abs(states[i].last_phase - current_phase) == group_phase_tolerance + DSSS_DECIMATION) { + /* Process window results */ + states[i].data[ states[i].data_pos ] = states[i].candidate_data; + states[i].data_pos = states[i].data_pos + 1; + states[i].last_score = score_depreciation * states[i].last_score + + (1.0f - score_depreciation) * states[i].candidate_score; + states[i].candidate_score = 0.0f; + + if (states[i].data_pos == TRANSMISSION_SYMBOLS) { + /* Frame received completely */ + DEBUG_PRINT("match on index %d phase %d score %.5f", i, states[i].last_phase, states[i].last_score); + handle_dsss_received(states[i].data); + states[i].last_phase = -1; /* invalidate entry */ + } } } +} + +static float gaussian(float a, float b, float c, float x) { + float n = x-b; + return a*expf(-n*n / (2.0f* c*c)); +} - /* Return weakest group if weaker than candidate */ - if (min_score < score_group(g)) - return min_idx; - return -1; +static float score_group(const struct group *g, int phase_delta) { + /* TODO make these constants configurable from Makefile */ + const float distance_func_phase_tolerance = 10.0f; + return fabsf(g->max) * gaussian(1.0f, 0.0f, distance_func_phase_tolerance, phase_delta); } 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; + static_assert(group_phase_tolerance > 10); /* FIXME debug, remove */ - /* 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; + const int group_phase = st->group.max_ts % DSSS_CORRELATION_LENGTH; + /* This is the score of a decoding starting at this group (with no context) */ + float base_score = score_group(&st->group, 0); - float mean_phase = 0.0; - for (size_t i=0; i<DSSS_GROUP_CACHE_SIZE; i++) - mean_phase += (st->group_cache[i].max_ts) % DSSS_CORRELATION_LENGTH; - mean_phase /= DSSS_GROUP_CACHE_SIZE; + float min_score = INFINITY; + ssize_t min_idx = -1; + ssize_t empty_idx = -1; + for (size_t i=0; i<DSSS_MATCHER_CACHE_SIZE; i++) { + /* Search for entries with matching phase */ + + /* This is the score of this group given the cached decoding at [i] */ + int phase_delta = st->matcher_cache[i].last_phase - group_phase; + if (abs(phase_delta) <= group_phase_tolerance) { + + float group_score = score_group(&st->group, phase_delta); + if (st->matcher_cache[i].candidate_score < group_score) { + st->matcher_cache[i].candidate_score = group_score; + st->matcher_cache[i].candidate_phase = group_phase; + st->matcher_cache[i].candidate_data = decode_peak(st->group.max_ch, st->group.max); + } + } - + /* Search for empty entries */ + if (st->matcher_cache[i].last_phase == -1) + empty_idx = i; + + /* Search for weakest entry */ + float score = st->matcher_cache[i].last_score; + if (score < min_score) { + min_idx = i; + min_score = score; + } + } + + /* If we found empty entries, replace one by a new decoding starting at this group */ + if (empty_idx >= 0) { + st->matcher_cache[empty_idx].last_phase = group_phase; + st->matcher_cache[empty_idx].candidate_score = base_score; + st->matcher_cache[empty_idx].last_score = base_score; + st->matcher_cache[empty_idx].candidate_phase = group_phase; + st->matcher_cache[empty_idx].candidate_data = decode_peak(st->group.max_ch, st->group.max); + st->matcher_cache[empty_idx].data_pos = 0; + } + + /* If the weakest decoding in cache is weaker than a new decoding starting here, replace it */ + if (min_score < base_score) { + assert(min_idx >= 0); + st->matcher_cache[min_idx].last_phase = group_phase; + st->matcher_cache[min_idx].candidate_score = base_score; + st->matcher_cache[min_idx].last_score = base_score; + st->matcher_cache[min_idx].candidate_phase = group_phase; + st->matcher_cache[min_idx].candidate_data = decode_peak(st->group.max_ch, st->group.max); + st->matcher_cache[min_idx].data_pos = 0; + } } float run_iir(const float x, const int order, const struct iir_biquad q[order], struct iir_biquad_state st[order]) { diff --git a/controller/fw/src/dsss_demod.h b/controller/fw/src/dsss_demod.h index 30ec3f7..b9e75d2 100644 --- a/controller/fw/src/dsss_demod.h +++ b/controller/fw/src/dsss_demod.h @@ -5,6 +5,9 @@ #define DSSS_GOLD_CODE_COUNT ((1<<DSSS_GOLD_CODE_NBITS) + 1) #define DSSS_CORRELATION_LENGTH (DSSS_GOLD_CODE_LENGTH * DSSS_DECIMATION) +/* FIXME: move to makefile */ +#define DSSS_MATCHER_CACHE_SIZE 8 +/* FIXME: move to more appropriate header */ #define PAYLOAD_DATA_BYTE ((PAYLOAD_DATA_BIT+7)/8) struct iir_biquad { @@ -20,22 +23,26 @@ struct cwt_iir_filter_state { struct iir_biquad_state st[3]; }; -struct { +struct group { int len; /* length of group in samples */ float max; /* signed value of largest peak in group on any channel */ uint64_t max_ts; /* absolute position of above peak */ int max_ch; /* channel (gold sequence index) of above peak */ -} group; +}; -struct decoder_state { - int last_phase; +struct matcher_state { + int last_phase; /* 0 .. DSSS_CORRELATION_LENGTH */ int candidate_phase; float last_score; float candidate_score; - uint8_t data[PAYLOAD_DATA_BYTE]; +#if DSSS_GOLD_CODE_NBITS > 7 +#error DSSS_GOLD_CODE_NBITS is too large for matcher_state.data data type (uint8_t) +#endif + uint8_t data[TRANSMISSION_SYMBOLS]; int data_pos; + uint8_t candidate_data; }; struct dsss_demod_state { @@ -49,9 +56,13 @@ struct dsss_demod_state { struct group group; - struct group group_cache[DSSS_GROUP_CACHE_SIZE]; + struct matcher_state matcher_cache[DSSS_MATCHER_CACHE_SIZE]; }; + +extern void handle_dsss_received(uint8_t data[TRANSMISSION_SYMBOLS]); + +void dsss_demod_init(struct dsss_demod_state *st); #ifdef SIMULATION void dsss_demod_step(struct dsss_demod_state *st, float new_value, uint64_t ts, int record_channel); #else /* SIMULATION */ diff --git a/controller/fw/tools/dsss_demod_test.c b/controller/fw/tools/dsss_demod_test.c index 4370f80..d76a8bd 100644 --- a/controller/fw/tools/dsss_demod_test.c +++ b/controller/fw/tools/dsss_demod_test.c @@ -12,6 +12,16 @@ #include "dsss_demod.h" +void handle_dsss_received(uint8_t data[TRANSMISSION_SYMBOLS]) { + printf("data sequence received: [ "); + for (size_t i=0; i<TRANSMISSION_SYMBOLS; i++) { + printf("%+3d", ((data[i]&1) ? 1 : -1) * (data[i]>>1)); + if (i+1 < TRANSMISSION_SYMBOLS) + printf(", "); + } + printf(" ]\n"); +} + void print_usage() { fprintf(stderr, "Usage: dsss_demod_test [test_data.bin] [optional recording channel number]\n"); } @@ -87,7 +97,7 @@ int main(int argc, char **argv) { fprintf(stderr, "Starting simulation.\n"); struct dsss_demod_state demod; - memset(&demod, 0, sizeof(demod)); + dsss_demod_init(&demod); for (size_t i=0; i<n_samples; i++) { //fprintf(stderr, "Iteration %zd/%zd\n", i, n_samples); dsss_demod_step(&demod, buf_f[i], i, record_channel); |