Correlator seems to be working

5 files changed, 33 insertions, 18 deletions

diff --git a/controller/fw/Makefile b/controller/fw/Makefile
index 6c5d5ea..2c75bbf 100644
--- a/controller/fw/Makefile
+++ b/controller/fw/Makefile
@@ -51,11 +51,11 @@ FMEAS_SAMPLING_RATE		?= 10.0
 
 DSSS_GOLD_CODE_NBITS 	?= 5
 DSSS_DECIMATION			?= 10
-DSSS_THESHOLD_FACTOR	?= 4.0f
+DSSS_THESHOLD_FACTOR	?= 5.0f
 DSSS_WAVELET_WIDTH		?= 7.3
 DSSS_WAVELET_LUT_SIZE	?= 69
-DSSS_FILTER_FC			?= 20e-3
-DSSS_FILTER_ORDER		?= 8
+DSSS_FILTER_FC			?= 10e-3
+DSSS_FILTER_ORDER		?= 10
 
 CC			:= $(PREFIX)gcc
 CXX			:= $(PREFIX)g++
diff --git a/controller/fw/cmsis b/controller/fw/cmsis
-Subproject 4a65d88011a1595b7c8b42fa0d70b7bdfc132ac
+Subproject 6a86f4ca00d2b96b82879e1e5bd9e89c5750dd2
diff --git a/controller/fw/libsodium b/controller/fw/libsodium
-Subproject a0a8706c9dc9e43bc51d16334cd6c0f6ae084ce
+Subproject afae623190f025e7cf2fb0222bfe796b69a3694
diff --git a/controller/fw/src/dsss_demod.c b/controller/fw/src/dsss_demod.c
index 7a2e0cc..7feb41b 100644
--- a/controller/fw/src/dsss_demod.c
+++ b/controller/fw/src/dsss_demod.c
@@ -32,6 +32,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 == DSSS_CORRELATION_LENGTH-1;
+    //bool debug = sim_pos > 1000;
     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;
@@ -42,12 +43,16 @@ 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->signal_wpos, debug);
+        st->correlation[i][st->correlation_wpos] = gold_correlate_step(i, st->signal, st->signal_wpos, false);
     /* debug */
     if (debug) {
+        DEBUG_PRINTN("                 [");
+        for (size_t i=0; i<DSSS_GOLD_CODE_COUNT; i++)
+            DEBUG_PRINTN("%8d  ", i);
+        DEBUG_PRINTN("]\n");
         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("%8.5f, ", st->correlation[i][st->correlation_wpos]);
         DEBUG_PRINTN("]\n");
     }
     /* end */
@@ -57,24 +62,20 @@ 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: [");
+    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("%8.5f, ", cwt[i]);
     if (debug) DEBUG_PRINTN("]\n");
-    */
     /* end */
 
     float avg[DSSS_GOLD_CODE_COUNT];
     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 */
-    /*
-    DEBUG_PRINTN("    avg: [");
+    if (debug) DEBUG_PRINTN("    avg:         [");
     for (size_t i=0; i<DSSS_GOLD_CODE_COUNT; i++)
-        DEBUG_PRINTN("%f, ", avg[i]);
-    DEBUG_PRINTN("]\n");
-    */
+        if (debug) DEBUG_PRINTN("%8.5f, ", avg[i]);
+    if (debug) DEBUG_PRINTN("]\n");
     /* end */
 
     float max_val = st->group.max;
@@ -150,7 +151,7 @@ float cwt_convolve_step(const float v[DSSS_WAVELET_LUT_SIZE], size_t offx) {
         sum += v[(offx + j) % DSSS_WAVELET_LUT_SIZE] * dsss_cwt_wavelet_table[j];
         //DEBUG_PRINT("        j=%d v=%f w=%f", j, v[(offx + j) % DSSS_WAVELET_LUT_SIZE], dsss_cwt_wavelet_table[j]);
     }
-    return sum / DSSS_WAVELET_LUT_SIZE;
+    return sum;
 }
 
 /* Compute last element of correlation for input [a] and hard-coded gold sequences.
diff --git a/controller/fw/tools/butter_filter_gen.py b/controller/fw/tools/butter_filter_gen.py
index 059dea0..c77a6ec 100644
--- a/controller/fw/tools/butter_filter_gen.py
+++ b/controller/fw/tools/butter_filter_gen.py
@@ -1,9 +1,11 @@
 #!/usr/bin/env python3
 
+import math
 import sys
 import contextlib
 
 import scipy.signal as sig
+import numpy as np
 
 
 @contextlib.contextmanager
@@ -38,13 +40,25 @@ if __name__ == '__main__':
         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:
+
+        # scipy.signal.butter by default returns extremely small bs for the first biquad and large ones for subsequent
+        # sections. Balance magnitudes to reduce possible rounding errors.
+        first_biquad_bs = sos[0][:3]
+        approx_mag = round(math.log10(np.mean(first_biquad_bs)))
+        mags = [approx_mag // len(sos)] * len(sos)
+        mags[0] += approx_mag - sum(mags)
+        sos[0][:3] /= 10**approx_mag
+
+        for mag, sec in zip(mags, sos):
+            bs, ases = sec[:3], sec[4:6]
+            bs *= 10**mag
+
             with wrap():
                 print('.b=', end='')
                 with wrap():
-                    print(', '.join(f'{v}' for v in sec[:3]), end='')
+                    print(', '.join(f'{v}' for v in bs), end='')
                 print(', .a=', end='')
                 with wrap():
-                    print(', '.join(f'{v}' for v in sec[4:6]), end='')
+                    print(', '.join(f'{v}' for v in ases), end='')
             print(', ', end='')
         print()