decoding WIP

4 files changed, 117 insertions, 22 deletions

diff --git a/controller/fw/Makefile b/controller/fw/Makefile
index 3b9cca6..c549e47 100644
--- a/controller/fw/Makefile
+++ b/controller/fw/Makefile
@@ -52,11 +52,14 @@ FMEAS_SAMPLING_RATE		?= 10.0
 DSSS_GOLD_CODE_NBITS 	?= 5
 DSSS_DECIMATION			?= 10
 # TODO maybe auto adjust this based on detection rate?
-DSSS_THESHOLD_FACTOR	?= 5.0f
+DSSS_THESHOLD_FACTOR	?= 6.0f
 DSSS_WAVELET_WIDTH		?= 7.3
 DSSS_WAVELET_LUT_SIZE	?= 69
 DSSS_FILTER_FC			?= 3e-3
 DSSS_FILTER_ORDER		?= 12
+DSSS_GROUP_CACHE_SIZE	?= 12
+
+PAYLOAD_DATA_BIT		?= 64
 
 CC			:= $(PREFIX)gcc
 CXX			:= $(PREFIX)g++
@@ -102,6 +105,8 @@ COMMON_CFLAGS	+= -DDSSS_DECIMATION=$(DSSS_DECIMATION)
 COMMON_CFLAGS	+= -DDSSS_THESHOLD_FACTOR=$(DSSS_THESHOLD_FACTOR)
 COMMON_CFLAGS	+= -DDSSS_WAVELET_WIDTH=$(DSSS_WAVELET_WIDTH)
 COMMON_CFLAGS	+= -DDSSS_WAVELET_LUT_SIZE=$(DSSS_WAVELET_LUT_SIZE)
+COMMON_CFLAGS	+= -DDSSS_GROUP_CACHE_SIZE=$(DSSS_GROUP_CACHE_SIZE)
+COMMON_CFLAGS	+= -DPAYLOAD_DATA_BIT=$(PAYLOAD_DATA_BIT)
 
 # for musl
 CFLAGS			+= -Dhidden=
