2 files changed, 162 insertions, 43 deletions

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