Finish DSSS demodulation stage 1

5 files changed, 166 insertions, 57 deletions

diff --git a/controller/fw/Makefile b/controller/fw/Makefile
index 2c75bbf..3b9cca6 100644
--- a/controller/fw/Makefile
+++ b/controller/fw/Makefile
@@ -51,11 +51,12 @@ 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_WAVELET_WIDTH		?= 7.3
 DSSS_WAVELET_LUT_SIZE	?= 69
-DSSS_FILTER_FC			?= 10e-3
-DSSS_FILTER_ORDER		?= 10
+DSSS_FILTER_FC			?= 3e-3
+DSSS_FILTER_ORDER		?= 12
 
 CC			:= $(PREFIX)gcc
 CXX			:= $(PREFIX)g++
diff --git a/controller/fw/src/dsss_demod.c b/controller/fw/src/dsss_demod.c
index 7feb41b..cc1c34e 100644
--- a/controller/fw/src/dsss_demod.c
+++ b/controller/fw/src/dsss_demod.c
@@ -22,70 +22,77 @@ float gold_correlate_step(const size_t ncode, const float a[DSSS_CORRELATION_LEN
 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);
+void debug_print_vector(const char *name, size_t len, const float *data, size_t stride, bool index, bool debug);
 
 #ifdef SIMULATION
-void dsss_demod_step(struct dsss_demod_state *st, float new_value, size_t sim_pos) {
-#else /* SIMULATION */
+void debug_print_vector(const char *name, size_t len, const float *data, size_t stride, bool index, bool debug) {
+    if (!debug)
+        return;
+
+    if (index) {
+        DEBUG_PRINTN("    %16s  [", "");
+        for (size_t i=0; i<len; i++)
+            DEBUG_PRINTN("%8d  ", i);
+        DEBUG_PRINTN("]\n");
+    }
+
+    DEBUG_PRINTN("    %16s: [", name);
+    for (size_t i=0; i<len; i++)
+        DEBUG_PRINTN("%8.5f, ", data[i*stride]);
+    DEBUG_PRINTN("]\n");
+}
+#else 
+void debug_print_vector(const char *name, size_t len, const float *data, size_t stride, bool index, bool debug) {}
+#endif
+
+#ifdef SIMULATION
+void dsss_demod_step(struct dsss_demod_state *st, float new_value, size_t sim_pos, int record_channel) {
+    bool debug = (record_channel == -1)
+        && (sim_pos > 1000)
+        && (sim_pos % DSSS_CORRELATION_LENGTH == DSSS_CORRELATION_LENGTH-1);
+
+    if (debug) DEBUG_PRINT("Iteration %zd: signal=%f", sim_pos, new_value);
+#else
 void dsss_demod_step(struct dsss_demod_state *st, float new_value) {
-#endif /* SIMULATION */
+#endif
 
-//#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;
 
     st->signal[st->signal_wpos] = new_value;
     st->signal_wpos = (st->signal_wpos + 1) % ARRAY_LENGTH(st->signal);
-    if (debug) DEBUG_PRINT("   signal: %f", 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, 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("%8.5f, ", st->correlation[i][st->correlation_wpos]);
-        DEBUG_PRINTN("]\n");
-    }
-    /* end */
+
+    debug_print_vector("correlation",
+            DSSS_GOLD_CODE_COUNT, &st->correlation[0][st->correlation_wpos], DSSS_WAVELET_LUT_SIZE, true, debug);
+
     st->correlation_wpos = (st->correlation_wpos + 1) % ARRAY_LENGTH(st->correlation[0]);
 
     float cwt[DSSS_GOLD_CODE_COUNT];
     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("%8.5f, ", cwt[i]);
-    if (debug) DEBUG_PRINTN("]\n");
-    /* end */
+
+    debug_print_vector("cwt", DSSS_GOLD_CODE_COUNT, cwt, 1, false, debug);
 
     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 */
-    if (debug) DEBUG_PRINTN("    avg:         [");
-    for (size_t i=0; i<DSSS_GOLD_CODE_COUNT; i++)
-        if (debug) DEBUG_PRINTN("%8.5f, ", avg[i]);
-    if (debug) DEBUG_PRINTN("]\n");
-    /* end */
+
+    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]);
 
     float max_val = st->group.max;
     int max_ch = st->group.max_ch;
     int max_idx = st->group.max_idx + 1;
     bool found = false;
-    //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 (fabs(val) > DSSS_THESHOLD_FACTOR)
             found = true;
@@ -96,12 +103,6 @@ void dsss_demod_step(struct dsss_demod_state *st, float new_value) {
             max_idx = st->group.len;
         }
     }
-    //if (debug) DEBUG_PRINTN("]\n");
-
-    /* debug */
-    if (debug) DEBUG_PRINT("    found=%d len=%d idx=%d ch=%d max=%f",
-            found, st->group.len, st->group.max_idx, st->group.max_ch, st->group.max);
-    /* end */
 
     if (found) {
         /* Continue ongoing group */
@@ -117,8 +118,9 @@ void dsss_demod_step(struct dsss_demod_state *st, float new_value) {
         return;
 
     /* A group ended. Process result. */
-    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);
+    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);
 
     /* reset grouping state */
     st->group.len = 0;
diff --git a/controller/fw/src/dsss_demod.h b/controller/fw/src/dsss_demod.h
index 8ab5cac..b6947bc 100644
--- a/controller/fw/src/dsss_demod.h
+++ b/controller/fw/src/dsss_demod.h
@@ -36,7 +36,7 @@ struct dsss_demod_state {
 };
 
 #ifdef SIMULATION
-void dsss_demod_step(struct dsss_demod_state *st, float new_value, size_t sim_pos);
+void dsss_demod_step(struct dsss_demod_state *st, float new_value, size_t sim_pos, int record_channel);
 #else /* SIMULATION */
 void dsss_demod_step(struct dsss_demod_state *st, float new_value);
 #endif /* SIMULATION */
diff --git a/controller/fw/tools/dsss_demod_test.c b/controller/fw/tools/dsss_demod_test.c
index 51742b1..4370f80 100644
--- a/controller/fw/tools/dsss_demod_test.c
+++ b/controller/fw/tools/dsss_demod_test.c
@@ -13,11 +13,11 @@
 #include "dsss_demod.h"
 
 void print_usage() {
-    fprintf(stderr, "Usage: dsss_demod_test [test_data.bin]\n");
+    fprintf(stderr, "Usage: dsss_demod_test [test_data.bin] [optional recording channel number]\n");
 }
 
 int main(int argc, char **argv) {
-    if (argc != 2) {
+    if (argc != 2 && argc != 3) {
         fprintf(stderr, "Error: Invalid arguments.\n");
         print_usage();
         return 1;
@@ -46,7 +46,18 @@ int main(int argc, char **argv) {
         return 2;
     }
 
-    fprintf(stderr, "Reading %zd samples test data...", st.st_size/sizeof(float));
+    int record_channel = -1;
+    if (argc == 3) {
+        char *endptr;
+        record_channel = strtoul(argv[2], &endptr, 10);
+        if (!endptr || *endptr != '\0') {
+            fprintf(stderr, "Invalid channel number \"%s\"\n", argv[2]);
+            return 1;
+        }
+    }
+
+    if (record_channel != -1)
+        fprintf(stderr, "Reading %zd samples test data...", st.st_size/sizeof(float));
     size_t nread = 0;
     while (nread < st.st_size) {
         ssize_t rc = read(fd, buf, st.st_size - nread);
@@ -66,18 +77,20 @@ int main(int argc, char **argv) {
 
         nread += rc;
     }
-    fprintf(stderr, " done.\n");
+    if (record_channel != -1)
+        fprintf(stderr, " done.\n");
 
     const size_t n_samples = st.st_size / sizeof(float);
     float *buf_f = (float *)buf;
 
-    fprintf(stderr, "Starting simulation.\n");
+    if (record_channel != -1)
+        fprintf(stderr, "Starting simulation.\n");
 
     struct dsss_demod_state demod;
     memset(&demod, 0, sizeof(demod));
     for (size_t i=0; i<n_samples; i++) {
         //fprintf(stderr, "Iteration %zd/%zd\n", i, n_samples);
-        dsss_demod_step(&demod, buf_f[i], i);
+        dsss_demod_step(&demod, buf_f[i], i, record_channel);
     }
 
     return 0;
diff --git a/lab-windows/scratch.ipynb b/lab-windows/scratch.ipynb
index 671bedf..ddd2455 100644
--- a/lab-windows/scratch.ipynb
+++ b/lab-windows/scratch.ipynb
@@ -2,11 +2,12 @@
  "cells": [
   {
    "cell_type": "code",
-   "execution_count": 2,
+   "execution_count": 7,
    "metadata": {},
    "outputs": [],
    "source": [
     "import json\n",
+    "import csv\n",
     "\n",
     "import numpy as np\n",
     "from matplotlib import pyplot as plt\n",
@@ -17,7 +18,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 2,
+   "execution_count": 5,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -170,13 +171,105 @@
    "source": [
     "sig.butter(8, 20e-3, output='sos', fs=10.0)"
    ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 6,
+   "metadata": {},
+   "outputs": [
+    {
+     "data": {
+      "application/vnd.jupyter.widget-view+json": {
+       "model_id": "655ce5c77d2a4047905245df39a095b0",
+       "version_major": 2,
+       "version_minor": 0
+      },
+      "text/plain": [
+       "Canvas(toolbar=Toolbar(toolitems=[('Home', 'Reset original view', 'home', 'home'), ('Back', 'Back to previous …"
+      ]
+     },
+     "metadata": {},
+     "output_type": "display_data"
+    },
+    {
+     "data": {
+      "text/plain": [
+       "[<matplotlib.lines.Line2D at 0x7f4609ce6a90>]"
+      ]
+     },
+     "execution_count": 6,
+     "metadata": {},
+     "output_type": "execute_result"
+    }
+   ],
+   "source": [
+    "fig, ax = plt.subplots()\n",
+    "ax.plot([0-0.00012937261, 0-0.00022784119 , 0-0.00039295876 , 0-0.00066361829 , 00-0.0010971602 , 00-0.0017754816 ,\n",
+    "    00-0.0028116399 , 00-0.0043560231 , 00-0.0066005666 , 00-0.0097788338 , 000-0.014159188 , 000-0.020027947 ,\n",
+    "    000-0.027659611 , 000-0.037272236 , 000-0.048968014 , 0000-0.06266222 , 0000-0.07800759 , 000-0.094325546 ,\n",
+    "    0000-0.11055938 , 0000-0.12526666 , 0000-0.13666715 , 0000-0.14275811 , 0000-0.14149973 , 00000-0.1310612 ,\n",
+    "    0000-0.11010384 , 000-0.078063987 , 000-0.035389599 , 00000.016317957 , 00000.074297836 , 000000.13478363 ,\n",
+    "    000000.19331697 , 000000.24519242 , 000000.28597909 , 000000.31204596 , 000000.32101141 , 000000.31204596 ,\n",
+    "    000000.28597909 , 000000.24519242 , 000000.19331697 , 000000.13478363 , 00000.074297836 , 00000.016317957 ,\n",
+    "    000-0.035389599 , 000-0.078063987 , 0000-0.11010384 , 00000-0.1310612 , 0000-0.14149973 , 0000-0.14275811 ,\n",
+    "    0000-0.13666715 , 0000-0.12526666 , 0000-0.11055938 , 000-0.094325546 , 0000-0.07800759 , 0000-0.06266222 ,\n",
+    "    000-0.048968014 , 000-0.037272236 , 000-0.027659611 , 000-0.020027947 , 000-0.014159188 , 00-0.0097788338 ,\n",
+    "    00-0.0066005666 , 00-0.0043560231 , 00-0.0028116399 , 00-0.0017754816 , 00-0.0010971602 , 0-0.00066361829 ,\n",
+    "    0-0.00039295876 , 0-0.00022784119 , 0-0.00012937261 ])"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 25,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "data = np.genfromtxt('/tmp/foo.csv', delimiter=',')[1000:]"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 28,
+   "metadata": {},
+   "outputs": [
+    {
+     "data": {
+      "application/vnd.jupyter.widget-view+json": {
+       "model_id": "7c771472882e4ceeb8aeddfa5c08ca17",
+       "version_major": 2,
+       "version_minor": 0
+      },
+      "text/plain": [
+       "Canvas(toolbar=Toolbar(toolitems=[('Home', 'Reset original view', 'home', 'home'), ('Back', 'Back to previous …"
+      ]
+     },
+     "metadata": {},
+     "output_type": "display_data"
+    }
+   ],
+   "source": [
+    "fig, axs = plt.subplots(2, figsize=(15, 9), sharex=True)\n",
+    "axs = axs.flatten()\n",
+    "axs[0].set_title('corr')\n",
+    "axs[1].set_title('cwt')\n",
+    "#axs[2].set_title('iir')\n",
+    "\n",
+    "axs[0].plot(data[:,0], label='corr')\n",
+    "axs[1].plot(data[:,1], label='cwt')\n",
+    "axs[0].plot(data[:,2], label='avg')\n",
+    "axs[1].plot(data[:,2], label='avg')\n",
+    "\n",
+    "for ax in axs:\n",
+    "    ax.legend()\n",
+    "    ax.grid()"
+   ]
   }
  ],
  "metadata": {
   "kernelspec": {
-   "display_name": "winlabenv",
+   "display_name": "labenv",
    "language": "python",
-   "name": "winlabenv"
+   "name": "labenv"
   },
   "language_info": {
    "codemirror_mode": {
@@ -188,7 +281,7 @@
    "name": "python",
    "nbconvert_exporter": "python",
    "pygments_lexer": "ipython3",
-   "version": "3.7.6"
+   "version": "3.8.1"
   }
  },
  "nbformat": 4,