diff --git a/controller/fw/src/dsss_demod.c b/controller/fw/src/dsss_demod.c
index cc1c34e..1cad6f8 100644
--- a/controller/fw/src/dsss_demod.c
+++ b/controller/fw/src/dsss_demod.c
@@ -46,17 +46,16 @@ void debug_print_vector(const char *name, size_t len, const float *data, size_t
 #endif
 
 #ifdef SIMULATION
-void dsss_demod_step(struct dsss_demod_state *st, float new_value, size_t sim_pos, int record_channel) {
+void dsss_demod_step(struct dsss_demod_state *st, float new_value, uint64_t ts, int record_channel) {
     bool debug = (record_channel == -1)
-        && (sim_pos > 1000)
-        && (sim_pos % DSSS_CORRELATION_LENGTH == DSSS_CORRELATION_LENGTH-1);
+        && (ts > 1000)
+        && (ts % DSSS_CORRELATION_LENGTH == DSSS_CORRELATION_LENGTH-1);
 
-    if (debug) DEBUG_PRINT("Iteration %zd: signal=%f", sim_pos, new_value);
+    if (debug) DEBUG_PRINT("Iteration %zd: signal=%f", ts, new_value);
 #else
 void dsss_demod_step(struct dsss_demod_state *st, float new_value) {
 #endif
 
-    //const float peak_group_threshold = 0.05 * DSSS_CORRELATION_LENGTH;
     //const float hole_patching_threshold = 0.01 * DSSS_CORRELATION_LENGTH;
 
     st->signal[st->signal_wpos] = new_value;
@@ -89,7 +88,7 @@ void dsss_demod_step(struct dsss_demod_state *st, float new_value) {
 
     float max_val = st->group.max;
     int max_ch = st->group.max_ch;
-    int max_idx = st->group.max_idx + 1;
+    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];
@@ -100,7 +99,7 @@ void dsss_demod_step(struct dsss_demod_state *st, float new_value) {
         if (fabs(val) > fabs(max_val)) {
             max_val = val;
             max_ch = i;
-            max_idx = st->group.len;
+            max_ts = ts;
         }
     }
 
@@ -109,7 +108,7 @@ void dsss_demod_step(struct dsss_demod_state *st, float new_value) {
         st->group.len++;
         st->group.max = max_val;
         st->group.max_ch = max_ch;
-        st->group.max_idx = max_idx;
+        st->group.max_ts = max_ts;
         return;
     }
     
@@ -120,15 +119,60 @@ void dsss_demod_step(struct dsss_demod_state *st, float new_value) {
     /* A group ended. Process result. */
     if (record_channel == -1)
         DEBUG_PRINT("GROUP FOUND: %8d len=%3d max=%f ch=%d offx=%d",
-                sim_pos, st->group.len, st->group.max, st->group.max_ch, st->group.max_idx);
+                ts, st->group.len, st->group.max, st->group.max_ch, st->group.max_ts);
 
     /* reset grouping state */
     st->group.len = 0;
-    st->group.max_idx = 0;
+    st->group.max_ts = 0;
     st->group.max_ch = 0;
     st->group.max = 0.0f;
 }
 
+float score_group(const struct group *g, uint64_t ts) {
+    return fabs(g->max); /* Possibly at time penalty 1/(ts-max_ts) later */
+}
+
+ssize_t group_cache_insertion_index(const struct group *g, const struct group *cache, size_t cache_size, uint64_t ts) {
+    float min_score = INFINITY;
+    ssize_t min_idx = -1;
+    for (size_t i=0; i<cache_size; i++) {
+        /* If we find an empty or expired entry, use that */
+        if (cache[i].max_ts == 0 || ts - cache[i].max_ts > group_cache_expiration)
+            return i;
+
+        /* Otherwise check weakest entry */
+        float score = score_group(&cache[i]);
+        if (score < min_score) {
+            min_idx = i;
+            min_score = score;
+        }
+    }
+
+    /* Return weakest group if weaker than candidate */
+    if (min_score < score_group(g))
+        return min_idx;
+
+    return -1;
+}
+
+void group_received(struct dsss_demod_state *st, uint64_t ts) {
+    /* TODO make these constants configurable from Makefile */
+    const uint64_t group_cache_expiration = DSSS_CORRELATION_LENGTH * DSSS_GROUP_CACHE_SIZE;
+
+    /* Insert into group cache if space is available or there is a weaker entry to replace */
+    ssize_t found = group_cache_insertion_index(&st->group, st->group_cache, DSSS_GROUP_CACHE_SIZE);
+    if (!found)
+        return; /* Nothing changed */
+    st->group_cache[found] = st->group;
+
+    float mean_phase = 0.0;
+    for (size_t i=0; i<DSSS_GROUP_CACHE_SIZE; i++)
+        mean_phase += (st->group_cache[i].max_ts) % DSSS_CORRELATION_LENGTH;
+    mean_phase /= DSSS_GROUP_CACHE_SIZE;
+
+    
+}
+
 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++)
diff --git a/controller/fw/src/dsss_demod.h b/controller/fw/src/dsss_demod.h
index b6947bc..30ec3f7 100644
--- a/controller/fw/src/dsss_demod.h
+++ b/controller/fw/src/dsss_demod.h
@@ -5,6 +5,8 @@
 #define DSSS_GOLD_CODE_COUNT    ((1<<DSSS_GOLD_CODE_NBITS) + 1)
 #define DSSS_CORRELATION_LENGTH (DSSS_GOLD_CODE_LENGTH * DSSS_DECIMATION)
 
+#define PAYLOAD_DATA_BYTE ((PAYLOAD_DATA_BIT+7)/8)
+
 struct iir_biquad {
     float a[2];
     float b[3];
@@ -18,6 +20,24 @@ struct cwt_iir_filter_state {
     struct iir_biquad_state st[3];
 };
 
+struct {
+    int len; /* length of group in samples */
+    float max; /* signed value of largest peak in group on any channel */
+    uint64_t max_ts; /* absolute position of above peak */
+    int max_ch; /* channel (gold sequence index) of above peak */
+} group;
+
+struct decoder_state {
+    int last_phase;
+    int candidate_phase;
+
+    float last_score;
+    float candidate_score;
+
+    uint8_t data[PAYLOAD_DATA_BYTE]; 
+    int data_pos;
+};
+
 struct dsss_demod_state {
     float signal[DSSS_CORRELATION_LENGTH];
     size_t signal_wpos;
@@ -27,18 +47,15 @@ struct dsss_demod_state {
 
     struct cwt_iir_filter_state cwt_filter[DSSS_GOLD_CODE_COUNT];
 
-    struct {
-        int len; /* length of group in samples */
-        float max; /* signed value of largest peak in group on any channel */
-        int max_idx; /* position of above peak counted from start of group */
-        int max_ch; /* channel (gold sequence index) of above peak */
-    } group;
+    struct group group;
+
+    struct group group_cache[DSSS_GROUP_CACHE_SIZE];
 };
 
 #ifdef SIMULATION
-void dsss_demod_step(struct dsss_demod_state *st, float new_value, size_t sim_pos, int record_channel);
+void dsss_demod_step(struct dsss_demod_state *st, float new_value, uint64_t ts, int record_channel);
 #else /* SIMULATION */
-void dsss_demod_step(struct dsss_demod_state *st, float new_value);
+void dsss_demod_step(struct dsss_demod_state *st, float new_value, uint64_t ts);
 #endif /* SIMULATION */
 
 #endif /* __DSSS_DEMOD_H__ */
diff --git a/controller/fw/tools/butter_filter_gen.py b/controller/fw/tools/butter_filter_gen.py
index c77a6ec..0bb81bc 100644
--- a/controller/fw/tools/butter_filter_gen.py
+++ b/controller/fw/tools/butter_filter_gen.py
@@ -18,11 +18,21 @@ def wrap(left='{', right='}', file=None, end=''):
 def print_include_guards(macro_name):
     print(f'#ifndef {macro_name}')
     print(f'#define {macro_name}')
+    print()
     yield
+    print()
     print(f'#endif /* {macro_name} */')
 
 macro_float = lambda f: f'{f}'.replace('.', 'F').replace('-', 'N').replace('+', 'P')
 
+ordinal = lambda n: "%d%s" % (n,"tsnrhtdd"[(n//10%10!=1)*(n%10<4)*n%10::4])
+
+SI_TABLE = {-18: 'a', -15: 'f', -12: 'p', -9: 'n', -6: 'µ', -3: 'm', 0: '', 3: 'k', 6: 'M', 9: 'G', 12: 'T', 15: 'P', 18: 'E'}
+def siprefix(x, space=' ', unit=''):
+    l = math.log10(x)//3*3
+    if l in SI_TABLE:
+        return f'{x/10**l}{space}{SI_TABLE[l]}{unit}'
+    return f'{x}{space}{unit}'
 
 if __name__ == '__main__':
     import argparse
@@ -36,10 +46,17 @@ if __name__ == '__main__':
     sos = sig.butter(args.n, args.fc, fs=args.fs, output='sos')
 
     print('/* THIS IS A GENERATED FILE. DO NOT EDIT! */')
+    print()
     with print_include_guards(f'__BUTTER_FILTER_GENERATED_{args.n}_{macro_float(args.fc)}_{macro_float(args.fs)}__'):
+
+        print(f'/* {ordinal(args.n)} order Butterworth IIR filter coefficients')
+        print(f' *')
+        print(f' * corner frequency f_c = {siprefix(args.fc)}Hz')
+        print(f' * sampling rate f_s = {siprefix(args.fs)}Hz')
+        print(f' */')
+        print()
         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='')
 
         # 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.
@@ -48,10 +65,14 @@ if __name__ == '__main__':
         mags = [approx_mag // len(sos)] * len(sos)
         mags[0] += approx_mag - sum(mags)
         sos[0][:3] /= 10**approx_mag
+        sos = np.array([ sec * np.array([10**mag, 10**mag, 10**mag, 1, 1, 1]) for mag, sec in zip(mags, sos) ])
+
+        ones = np.ones([100000])
+        _, steady_state = sig.sosfilt(sos, ones, zi=np.zeros([(args.n+1)//2, 2]))
 
-        for mag, sec in zip(mags, sos):
+        print(f'#define {args.macro_name.upper()}_COEFF ', end='')
+        for sec in sos:
             bs, ases = sec[:3], sec[4:6]
-            bs *= 10**mag
 
             with wrap():
                 print('.b=', end='')
@@ -62,3 +83,11 @@ if __name__ == '__main__':
                     print(', '.join(f'{v}' for v in ases), end='')
             print(', ', end='')
         print()
+
+        print(f'#define {args.macro_name.upper()}_STEADY_STATE ', end='')
+        for sec in steady_state:
+            with wrap():
+                print(', '.join(f'{v}' for v in sec), end='')
+            print(', ', end='')
+        print()
+