jaseg/master-thesis
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Finish DSSS demodulation stage 1

Browse source
This commit is contained in:
jaseg 2020-03-06 12:39:21 +01:00
parent e4693349cf
commit 55ebbcbdbc
5 changed files with 167 additions and 58 deletions
Download patch file Download diff file Expand all files Collapse all files

5
controller/fw/Makefile
View file

@ -51,11 +51,12 @@ FMEAS_SAMPLING_RATE ?= 10.0
DSSS_GOLD_CODE_NBITS ?= 5 DSSS_GOLD_CODE_NBITS ?= 5
DSSS_DECIMATION ?= 10 DSSS_DECIMATION ?= 10
# TODO maybe auto adjust this based on detection rate?
DSSS_THESHOLD_FACTOR ?= 5.0f DSSS_THESHOLD_FACTOR ?= 5.0f
DSSS_WAVELET_WIDTH ?= 7.3 DSSS_WAVELET_WIDTH ?= 7.3
DSSS_WAVELET_LUT_SIZE ?= 69 DSSS_WAVELET_LUT_SIZE ?= 69
DSSS_FILTER_FC ?= 10e-3 DSSS_FILTER_FC ?= 3e-3
DSSS_FILTER_ORDER ?= 10 DSSS_FILTER_ORDER ?= 12
CC := $(PREFIX)gcc CC := $(PREFIX)gcc
CXX := $(PREFIX)g++ CXX := $(PREFIX)g++

90
controller/fw/src/dsss_demod.c
View file

