Improve detector

1 files changed, 38 insertions, 11 deletions

diff --git a/controller/fw/src/dsss_demod.c b/controller/fw/src/dsss_demod.c
index 0758525..f9efda6 100644
--- a/controller/fw/src/dsss_demod.c
+++ b/controller/fw/src/dsss_demod.c
@@ -23,10 +23,8 @@ struct iir_biquad cwt_filter_bq[DSSS_FILTER_CLEN] = {DSSS_FILTER_COEFF};
 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);
-#if 0
 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);
-#endif
 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);
@@ -41,7 +39,7 @@ void debug_print_vector(const char *name, size_t len, const float *data, size_t
     if (index) {
         DEBUG_PRINTN("    %16s  [", "");
         for (size_t i=0; i<len; i++)
-            DEBUG_PRINTN("%8zd  ", i);
+            DEBUG_PRINTN("%8zu  ", i);
         DEBUG_PRINTN("]\n");
     }
 
@@ -62,6 +60,8 @@ void dsss_demod_init(struct dsss_demod_state *st) {
 
 void dsss_demod_step(struct dsss_demod_state *st, float new_value, uint64_t ts) {
     //const float hole_patching_threshold = 0.01 * DSSS_CORRELATION_LENGTH;
+    bool log = false;
+    bool log_groups = true;
 
     st->signal[st->signal_wpos] = new_value;
     st->signal_wpos = (st->signal_wpos + 1) % ARRAY_LENGTH(st->signal);
@@ -80,6 +80,7 @@ void dsss_demod_step(struct dsss_demod_state *st, float new_value, uint64_t ts)
     for (size_t i=0; i<DSSS_GOLD_CODE_COUNT; i++)
         avg += fabsf(cwt[i]);
     avg /= (float)DSSS_GOLD_CODE_COUNT;
+    if (log) DEBUG_PRINTN("%6zu: %f ", ts, avg);
     /* FIXME fix this filter */
     //avg = run_iir(avg, ARRAY_LENGTH(cwt_filter_bq), cwt_filter_bq, st->cwt_filter.st);
 
@@ -89,18 +90,21 @@ void dsss_demod_step(struct dsss_demod_state *st, float new_value, uint64_t ts)
     bool found = false;
     for (size_t i=0; i<DSSS_GOLD_CODE_COUNT; i++) {
         float val = cwt[i] / avg;
+        if (log) DEBUG_PRINTN("%f ", cwt[i]);
+        if (log) DEBUG_PRINTN("%f ", val);
 
         if (fabsf(val) > fabsf(max_val)) {
             max_val = val;
             max_ch = i;
             max_ts = ts;
-
-            if (fabsf(val) > DSSS_THESHOLD_FACTOR)
-                found = true;
         }
+
+        if (fabsf(val) > DSSS_THESHOLD_FACTOR)
+            found = true;
     }
+    if (log) DEBUG_PRINTN("%f %d ", max_val, found);
+    if (log) DEBUG_PRINTN("\n");
 
-    /* FIXME: skipped sample handling here */
     matcher_tick(st->matcher_cache, ts, max_ch, max_val);
 
     if (found) {
@@ -116,6 +120,7 @@ void dsss_demod_step(struct dsss_demod_state *st, float new_value, uint64_t ts)
         /* We're between groups */
         return;
 
+    if (log_groups) DEBUG_PRINTN("GROUP: %zu %d %f\n", st->group.max_ts, st->group.max_ch, st->group.max);
     /* A group ended. Process result. */
     group_received(st);
 
@@ -152,7 +157,9 @@ void matcher_tick(struct matcher_state states[static DSSS_MATCHER_CACHE_SIZE], u
     const float score_depreciation = 0.1f; /* 0.0 -> no depreciation, 1.0 -> complete disregard */
     const int current_phase = ts % DSSS_CORRELATION_LENGTH;
     const int max_skips = TRANSMISSION_SYMBOLS/4*3;
+    bool debug = true;
 
+    bool header_printed = false;
     for (size_t i=0; i<DSSS_MATCHER_CACHE_SIZE; i++) {
         if (states[i].last_phase == -1)
             continue; /* Inactive entry */
@@ -161,6 +168,11 @@ void matcher_tick(struct matcher_state states[static DSSS_MATCHER_CACHE_SIZE], u
             /* Skip sampling */
             float score = fabsf(peak_ampl) * (1.0f - skip_sampling_depreciation);
             if (score > states[i].candidate_score) {
+                if (debug && !header_printed) {
+                    header_printed = true;
+                    DEBUG_PRINTN("windows %zu\n", ts);
+                }
+                if (debug) DEBUG_PRINTN("    skip %d old=%f new=%f\n", i, states[i].candidate_score, score);
                 /* We win, update candidate */
                 assert(i < DSSS_MATCHER_CACHE_SIZE);
                 states[i].candidate_score = score;
@@ -175,7 +187,15 @@ void matcher_tick(struct matcher_state states[static DSSS_MATCHER_CACHE_SIZE], u
          * 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) {
+        int phase_delta = current_phase - states[i].last_phase;
+        if (phase_delta < 0)
+            phase_delta += DSSS_CORRELATION_LENGTH;
+        if (phase_delta == group_phase_tolerance + DSSS_DECIMATION) {
+            if (debug && !header_printed) {
+                header_printed = true;
+                DEBUG_PRINTN("windows %zu\n", ts);
+            }
+            if (debug) DEBUG_PRINTN("    %d ", i);
             /* Process window results */
             assert(i < DSSS_MATCHER_CACHE_SIZE);
             assert(0 <= states[i].data_pos && states[i].data_pos < TRANSMISSION_SYMBOLS);
@@ -183,17 +203,21 @@ void matcher_tick(struct matcher_state states[static DSSS_MATCHER_CACHE_SIZE], u
             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;
+            if (debug) DEBUG_PRINTN("commit pos=%d val=%d score=%f ", states[i].data_pos, states[i].candidate_data, states[i].last_score);
             states[i].candidate_score = 0.0f;
             states[i].last_skips += states[i].candidate_skips;
 
             if (states[i].last_skips > max_skips) {
+                if (debug) DEBUG_PRINTN("expire ");
                 states[i].last_phase = -1; /* invalidate entry */
 
             } else if (states[i].data_pos == TRANSMISSION_SYMBOLS) {
+                if (debug) DEBUG_PRINTN("match ");
                 /* Frame received completely */
                 handle_dsss_received(states[i].data);
                 states[i].last_phase = -1; /* invalidate entry */
             }
+            if (debug) DEBUG_PRINTN("\n");
         }
     }
 }
@@ -211,6 +235,7 @@ static float score_group(const struct group *g, int phase_delta) {
 }
 
 void group_received(struct dsss_demod_state *st) {
+    bool debug = true;
     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);
@@ -234,6 +259,7 @@ void group_received(struct dsss_demod_state *st) {
             float group_score = score_group(&st->group, phase_delta);
             if (st->matcher_cache[i].candidate_score < group_score) {
                 assert(i < DSSS_MATCHER_CACHE_SIZE);
+                if (debug) DEBUG_PRINTN("    appending %zu %d score=%f pd=%d\n", i, decode_peak(st->group.max_ch, st->group.max), group_score, phase_delta);
                 /* Append to entry */
                 st->matcher_cache[i].candidate_score = group_score;
                 st->matcher_cache[i].candidate_phase = group_phase;
@@ -243,7 +269,8 @@ void group_received(struct dsss_demod_state *st) {
         }
 
         /* Search for weakest entry */
-        float score = st->matcher_cache[i].last_score;
+        /* TODO figure out this fitness function */
+        float score = st->matcher_cache[i].last_score * (1.5f + 1.0f/st->matcher_cache[i].data_pos);
         if (score < min_score) {
             min_idx = i;
             min_score = score;
@@ -252,6 +279,7 @@ void group_received(struct dsss_demod_state *st) {
 
     /* If we found empty entries, replace one by a new decoding starting at this group */
     if (empty_idx >= 0) {
+        if (debug) DEBUG_PRINTN("    empty %zd %d\n", empty_idx, decode_peak(st->group.max_ch, st->group.max));
         assert(0 <= empty_idx && empty_idx < DSSS_MATCHER_CACHE_SIZE);
         st->matcher_cache[empty_idx].last_phase = group_phase;
         st->matcher_cache[empty_idx].candidate_score = base_score;
@@ -264,6 +292,7 @@ void group_received(struct dsss_demod_state *st) {
 
     /* If the weakest decoding in cache is weaker than a new decoding starting here, replace it */
     } else if (min_score < base_score && min_idx >= 0) {
+        if (debug) DEBUG_PRINTN("    min %zd %d\n", min_idx, decode_peak(st->group.max_ch, st->group.max));
         assert(0 <= min_idx && min_idx < DSSS_MATCHER_CACHE_SIZE);
         st->matcher_cache[min_idx].last_phase = group_phase;
         st->matcher_cache[min_idx].candidate_score = base_score;
@@ -276,7 +305,6 @@ void group_received(struct dsss_demod_state *st) {
     }
 }
 
-#if 0
 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++)
@@ -292,7 +320,6 @@ float run_biquad(float x, const struct iir_biquad *const q, struct iir_biquad_st
     st->reg[0] = intermediate;
     return out;
 }
-#endif
 
 float cwt_convolve_step(const float v[DSSS_WAVELET_LUT_SIZE], size_t offx) {
     float sum = 0.0f;