diff options
-rw-r--r-- | controller/fw/Makefile | 5 | ||||
-rw-r--r-- | controller/fw/src/dsss_demod.c | 88 | ||||
-rw-r--r-- | controller/fw/src/dsss_demod.h | 2 | ||||
-rw-r--r-- | controller/fw/tools/dsss_demod_test.c | 25 | ||||
-rw-r--r-- | lab-windows/scratch.ipynb | 103 |
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, |