WIP DSSS decoding

1 files changed, 34 insertions, 27 deletions

diff --git a/controller/fw/src/dsss_demod.c b/controller/fw/src/dsss_demod.c
index ec54d41..7a2e0cc 100644
--- a/controller/fw/src/dsss_demod.c
+++ b/controller/fw/src/dsss_demod.c
@@ -11,17 +11,14 @@
 #include "simulation.h"
 
 #include "generated/dsss_gold_code.h"
+#include "generated/dsss_butter_filter.h"
 
 /* Generated CWT wavelet LUT */
 extern const float * const dsss_cwt_wavelet_table;
 
-struct iir_biquad cwt_filter_bq[3] = {
-    {.a = {-1.993939440f, 0.993949280f}, .b = {0.2459934300683e-5, 0.4919868601367e-5, 0.2459934300683e-5}},
-    {.a = {-1.995557124f, 0.995566972f}, .b = {0.2461930046414e-5, 0.4923860092828e-5, 0.2461930046414e-5}},
-    {.a = {-1.998365254f, 0.998375115f}, .b = {0.2465394452097e-5, 0.4930788904195e-5, 0.2465394452097e-5}},
-};
+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);
+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);
@@ -34,7 +31,7 @@ void dsss_demod_step(struct dsss_demod_state *st, float new_value) {
 
 //#define DEBUG_PRINT(...) ((void)0)
 //#define DEBUG_PRINTN(...) ((void)0)
-    bool debug = sim_pos % DSSS_CORRELATION_LENGTH == 0;
+    bool debug = sim_pos % DSSS_CORRELATION_LENGTH == DSSS_CORRELATION_LENGTH-1;
     if (debug) DEBUG_PRINT("Iteration %zd", sim_pos);
     //const float peak_group_threshold = 0.05 * DSSS_CORRELATION_LENGTH;
     //const float hole_patching_threshold = 0.01 * DSSS_CORRELATION_LENGTH;
@@ -45,14 +42,14 @@ void dsss_demod_step(struct dsss_demod_state *st, float new_value) {
 
     /* use new, incremented wpos for gold_correlate_step as first element of old data in ring buffer */
     for (size_t i=0; i<DSSS_GOLD_CODE_COUNT; i++)
-        st->correlation[i][st->correlation_wpos] = gold_correlate_step(i, st->signal, st->correlation_wpos);
+        st->correlation[i][st->correlation_wpos] = gold_correlate_step(i, st->signal, st->signal_wpos, debug);
     /* debug */
-    /*
-    DEBUG_PRINTN("    correlation: [");
-    for (size_t i=0; i<DSSS_GOLD_CODE_COUNT; i++)
-        DEBUG_PRINTN("%f, ", st->correlation[i][st->correlation_wpos]);
-    DEBUG_PRINTN("]\n");
-    */
+    if (debug) {
+        DEBUG_PRINTN("    correlation: [");
+        for (size_t i=0; i<DSSS_GOLD_CODE_COUNT; i++)
+            DEBUG_PRINTN("%f, ", st->correlation[i][st->correlation_wpos]);
+        DEBUG_PRINTN("]\n");
+    }
     /* end */
     st->correlation_wpos = (st->correlation_wpos + 1) % ARRAY_LENGTH(st->correlation[0]);
 
@@ -60,10 +57,12 @@ 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 */
+    /*
     if (debug) DEBUG_PRINTN("    cwt: [");
     for (size_t i=0; i<DSSS_GOLD_CODE_COUNT; i++)
         if (debug) DEBUG_PRINTN("%f, ", cwt[i]);
     if (debug) DEBUG_PRINTN("]\n");
+    */
     /* end */
 
     float avg[DSSS_GOLD_CODE_COUNT];
@@ -82,10 +81,10 @@ void dsss_demod_step(struct dsss_demod_state *st, float new_value) {
     int max_ch = st->group.max_ch;
     int max_idx = st->group.max_idx + 1;
     bool found = false;
-    if (debug) DEBUG_PRINTN("    rel: [");
+    //if (debug) DEBUG_PRINTN("    rel: [");
     for (size_t i=0; i<DSSS_GOLD_CODE_COUNT; i++) {
         float val = cwt[i] / avg[i];
-        if (debug) DEBUG_PRINTN("%f, ", val);
+        //if (debug) DEBUG_PRINTN("%f, ", val);
 
         if (fabs(val) > DSSS_THESHOLD_FACTOR)
             found = true;
@@ -96,7 +95,7 @@ void dsss_demod_step(struct dsss_demod_state *st, float new_value) {
             max_idx = st->group.len;
         }
     }
-    if (debug) DEBUG_PRINTN("]\n");
+    //if (debug) DEBUG_PRINTN("]\n");
 
     /* debug */
     if (debug) DEBUG_PRINT("    found=%d len=%d idx=%d ch=%d max=%f",
@@ -162,19 +161,27 @@ float cwt_convolve_step(const float v[DSSS_WAVELET_LUT_SIZE], size_t offx) {
  *
  * [0] https://docs.scipy.org/doc/numpy/reference/generated/numpy.correlate.html
  */
-float gold_correlate_step(const size_t ncode, const float a[DSSS_CORRELATION_LENGTH], size_t offx) {
+float gold_correlate_step(const size_t ncode, const float a[DSSS_CORRELATION_LENGTH], size_t offx, bool debug) {
+
     float acc_outer = 0.0f;
     uint8_t table_byte = 0;
-    for (size_t i=0, pos=0; i<DSSS_GOLD_CODE_LENGTH; i++, pos += DSSS_DECIMATION) {
-        float acc_inner = 0.0f;
-        for (size_t j=0; j<DSSS_DECIMATION; j++) {
-            if ((pos&7) == 0)
-                table_byte = dsss_gold_code_table[ncode][pos>>3]; /* Fetch sequence table item */
-            int bv = table_byte & (1<<(pos&7)); /* Extract bit */
-            bv = !!bv*2 - 1; /* Map 0, 1 -> -1, 1 */
-            acc_inner += a[(offx + i + j) % DSSS_CORRELATION_LENGTH] * bv; /* Multiply item */
+    if (debug) DEBUG_PRINTN("Correlate n=%d: ", ncode);
+    for (size_t i=0; i<DSSS_GOLD_CODE_LENGTH; i++) {
+
+        if ((i&7) == 0) {
+            table_byte = dsss_gold_code_table[ncode][i>>3]; /* Fetch sequence table item */
+            if (debug) DEBUG_PRINTN("|");
         }
-        acc_outer += acc_inner;
+        int bv = table_byte & (0x80>>(i&7)); /* Extract bit */
+        bv = !!bv*2 - 1; /* Map 0, 1 -> -1, 1 */
+        if (debug) DEBUG_PRINTN("%s%d\033[0m", bv == 1 ? "\033[92m" : "\033[91m", (bv+1)/2);
+
+        float acc_inner = 0.0f;
+        for (size_t j=0; j<DSSS_DECIMATION; j++)
+            acc_inner += a[(offx + i*DSSS_DECIMATION + j) % DSSS_CORRELATION_LENGTH]; /* Multiply item */
+        //if (debug) DEBUG_PRINTN("%.2f ", acc_inner);
+        acc_outer += acc_inner * bv;
     }
+    if (debug) DEBUG_PRINTN("\n");
     return acc_outer / DSSS_CORRELATION_LENGTH;
 }