foo

3 files changed, 35 insertions, 26 deletions

diff --git a/controller/fw/src/dsss_demod.c b/controller/fw/src/dsss_demod.c
index 32d6104..2f6af44 100644
--- a/controller/fw/src/dsss_demod.c
+++ b/controller/fw/src/dsss_demod.c
@@ -58,12 +58,10 @@ 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) {
-    bool debug = false;
-    /*
+    //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
@@ -88,24 +86,19 @@ void dsss_demod_step(struct dsss_demod_state *st, float new_value) {
     for (size_t i=0; i<DSSS_GOLD_CODE_COUNT; i++)
         cwt[i] = cwt_convolve_step(st->correlation[i], st->correlation_wpos);
 
-    debug_print_vector("cwt", DSSS_GOLD_CODE_COUNT, cwt, 1, false, debug);
-
-    float avg[DSSS_GOLD_CODE_COUNT];
+    float avg;
     for (size_t i=0; i<DSSS_GOLD_CODE_COUNT; i++)
-        avg[i] = run_iir(fabs(cwt[i]), ARRAY_LENGTH(cwt_filter_bq), cwt_filter_bq, st->cwt_filter[i].st);
-
-    debug_print_vector("avg", DSSS_GOLD_CODE_COUNT, avg, 1, false, debug);
-
-    if (record_channel != -1)
-        DEBUG_PRINTN("%f, %f, %f\n",
-                st->correlation[record_channel][st->correlation_wpos], cwt[record_channel], avg[record_channel]);
+        avg += fabs(cwt[i]);
+    avg /= (float)DSSS_GOLD_CODE_COUNT;
+    /* FIXME fix this filter */
+    //avg = run_iir(avg, ARRAY_LENGTH(cwt_filter_bq), cwt_filter_bq, st->cwt_filter.st);
 
     float max_val = st->group.max;
     int max_ch = st->group.max_ch;
     int max_ts = st->group.max_ts;
     bool found = false;
     for (size_t i=0; i<DSSS_GOLD_CODE_COUNT; i++) {
-        float val = cwt[i] / avg[i];
+        float val = cwt[i] / avg;
 
         if (fabs(val) > fabs(max_val)) {
             max_val = val;
@@ -171,6 +164,7 @@ void matcher_tick(struct matcher_state states[static DSSS_MATCHER_CACHE_SIZE], u
     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;
+    const int max_skips = TRANSMISSION_SYMBOLS/4*3;
 
     for (size_t i=0; i<DSSS_MATCHER_CACHE_SIZE; i++) {
         if (states[i].last_phase == -1)
@@ -184,6 +178,7 @@ void matcher_tick(struct matcher_state states[static DSSS_MATCHER_CACHE_SIZE], u
                 states[i].candidate_score = score;
                 states[i].candidate_phase = current_phase;
                 states[i].candidate_data = decode_peak(peak_ch, peak_ampl);
+                states[i].candidate_skips = 1;
             }
         }
 
@@ -199,7 +194,15 @@ void matcher_tick(struct matcher_state states[static DSSS_MATCHER_CACHE_SIZE], u
             states[i].last_score = score_depreciation * states[i].last_score +
                 (1.0f - score_depreciation) * states[i].candidate_score;
             states[i].candidate_score = 0.0f;
+            states[i].last_skips += states[i].candidate_skips;
+            DEBUG_PRINT("skips %d", states[i].last_skips);
+
+            /*
+            if (states[i].last_skips > max_skips) {
+                states[i].last_phase = -1; / invalidate entry /
 
+            } else
+                */
             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);
@@ -223,8 +226,6 @@ static float score_group(const struct group *g, int phase_delta) {
 }
 
 void group_received(struct dsss_demod_state *st, uint64_t ts) {
-    static_assert(group_phase_tolerance > 10); /* FIXME debug, remove */
-
     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);
@@ -233,8 +234,14 @@ void group_received(struct dsss_demod_state *st, uint64_t ts) {
     ssize_t min_idx = -1;
     ssize_t empty_idx = -1;
     for (size_t i=0; i<DSSS_MATCHER_CACHE_SIZE; i++) {
+
+        /* Search for empty entries */
+        if (st->matcher_cache[i].last_phase == -1) {
+            empty_idx = i;
+            continue;
+        }
+
         /* 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) {
@@ -244,13 +251,10 @@ void group_received(struct dsss_demod_state *st, uint64_t ts) {
                 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);
+                st->matcher_cache[i].candidate_skips = 0;
             }
         }
 
-        /* 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) {
@@ -267,17 +271,19 @@ void group_received(struct dsss_demod_state *st, uint64_t ts) {
         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;
-    }
+        st->matcher_cache[empty_idx].candidate_skips = 0;
+        st->matcher_cache[empty_idx].last_skips = 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);
+    } else if (min_score < base_score && 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;
+        st->matcher_cache[min_idx].candidate_skips = 0;
+        st->matcher_cache[min_idx].last_skips = 0;
     }
 }
 
diff --git a/controller/fw/src/dsss_demod.h b/controller/fw/src/dsss_demod.h
index b9e75d2..2e8c003 100644
--- a/controller/fw/src/dsss_demod.h
+++ b/controller/fw/src/dsss_demod.h
@@ -37,6 +37,9 @@ struct matcher_state {
     float last_score;
     float candidate_score;
 
+    int last_skips;
+    int candidate_skips;
+
 #if DSSS_GOLD_CODE_NBITS > 7
 #error DSSS_GOLD_CODE_NBITS is too large for matcher_state.data data type (uint8_t)
 #endif
@@ -52,7 +55,7 @@ struct dsss_demod_state {
     float correlation[DSSS_GOLD_CODE_COUNT][DSSS_WAVELET_LUT_SIZE];
     size_t correlation_wpos;
 
-    struct cwt_iir_filter_state cwt_filter[DSSS_GOLD_CODE_COUNT];
+    struct cwt_iir_filter_state cwt_filter;
 
     struct group group;
 
diff --git a/controller/fw/tools/dsss_demod_test.c b/controller/fw/tools/dsss_demod_test.c
index d76a8bd..fb08fb3 100644
--- a/controller/fw/tools/dsss_demod_test.c
+++ b/controller/fw/tools/dsss_demod_test.c
@@ -40,7 +40,7 @@ int main(int argc, char **argv) {
         return 2;
     }
 
-    if (st.st_size < 0 || st.st_size > 1000000) {
+    if (st.st_size < 0 || st.st_size > 10000000) {
         fprintf(stderr, "Error reading test data: too much test data (size=%zd)\n", st.st_size);
         return 2;
     }