WIP DSSS decoding

6 files changed, 94 insertions, 35 deletions

diff --git a/controller/fw/Makefile b/controller/fw/Makefile
index cc7d156..6c5d5ea 100644
--- a/controller/fw/Makefile
+++ b/controller/fw/Makefile
@@ -46,12 +46,16 @@ FMEAS_ADC_MAX			?= 4096
 FMEAS_FFT_LEN			?= 256
 FMEAS_FFT_WINDOW 		?= gaussian
 FMEAS_FFT_WINDOW_SIGMA 	?= 16.0
+# TODO: validate
+FMEAS_SAMPLING_RATE		?= 10.0
 
 DSSS_GOLD_CODE_NBITS 	?= 5
 DSSS_DECIMATION			?= 10
 DSSS_THESHOLD_FACTOR	?= 4.0f
 DSSS_WAVELET_WIDTH		?= 7.3
 DSSS_WAVELET_LUT_SIZE	?= 69
+DSSS_FILTER_FC			?= 20e-3
+DSSS_FILTER_ORDER		?= 8
 
 CC			:= $(PREFIX)gcc
 CXX			:= $(PREFIX)g++
@@ -122,7 +126,7 @@ LDFLAGS			+= -L$(OPENCM3_DIR_ABS)/lib -l$(OPENCM3_LIB) $(shell $(CC) -print-libg
 
 all: $(BUILDDIR)/$(BINARY)
 
-src/dsss_demod.c: $(BUILDDIR)/generated/dsss_gold_code.h
+src/dsss_demod.c: $(BUILDDIR)/generated/dsss_gold_code.h $(BUILDDIR)/generated/dsss_butter_filter.h
 
 $(BUILDDIR)/generated/dsss_gold_code.h: $(BUILDDIR)/generated/gold_code_$(DSSS_GOLD_CODE_NBITS).h
 	ln -srf $< $@
@@ -139,6 +143,10 @@ $(BUILDDIR)/generated/fmeas_fft_window.c: | $(BUILDDIR)/generated
 $(BUILDDIR)/generated/dsss_cwt_wavelet.c: | $(BUILDDIR)/generated
 	$(PYTHON3) tools/cwt_wavelet_header_gen.py -v dsss_cwt_wavelet_table $(DSSS_WAVELET_LUT_SIZE) $(DSSS_WAVELET_WIDTH) > $@
 
+.PRECIOUS: $(BUILDDIR)/generated/dsss_butter_filter.h
+$(BUILDDIR)/generated/dsss_butter_filter.h: | $(BUILDDIR)/generated
+	$(PYTHON3) tools/butter_filter_gen.py -m dsss_filter $(DSSS_FILTER_FC) $(FMEAS_SAMPLING_RATE) $(DSSS_FILTER_ORDER) > $@
+
 $(BUILDDIR)/generated: ; mkdir -p $@
 
 OBJS := $(addprefix $(BUILDDIR)/,$(C_SOURCES:.c=.o) $(CXX_SOURCES:.cpp=.o))
diff --git a/controller/fw/cmsis b/controller/fw/cmsis
-Subproject 6a86f4ca00d2b96b82879e1e5bd9e89c5750dd2
+Subproject 4a65d88011a1595b7c8b42fa0d70b7bdfc132ac
diff --git a/controller/fw/libsodium b/controller/fw/libsodium
-Subproject afae623190f025e7cf2fb0222bfe796b69a3694
+Subproject a0a8706c9dc9e43bc51d16334cd6c0f6ae084ce
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;
 }
