From 4b419bd1ad9f22266e068341176f5ab665a26222 Mon Sep 17 00:00:00 2001 From: jaseg Date: Thu, 5 Mar 2020 19:15:28 +0100 Subject: Working on DSSS demodulator sim --- controller/fw/Makefile | 64 ++++++--- controller/fw/src/dsss_demod.c | 180 ++++++++++++++++++++++++++ controller/fw/src/dsss_demod.h | 44 +++++++ controller/fw/src/sr_global.h | 1 + controller/fw/tools/cwt_wavelet_header_gen.py | 29 +++++ controller/fw/tools/dsss_demod_test.c | 84 ++++++++++++ controller/fw/tools/dsss_demod_test_runner.py | 39 ++++++ controller/fw/tools/freq_meas_test.c | 2 +- controller/fw/tools/gold_code_header_gen.py | 59 +++++++-- 9 files changed, 468 insertions(+), 34 deletions(-) create mode 100644 controller/fw/src/dsss_demod.c create mode 100644 controller/fw/src/dsss_demod.h create mode 100644 controller/fw/tools/cwt_wavelet_header_gen.py create mode 100644 controller/fw/tools/dsss_demod_test.c create mode 100644 controller/fw/tools/dsss_demod_test_runner.py (limited to 'controller/fw') diff --git a/controller/fw/Makefile b/controller/fw/Makefile index 8dfcb1d..cc7d156 100644 --- a/controller/fw/Makefile +++ b/controller/fw/Makefile @@ -6,7 +6,7 @@ LIBSODIUM_DIR ?= libsodium TINYAES_DIR ?= tinyaes MUSL_DIR ?= musl -C_SOURCES := src/main.c src/mspdebug_wrapper.c src/spi_flash.c src/freq_meas.c +C_SOURCES := src/main.c src/mspdebug_wrapper.c src/spi_flash.c src/freq_meas.c src/dsss_demod.c C_SOURCES += $(MSPDEBUG_DIR)/drivers/jtaglib.c CMSIS_SOURCES += $(CMSIS_DIR)/CMSIS/DSP/Source/TransformFunctions/arm_rfft_fast_init_f32.c @@ -41,13 +41,17 @@ OPENCM3_LIB := opencm3_stm32f4 PREFIX ?= arm-none-eabi- -GOLD_CODE_NBITS ?= 5 FMEAS_ADC_SAMPLING_RATE ?= 1000 FMEAS_ADC_MAX ?= 4096 FMEAS_FFT_LEN ?= 256 FMEAS_FFT_WINDOW ?= gaussian FMEAS_FFT_WINDOW_SIGMA ?= 16.0 +DSSS_GOLD_CODE_NBITS ?= 5 +DSSS_DECIMATION ?= 10 +DSSS_THESHOLD_FACTOR ?= 4.0f +DSSS_WAVELET_WIDTH ?= 7.3 +DSSS_WAVELET_LUT_SIZE ?= 69 CC := $(PREFIX)gcc CXX := $(PREFIX)g++ @@ -78,6 +82,7 @@ MUSL_DIR_ABS := $(abspath $(MUSL_DIR)) COMMON_CFLAGS += -I$(OPENCM3_DIR_ABS)/include -Imspdebug/util -Imspdebug/drivers -Ilevmarq COMMON_CFLAGS += -I$(CMSIS_DIR_ABS)/CMSIS/DSP/Include -I$(CMSIS_DIR_ABS)/CMSIS/Core/Include CFLAGS += -I$(abspath musl_include_shims) +COMMON_CFLAGS += -I$(BUILDDIR) -Isrc COMMON_CFLAGS += -Os -std=gnu11 -g -DSTM32F4 CFLAGS += -mthumb -mcpu=cortex-m4 -mfloat-abi=hard -mfpu=fpv4-sp-d16 @@ -85,13 +90,18 @@ CFLAGS += -mthumb -mcpu=cortex-m4 -mfloat-abi=hard -mfpu=fpv4-sp-d16 # Note: libopencm3 requires some standard libc definitions from stdint.h and stdbool.h, so we don't pass -nostdinc here. CFLAGS += -nostdlib -ffreestanding CFLAGS += -fno-common -ffunction-sections -fdata-sections -COMMON_CFLAGS += -DGOLD_CODE_NBITS=$(GOLD_CODE_NBITS) -DFMEAS_FFT_LEN=$(FMEAS_FFT_LEN) -DFMEAS_ADC_MAX=$(FMEAS_ADC_MAX) +COMMON_CFLAGS += -DDSSS_GOLD_CODE_NBITS=$(DSSS_GOLD_CODE_NBITS) -DFMEAS_FFT_LEN=$(FMEAS_FFT_LEN) -DFMEAS_ADC_MAX=$(FMEAS_ADC_MAX) COMMON_CFLAGS += -DFMEAS_ADC_SAMPLING_RATE=$(FMEAS_ADC_SAMPLING_RATE) COMMON_CFLAGS += -DFMEAS_FFT_WINDOW_SIGMA=$(FMEAS_FFT_WINDOW_SIGMA) +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) + # for musl CFLAGS += -Dhidden= -SIM_CFLAGS += -Isrc -lm -DSIMULATION +SIM_CFLAGS += -lm -DSIMULATION INT_CFLAGS += -Wall -Wextra -Wpedantic -Wshadow -Wimplicit-function-declaration -Wundef INT_CFLAGS += -Wredundant-decls -Wmissing-prototypes -Wstrict-prototypes @@ -112,7 +122,24 @@ LDFLAGS += -L$(OPENCM3_DIR_ABS)/lib -l$(OPENCM3_LIB) $(shell $(CC) -print-libg all: $(BUILDDIR)/$(BINARY) -tests: $(BUILDDIR)/tools/freq_meas_test +src/dsss_demod.c: $(BUILDDIR)/generated/dsss_gold_code.h + +$(BUILDDIR)/generated/dsss_gold_code.h: $(BUILDDIR)/generated/gold_code_$(DSSS_GOLD_CODE_NBITS).h + ln -srf $< $@ + +.PRECIOUS: $(BUILDDIR)/generated/gold_code_%.c +$(BUILDDIR)/generated/gold_code_%.c $(BUILDDIR)/generated/gold_code_%.h: | $(BUILDDIR)/generated + $(PYTHON3) tools/gold_code_header_gen.py -v dsss_gold_code_table -$(patsubst .%,%,$(suffix $@)) $* > $@ + +.PRECIOUS: $(BUILDDIR)/generated/fmeas_fft_window.c +$(BUILDDIR)/generated/fmeas_fft_window.c: | $(BUILDDIR)/generated + $(PYTHON3) tools/fft_window_header_gen.py -v fmeas_fft_window_table $(FMEAS_FFT_WINDOW) $(FMEAS_FFT_LEN) $(FMEAS_FFT_WINDOW_SIGMA) > $@ + +.PRECIOUS: $(BUILDDIR)/generated/dsss_cwt_wavelet.c +$(BUILDDIR)/generated/dsss_cwt_wavelet.c: | $(BUILDDIR)/generated + $(PYTHON3) tools/cwt_wavelet_header_gen.py -v dsss_cwt_wavelet_table $(DSSS_WAVELET_LUT_SIZE) $(DSSS_WAVELET_WIDTH) > $@ + +$(BUILDDIR)/generated: ; mkdir -p $@ OBJS := $(addprefix $(BUILDDIR)/,$(C_SOURCES:.c=.o) $(CXX_SOURCES:.cpp=.o)) @@ -121,16 +148,23 @@ ALL_OBJS += $(OPENCM3_DIR)/lib/lib$(OPENCM3_LIB).a ALL_OBJS += $(BUILDDIR)/libsodium/src/libsodium/.libs/libsodium.a ALL_OBJS += $(BUILDDIR)/tinyaes/aes.o ALL_OBJS += $(BUILDDIR)/levmarq/levmarq.o -ALL_OBJS += $(BUILDDIR)/generated/gold_code_$(GOLD_CODE_NBITS).o +ALL_OBJS += $(BUILDDIR)/generated/gold_code_$(DSSS_GOLD_CODE_NBITS).o ALL_OBJS += $(BUILDDIR)/generated/fmeas_fft_window.o +ALL_OBJS += $(BUILDDIR)/generated/dsss_cwt_wavelet.o $(BUILDDIR)/$(BINARY): $(ALL_OBJS) $(LD) -T$(LDSCRIPT) $(COMMON_LDFLAGS) $(LDFLAGS) -o $@ -Wl,-Map=$(BUILDDIR)/src/$*.map $^ +tools: $(BUILDDIR)/tools/freq_meas_test $(BUILDDIR)/tools/freq_meas_test: tools/freq_meas_test.c src/freq_meas.c levmarq/levmarq.c $(BUILDDIR)/generated/fmeas_fft_window.c $(CMSIS_SOURCES) mkdir -p $(@D) $(HOST_CC) $(COMMON_CFLAGS) $(SIM_CFLAGS) -o $@ $^ +tools: $(BUILDDIR)/tools/dsss_demod_test +$(BUILDDIR)/tools/dsss_demod_test: tools/dsss_demod_test.c src/dsss_demod.c $(BUILDDIR)/generated/dsss_cwt_wavelet.c $(BUILDDIR)/generated/gold_code_$(DSSS_GOLD_CODE_NBITS).c + mkdir -p $(@D) + $(HOST_CC) $(COMMON_CFLAGS) $(SIM_CFLAGS) -o $@ $^ + $(BUILDDIR)/src/%.o: src/%.c mkdir -p $(@D) $(CC) $(COMMON_CFLAGS) $(CFLAGS) $(INT_CFLAGS) -o $@ -c $< @@ -158,14 +192,6 @@ $(BUILDDIR)/tinyaes/aes.o: mkdir -p $(@D) make -C $(@D) -f $(TINYAES_DIR_ABS)/Makefile VPATH=$(TINYAES_DIR_ABS) CFLAGS="$(COMMON_CFLAGS) $(CFLAGS) -c" CC=$(CC) LD=$(LD) AR=$(AR) aes.o -$(BUILDDIR)/generated/gold_code_%.c: - mkdir -p $(@D) - $(PYTHON3) tools/gold_code_header_gen.py $* > $@ - -$(BUILDDIR)/generated/fmeas_fft_window.c: - mkdir -p $(@D) - $(PYTHON3) tools/fft_window_header_gen.py -v fmeas_fft_window_table $(FMEAS_FFT_WINDOW) $(FMEAS_FFT_LEN) $(FMEAS_FFT_WINDOW_SIGMA) > $@ - $(BUILDDIR)/musl/lib/libc.a: mkdir -p $(BUILDDIR)/musl cd $(BUILDDIR)/musl && CFLAGS="$(SIM_CFLAGS) $(COMMON_CFLAGS)" CC=$(CC) LD=$(LD) AR=$(AR) $(MUSL_DIR_ABS)/configure && $(MAKE) TARGET=arm-linux-musleabihf GCC_CONFIG="-mcpu=cortex-m4 -mfloat-abi=soft" -j $(shell nproc) @@ -174,13 +200,13 @@ build/ldpc_decoder_test.so: src/ldpc_decoder.c gcc -fPIC -shared -Wall -Wextra -Wpedantic -std=gnu11 -O0 -g -o $@ $^ clean: - -rm -r $(BUILDDIR)/src - -rm -r $(BUILDDIR)/generated - -rm $(BUILDDIR)/$(BINARY) - -rm $(BUILDDIR)/tools/freq_meas_test + rm -rf $(BUILDDIR)/src + rm -rf $(BUILDDIR)/generated + rm -f $(BUILDDIR)/$(BINARY) + rm -f $(BUILDDIR)/tools/freq_meas_test mrproper: clean - -rm -r build + rm -rf build -$(MAKE) -C $(OPENCM3_DIR) clean .PHONY: clean mrproper diff --git a/controller/fw/src/dsss_demod.c b/controller/fw/src/dsss_demod.c new file mode 100644 index 0000000..ec54d41 --- /dev/null +++ b/controller/fw/src/dsss_demod.c @@ -0,0 +1,180 @@ + +#include +#include +#include + +#include + +#include "freq_meas.h" +#include "sr_global.h" +#include "dsss_demod.h" +#include "simulation.h" + +#include "generated/dsss_gold_code.h" + +/* Generated CWT wavelet LUT */ +extern const float * const dsss_cwt_wavelet_table; + +struct iir_biquad cwt_filter_bq[3] = { + {.a = {-1.993939440f, 0.993949280f}, .b = {0.2459934300683e-5, 0.4919868601367e-5, 0.2459934300683e-5}}, + {.a = {-1.995557124f, 0.995566972f}, .b = {0.2461930046414e-5, 0.4923860092828e-5, 0.2461930046414e-5}}, + {.a = {-1.998365254f, 0.998375115f}, .b = {0.2465394452097e-5, 0.4930788904195e-5, 0.2465394452097e-5}}, +}; + +float gold_correlate_step(const size_t ncode, const float a[DSSS_CORRELATION_LENGTH], 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_biquad(float x, const struct iir_biquad *const q, struct iir_biquad_state *const restrict st); + +#ifdef SIMULATION +void dsss_demod_step(struct dsss_demod_state *st, float new_value, size_t sim_pos) { +#else /* SIMULATION */ +void dsss_demod_step(struct dsss_demod_state *st, float new_value) { +#endif /* SIMULATION */ + +//#define DEBUG_PRINT(...) ((void)0) +//#define DEBUG_PRINTN(...) ((void)0) + bool debug = sim_pos % DSSS_CORRELATION_LENGTH == 0; + 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; icorrelation[i][st->correlation_wpos] = gold_correlate_step(i, st->signal, st->correlation_wpos); + /* debug */ + /* + DEBUG_PRINTN(" correlation: ["); + for (size_t i=0; icorrelation[i][st->correlation_wpos]); + DEBUG_PRINTN("]\n"); + */ + /* end */ + st->correlation_wpos = (st->correlation_wpos + 1) % ARRAY_LENGTH(st->correlation[0]); + + float cwt[DSSS_GOLD_CODE_COUNT]; + for (size_t i=0; icorrelation[i], st->correlation_wpos); + /* debug */ + if (debug) DEBUG_PRINTN(" cwt: ["); + for (size_t i=0; icwt_filter[i].st); + /* debug */ + /* + DEBUG_PRINTN(" avg: ["); + for (size_t i=0; igroup.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_THESHOLD_FACTOR) + found = true; + + if (fabs(val) > fabs(max_val)) { + max_val = val; + max_ch = i; + 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 */ + st->group.len++; + st->group.max = max_val; + st->group.max_ch = max_ch; + st->group.max_idx = max_idx; + return; + } + + if (st->group.len == 0) + /* We're between groups */ + 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); + + /* reset grouping state */ + st->group.len = 0; + st->group.max_idx = 0; + st->group.max_ch = 0; + st->group.max = 0.0f; +} + +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++) + intermediate = run_biquad(intermediate, &q[i], &st[i]); + return intermediate; +} + +float run_biquad(float x, const struct iir_biquad *const q, struct iir_biquad_state *const restrict st) { + /* direct form 2, see https://en.wikipedia.org/wiki/Digital_biquad_filter */ + float intermediate = x + st->reg[0] * -q->a[0] + st->reg[1] * -q->a[1]; + float out = intermediate * q->b[0] + st->reg[0] * q->b[1] + st->reg[1] * q->b[2]; + st->reg[1] = st->reg[0]; + st->reg[0] = intermediate; + return out; +} + +float cwt_convolve_step(const float v[DSSS_WAVELET_LUT_SIZE], size_t offx) { + float sum = 0.0f; + for (ssize_t j=0; j>3]; /* Fetch sequence table item */ + int bv = table_byte & (1<<(pos&7)); /* Extract bit */ + bv = !!bv*2 - 1; /* Map 0, 1 -> -1, 1 */ + acc_inner += a[(offx + i + j) % DSSS_CORRELATION_LENGTH] * bv; /* Multiply item */ + } + acc_outer += acc_inner; + } + return acc_outer / DSSS_CORRELATION_LENGTH; +} diff --git a/controller/fw/src/dsss_demod.h b/controller/fw/src/dsss_demod.h new file mode 100644 index 0000000..8ab5cac --- /dev/null +++ b/controller/fw/src/dsss_demod.h @@ -0,0 +1,44 @@ +#ifndef __DSSS_DEMOD_H__ +#define __DSSS_DEMOD_H__ + +#define DSSS_GOLD_CODE_LENGTH ((1<015.8g}f,' for f in win) + print(textwrap.fill(par, + initial_indent=' '*4, subsequent_indent=' '*4, + width=120, + replace_whitespace=False, drop_whitespace=False)) + print('};') + print() + print(f'const float * const {args.variable} __attribute__((weak)) = {varname};') + diff --git a/controller/fw/tools/dsss_demod_test.c b/controller/fw/tools/dsss_demod_test.c new file mode 100644 index 0000000..51742b1 --- /dev/null +++ b/controller/fw/tools/dsss_demod_test.c @@ -0,0 +1,84 @@ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "dsss_demod.h" + +void print_usage() { + fprintf(stderr, "Usage: dsss_demod_test [test_data.bin]\n"); +} + +int main(int argc, char **argv) { + if (argc != 2) { + fprintf(stderr, "Error: Invalid arguments.\n"); + print_usage(); + return 1; + } + + int fd = open(argv[1], O_RDONLY); + struct stat st; + if (fstat(fd, &st)) { + fprintf(stderr, "Error querying test data file size: %s\n", strerror(errno)); + return 2; + } + + if (st.st_size < 0 || st.st_size > 1000000) { + fprintf(stderr, "Error reading test data: too much test data (size=%zd)\n", st.st_size); + return 2; + } + + if (st.st_size % sizeof(float) != 0) { + fprintf(stderr, "Error reading test data: file size is not divisible by %zd (size=%zd)\n", sizeof(float), st.st_size); + return 2; + } + + char *buf = malloc(st.st_size); + if (!buf) { + fprintf(stderr, "Error allocating memory"); + return 2; + } + + 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); + + if (rc == -EINTR || rc == -EAGAIN) + continue; + + if (rc < 0) { + fprintf(stderr, "\nError reading test data: %s\n", strerror(errno)); + return 2; + } + + if (rc == 0) { + fprintf(stderr, "\nError reading test data: Unexpected end of file\n"); + return 2; + } + + nread += rc; + } + fprintf(stderr, " done.\n"); + + const size_t n_samples = st.st_size / sizeof(float); + float *buf_f = (float *)buf; + + fprintf(stderr, "Starting simulation.\n"); + + struct dsss_demod_state demod; + memset(&demod, 0, sizeof(demod)); + for (size_t i=0; i') - print() - print(f'/* {args.n} bit gold sequences: {2**args.n+1} sequences of length {2**args.n-1} bit.') - print(f' *') - print(f' * Each code is packed left-aligned into {2**args.n // 8} bytes in big-endian byte order.') - print(f' */') - print(f'const uint8_t gold_code_{args.n}bit[] = {{') - for i, code in enumerate(gold(args.n)): - par = ' '.join(f'0x{d:02x},' for d in np.packbits(code)) + f' /* {i: 3d} "{"".join(str(x) for x in code)}" */' - print(textwrap.fill(par, initial_indent=' '*4, subsequent_indent=' '*4, width=120)) - print('};') - print() - print(f'const uint8_t * const gold_code_table __attribute__((weak)) = gold_code_{args.n}bit;') + if not args.header != args.source: + print('Exactly one of --header and --source must be given.', file=sys.stderr) + sys.exit(1) + + nbytes = math.ceil((2**args.n-1)/8) + + if args.source: + print('/* THIS IS A GENERATED FILE. DO NOT EDIT! */') + print('#include ') + print('#include ') + print() + print(f'/* {args.n} bit gold sequences: {2**args.n+1} sequences of length {2**args.n-1} bit.') + print(f' *') + print(f' * Each code is packed left-aligned into {nbytes} bytes in big-endian byte order.') + print(f' */') + print(f'const uint8_t gold_code_{args.n}bit[{2**args.n+1}][{nbytes}] = {{') + for i, code in enumerate(gold(args.n)): + par = '{' + ' '.join(f'0x{d:02x},' for d in np.packbits(code)) + f'}}, /* {i: 3d} "{"".join(str(x) for x in code)}" */' + print(textwrap.fill(par, initial_indent=' '*4, subsequent_indent=' '*4, width=120)) + print('};') + print() + print(f'const uint8_t * const {args.variable} __attribute__((weak)) = (uint8_t *const)gold_code_{args.n}bit;') + print(f'const size_t {args.variable}_nbits __attribute__((weak)) = {args.n};') + else: + print('/* THIS IS A GENERATED FILE. DO NOT EDIT! */') + with print_include_guards(f'__GOLD_CODE_GENERATED_HEADER_{args.n}__'): + print(f'extern const uint8_t gold_code_{args.n}bit[{2**args.n+1}][{nbytes}];') + + with print_include_guards(f'__GOLD_CODE_GENERATED_HEADER_GLOBAL_SYM_{args.variable.upper()}__'): + print(f'extern const uint8_t {args.variable}[{2**args.n+1}][{nbytes}];') + print(f'extern const size_t {args.variable}_nbits;') -- cgit