@ -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 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_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); 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 #ifdef SIMULATION
void dsss_demod_step(struct dsss_demod_state *st, float new_value, size_t sim_pos) { void debug_print_vector(const char *name, size_t len, const float *data, size_t stride, bool index, bool debug) {
#else /* SIMULATION */ if (!debug)
void dsss_demod_step(struct dsss_demod_state *st, float new_value) { return;
#endif /* SIMULATION */
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
//#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 peak_group_threshold = 0.05 * DSSS_CORRELATION_LENGTH;
//const float hole_patching_threshold = 0.01 * DSSS_CORRELATION_LENGTH; //const float hole_patching_threshold = 0.01 * DSSS_CORRELATION_LENGTH;
st->signal[st->signal_wpos] = new_value; st->signal[st->signal_wpos] = new_value;
st->signal_wpos = (st->signal_wpos + 1) % ARRAY_LENGTH(st->signal); 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 */ /* 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++) 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); st->correlation[i][st->correlation_wpos] = gold_correlate_step(i, st->signal, st->signal_wpos, false);
/* debug */
if (debug) { debug_print_vector("correlation",
DEBUG_PRINTN(" ["); DSSS_GOLD_CODE_COUNT, &st->correlation[0][st->correlation_wpos], DSSS_WAVELET_LUT_SIZE, true, debug);
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 */
st->correlation_wpos = (st->correlation_wpos + 1) % ARRAY_LENGTH(st->correlation[0]); st->correlation_wpos = (st->correlation_wpos + 1) % ARRAY_LENGTH(st->correlation[0]);
float cwt[DSSS_GOLD_CODE_COUNT]; float cwt[DSSS_GOLD_CODE_COUNT];
for (size_t i=0; i<DSSS_GOLD_CODE_COUNT; i++) for (size_t i=0; i<DSSS_GOLD_CODE_COUNT; i++)
cwt[i] = cwt_convolve_step(st->correlation[i], st->correlation_wpos); cwt[i] = cwt_convolve_step(st->correlation[i], st->correlation_wpos);
/* debug */
if (debug) DEBUG_PRINTN(" cwt: ["); debug_print_vector("cwt", DSSS_GOLD_CODE_COUNT, cwt, 1, false, debug);
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 */
float avg[DSSS_GOLD_CODE_COUNT]; float avg[DSSS_GOLD_CODE_COUNT];
for (size_t i=0; i<DSSS_GOLD_CODE_COUNT; i++) 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); 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: ["); debug_print_vector("avg", DSSS_GOLD_CODE_COUNT, avg, 1, false, debug);
for (size_t i=0; i<DSSS_GOLD_CODE_COUNT; i++)
if (debug) DEBUG_PRINTN("%8.5f, ", avg[i]); if (record_channel != -1)
if (debug) DEBUG_PRINTN("]\n"); DEBUG_PRINTN("%f, %f, %f\n",
/* end */ st->correlation[record_channel][st->correlation_wpos], cwt[record_channel], avg[record_channel]);
float max_val = st->group.max; float max_val = st->group.max;
int max_ch = st->group.max_ch; int max_ch = st->group.max_ch;
int max_idx = st->group.max_idx + 1; int max_idx = st->group.max_idx + 1;
bool found = false; bool found = false;
//if (debug) DEBUG_PRINTN(" rel: [");
for (size_t i=0; i<DSSS_GOLD_CODE_COUNT; i++) { for (size_t i=0; i<DSSS_GOLD_CODE_COUNT; i++) {
float val = cwt[i] / avg[i]; float val = cwt[i] / avg[i];
//if (debug) DEBUG_PRINTN("%f, ", val);
if (fabs(val) > DSSS_THESHOLD_FACTOR) if (fabs(val) > DSSS_THESHOLD_FACTOR)
found = true; found = true;
@ -96,12 +103,6 @@ void dsss_demod_step(struct dsss_demod_state *st, float new_value) {
max_idx = st->group.len; 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) { if (found) {
/* Continue ongoing group */ /* Continue ongoing group */
@ -117,8 +118,9 @@ void dsss_demod_step(struct dsss_demod_state *st, float new_value) {
return; return;
/* A group ended. Process result. */ /* A group ended. Process result. */
DEBUG_PRINT("GROUP FOUND: %8d len=%3d max=%f ch=%d offx=%d", if (record_channel == -1)
sim_pos, st->group.len, st->group.max, st->group.max_ch, st->group.max_idx); 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 */ /* reset grouping state */
st->group.len = 0; st->group.len = 0;

2
controller/fw/src/dsss_demod.h
View file

@ -36,7 +36,7 @@ struct dsss_demod_state {
}; };
#ifdef SIMULATION #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 */ #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);
#endif /* SIMULATION */ #endif /* SIMULATION */

25
controller/fw/tools/dsss_demod_test.c
View file

@ -13,11 +13,11 @@
#include "dsss_demod.h" #include "dsss_demod.h"
void print_usage() { 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) { int main(int argc, char **argv) {
if (argc != 2) { if (argc != 2 && argc != 3) {
fprintf(stderr, "Error: Invalid arguments.\n"); fprintf(stderr, "Error: Invalid arguments.\n");
print_usage(); print_usage();
return 1; return 1;
@ -46,7 +46,18 @@ int main(int argc, char **argv) {
return 2; 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; size_t nread = 0;
while (nread < st.st_size) { while (nread < st.st_size) {
ssize_t rc = read(fd, buf, st.st_size - nread); ssize_t rc = read(fd, buf, st.st_size - nread);
@ -66,18 +77,20 @@ int main(int argc, char **argv) {
nread += rc; nread += rc;
} }
fprintf(stderr, " done.\n"); if (record_channel != -1)
fprintf(stderr, " done.\n");
const size_t n_samples = st.st_size / sizeof(float); const size_t n_samples = st.st_size / sizeof(float);
float *buf_f = (float *)buf; float *buf_f = (float *)buf;
fprintf(stderr, "Starting simulation.\n"); if (record_channel != -1)
fprintf(stderr, "Starting simulation.\n");
struct dsss_demod_state demod; struct dsss_demod_state demod;
memset(&demod, 0, sizeof(demod)); memset(&demod, 0, sizeof(demod));
for (size_t i=0; i<n_samples; i++) { for (size_t i=0; i<n_samples; i++) {
//fprintf(stderr, "Iteration %zd/%zd\n", i, n_samples); //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; return 0;

103
lab-windows/scratch.ipynb
View file

@ -2,11 +2,12 @@
"cells": [ "cells": [
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 2, "execution_count": 7,
"metadata": {}, "metadata": {},
"outputs": [], "outputs": [],
"source": [ "source": [
"import json\n", "import json\n",
"import csv\n",
"\n", "\n",
"import numpy as np\n", "import numpy as np\n",
"from matplotlib import pyplot as plt\n", "from matplotlib import pyplot as plt\n",
@ -17,7 +18,7 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 2, "execution_count": 5,
"metadata": {}, "metadata": {},
"outputs": [], "outputs": [],
"source": [ "source": [
@ -170,13 +171,105 @@
"source": [ "source": [
"sig.butter(8, 20e-3, output='sos', fs=10.0)" "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": { "metadata": {
"kernelspec": { "kernelspec": {
"display_name": "winlabenv", "display_name": "labenv",
"language": "python", "language": "python",
"name": "winlabenv" "name": "labenv"
}, },
"language_info": { "language_info": {
"codemirror_mode": { "codemirror_mode": {
@ -188,7 +281,7 @@
"name": "python", "name": "python",
"nbconvert_exporter": "python", "nbconvert_exporter": "python",
"pygments_lexer": "ipython3", "pygments_lexer": "ipython3",
"version": "3.7.6" "version": "3.8.1"
} }
}, },
"nbformat": 4, "nbformat": 4,