diff --git a/controller/fw/tools/butter_filter_gen.py b/controller/fw/tools/butter_filter_gen.py
new file mode 100644
index 0000000..059dea0
--- /dev/null
+++ b/controller/fw/tools/butter_filter_gen.py
@@ -0,0 +1,50 @@
+#!/usr/bin/env python3
+
+import sys
+import contextlib
+
+import scipy.signal as sig
+
+
+@contextlib.contextmanager
+def wrap(left='{', right='}', file=None, end=''):
+    print(left, file=file, end=end)
+    yield
+    print(right, file=file, end=end)
+
+@contextlib.contextmanager
+def print_include_guards(macro_name):
+    print(f'#ifndef {macro_name}')
+    print(f'#define {macro_name}')
+    yield
+    print(f'#endif /* {macro_name} */')
+
+macro_float = lambda f: f'{f}'.replace('.', 'F').replace('-', 'N').replace('+', 'P')
+
+
+if __name__ == '__main__':
+    import argparse
+    parser = argparse.ArgumentParser()
+    parser.add_argument('-m', '--macro-name', default='butter_filter', help='Prefix for output macro names')
+    parser.add_argument('fc', type=float, help='Corner frequency [Hz]')
+    parser.add_argument('fs', type=float, help='Sampling rate [Hz]')
+    parser.add_argument('n', type=int, nargs='?', default=6, help='Filter order')
+    args = parser.parse_args()
+    
+    sos = sig.butter(args.n, args.fc, fs=args.fs, output='sos')
+
+    print('/* THIS IS A GENERATED FILE. DO NOT EDIT! */')
+    with print_include_guards(f'__BUTTER_FILTER_GENERATED_{args.n}_{macro_float(args.fc)}_{macro_float(args.fs)}__'):
+        print(f'#define {args.macro_name.upper()}_ORDER {args.n}')
+        print(f'#define {args.macro_name.upper()}_CLEN {(args.n+1)//2}')
+        print(f'#define {args.macro_name.upper()}_COEFF ', end='')
+        for sec in sos:
+            with wrap():
+                print('.b=', end='')
+                with wrap():
+                    print(', '.join(f'{v}' for v in sec[:3]), end='')
+                print(', .a=', end='')
+                with wrap():
+                    print(', '.join(f'{v}' for v in sec[4:6]), end='')
+            print(', ', end='')
+        print()
diff --git a/controller/fw/tools/gold_code_header_gen.py b/controller/fw/tools/gold_code_header_gen.py
index 8247457..fa98fce 100644
--- a/controller/fw/tools/gold_code_header_gen.py
+++ b/controller/fw/tools/gold_code_header_gen.py
@@ -58,19 +58,13 @@ if __name__ == '__main__':
         print(f' *')
         print(f' * Each code is packed left-aligned into {nbytes} bytes in big-endian byte order.')
         print(f' */')
-        print(f'const uint8_t gold_code_{args.n}bit[{2**args.n+1}][{nbytes}] = {{')
+        print(f'const uint8_t {args.variable}[{2**args.n+1}][{nbytes}] = {{')
         for i, code in enumerate(gold(args.n)):
             par = '{' + ' '.join(f'0x{d:02x},' for d in np.packbits(code)) + f'}}, /* {i: 3d} "{"".join(str(x) for x in code)}" */'
             print(textwrap.fill(par, initial_indent=' '*4, subsequent_indent=' '*4, width=120))
         print('};')
         print()
-        print(f'const uint8_t * const {args.variable} __attribute__((weak)) = (uint8_t *const)gold_code_{args.n}bit;')
-        print(f'const size_t {args.variable}_nbits __attribute__((weak)) = {args.n};')
     else:
         print('/* THIS IS A GENERATED FILE. DO NOT EDIT! */')
         with print_include_guards(f'__GOLD_CODE_GENERATED_HEADER_{args.n}__'):
-            print(f'extern const uint8_t gold_code_{args.n}bit[{2**args.n+1}][{nbytes}];')
-
-        with print_include_guards(f'__GOLD_CODE_GENERATED_HEADER_GLOBAL_SYM_{args.variable.upper()}__'):
             print(f'extern const uint8_t {args.variable}[{2**args.n+1}][{nbytes}];')
-            print(f'extern const size_t {args.variable}_nbits;')