From d9b26d16c063aec32b70287ff861a1f23642a9f2 Mon Sep 17 00:00:00 2001 From: jaseg Date: Wed, 4 Mar 2020 17:10:31 +0100 Subject: Fix frequency measurement simulation --- controller/fw/Makefile | 29 +-- controller/fw/levmarq/levmarq.c | 272 ++++++++++++++------------- controller/fw/src/freq_meas.c | 69 +++++-- controller/fw/src/freq_meas.h | 2 +- controller/fw/src/simulation.h | 14 ++ controller/fw/tools/freq_meas_test.c | 38 ++-- controller/fw/tools/freq_meas_test_runner.py | 39 ++++ 7 files changed, 289 insertions(+), 174 deletions(-) create mode 100644 controller/fw/src/simulation.h create mode 100644 controller/fw/tools/freq_meas_test_runner.py (limited to 'controller/fw') diff --git a/controller/fw/Makefile b/controller/fw/Makefile index a4b8a66..8dfcb1d 100644 --- a/controller/fw/Makefile +++ b/controller/fw/Makefile @@ -46,17 +46,17 @@ FMEAS_ADC_SAMPLING_RATE ?= 1000 FMEAS_ADC_MAX ?= 4096 FMEAS_FFT_LEN ?= 256 FMEAS_FFT_WINDOW ?= gaussian -FMEAS_FFT_WINDOW_SIGMA ?= 8.0 +FMEAS_FFT_WINDOW_SIGMA ?= 16.0 -CC ?= $(PREFIX)gcc -CXX ?= $(PREFIX)g++ -LD ?= $(PREFIX)gcc -AR ?= $(PREFIX)ar -AS ?= $(PREFIX)as -OBJCOPY ?= $(PREFIX)objcopy -OBJDUMP ?= $(PREFIX)objdump -GDB ?= $(PREFIX)gdb +CC := $(PREFIX)gcc +CXX := $(PREFIX)g++ +LD := $(PREFIX)gcc +AR := $(PREFIX)ar +AS := $(PREFIX)as +OBJCOPY := $(PREFIX)objcopy +OBJDUMP := $(PREFIX)objdump +GDB := $(PREFIX)gdb HOST_CC ?= $(HOST_PREFIX)gcc HOST_CXX ?= $(HOST_PREFIX)g++ @@ -75,9 +75,9 @@ LIBSODIUM_DIR_ABS := $(abspath $(LIBSODIUM_DIR)) TINYAES_DIR_ABS := $(abspath $(TINYAES_DIR)) MUSL_DIR_ABS := $(abspath $(MUSL_DIR)) -COMMON_CFLAGS += -I$(OPENCM3_DIR_ABS)/include -Imspdebug/util -Imspdebug/drivers +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 -COMMON_CFLAGS += -I$(abspath musl_include_shims) -Ilevmarq +CFLAGS += -I$(abspath musl_include_shims) COMMON_CFLAGS += -Os -std=gnu11 -g -DSTM32F4 CFLAGS += -mthumb -mcpu=cortex-m4 -mfloat-abi=hard -mfpu=fpv4-sp-d16 @@ -91,7 +91,7 @@ COMMON_CFLAGS += -DFMEAS_FFT_WINDOW_SIGMA=$(FMEAS_FFT_WINDOW_SIGMA) # for musl CFLAGS += -Dhidden= -SIM_CFLAGS += -Isrc +SIM_CFLAGS += -Isrc -lm -DSIMULATION INT_CFLAGS += -Wall -Wextra -Wpedantic -Wshadow -Wimplicit-function-declaration -Wundef INT_CFLAGS += -Wredundant-decls -Wmissing-prototypes -Wstrict-prototypes @@ -112,7 +112,7 @@ LDFLAGS += -L$(OPENCM3_DIR_ABS)/lib -l$(OPENCM3_LIB) $(shell $(CC) -print-libg all: $(BUILDDIR)/$(BINARY) -tests: $(BUILDDIR)/tools/freq_meas_test.elf +tests: $(BUILDDIR)/tools/freq_meas_test OBJS := $(addprefix $(BUILDDIR)/,$(C_SOURCES:.c=.o) $(CXX_SOURCES:.cpp=.o)) @@ -127,7 +127,7 @@ ALL_OBJS += $(BUILDDIR)/generated/fmeas_fft_window.o $(BUILDDIR)/$(BINARY): $(ALL_OBJS) $(LD) -T$(LDSCRIPT) $(COMMON_LDFLAGS) $(LDFLAGS) -o $@ -Wl,-Map=$(BUILDDIR)/src/$*.map $^ -$(BUILDDIR)/tools/freq_meas_test.elf: tools/freq_meas_test.c src/freq_meas.c levmarq/levmarq.c $(BUILDDIR)/generated/fmeas_fft_window.c $(CMSIS_SOURCES) +$(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 $@ $^ @@ -177,6 +177,7 @@ clean: -rm -r $(BUILDDIR)/src -rm -r $(BUILDDIR)/generated -rm $(BUILDDIR)/$(BINARY) + -rm $(BUILDDIR)/tools/freq_meas_test mrproper: clean -rm -r build diff --git a/controller/fw/levmarq/levmarq.c b/controller/fw/levmarq/levmarq.c index bbf00c0..a3a77b5 100644 --- a/controller/fw/levmarq/levmarq.c +++ b/controller/fw/levmarq/levmarq.c @@ -20,6 +20,7 @@ #include #include "levmarq.h" +#include "simulation.h" #define TOL 1e-20f /* smallest value allowed in cholesky_decomp() */ @@ -28,18 +29,20 @@ /* set parameters required by levmarq() to default values */ void levmarq_init(LMstat *lmstat) { - lmstat->max_it = 10000; - lmstat->init_lambda = 0.0001f; - lmstat->up_factor = 10.0f; - lmstat->down_factor = 10.0f; - lmstat->target_derr = 1e-12f; + lmstat->max_it = 10000; + lmstat->init_lambda = 0.0001f; + lmstat->up_factor = 10.0f; + lmstat->down_factor = 10.0f; + lmstat->target_derr = 1e-12f; } +#ifndef SIMULATION float sqrtf(float arg) { float out=NAN; arm_sqrt_f32(arg, &out); return out; } +#endif /* perform least-squares minimization using the Levenberg-Marquardt algorithm. The arguments are as follows: @@ -49,140 +52,147 @@ float sqrtf(float arg) { ny number of measurements to be fit y array of measurements dysq array of error in measurements, squared - (set dysq=NULL for unweighted least-squares) + (set dysq=NULL for unweighted least-squares) func function to be fit grad gradient of "func" with respect to the input parameters fdata pointer to any additional data required by the function lmstat pointer to the "status" structure, where minimization parameters - are set and the final status is returned. + are set and the final status is returned. Before calling levmarq, several of the parameters in lmstat must be set. For default values, call levmarq_init(lmstat). - */ + */ int levmarq(int npar, float *par, int ny, float *y, float *dysq, - float (*func)(float *, int, void *), - void (*grad)(float *, float *, int, void *), - void *fdata, LMstat *lmstat) + float (*func)(float *, int, void *), + void (*grad)(float *, float *, int, void *), + void *fdata, LMstat *lmstat) { - int x,i,j,it,nit,ill; - float lambda,up,down,mult,weight,err,newerr,derr,target_derr; - float h[npar][npar],ch[npar][npar]; - float g[npar],d[npar],delta[npar],newpar[npar]; - - nit = lmstat->max_it; - lambda = lmstat->init_lambda; - up = lmstat->up_factor; - down = 1/lmstat->down_factor; - target_derr = lmstat->target_derr; - weight = 1; - derr = newerr = 0; /* to avoid compiler warnings */ - - /* calculate the initial error ("chi-squared") */ - err = error_func(par, ny, y, dysq, func, fdata); - - /* main iteration */ - for (it=0; it 0); - } - if (ill) { - mult = (1 + lambda*up)/(1 + lambda); - lambda *= up; - it++; - } + int x,i,j,it,nit,ill; + float lambda,up,down,mult,weight,err,newerr,derr,target_derr; + float h[npar][npar],ch[npar][npar]; + float g[npar],d[npar],delta[npar],newpar[npar]; + + nit = lmstat->max_it; + lambda = lmstat->init_lambda; + up = lmstat->up_factor; + down = 1/lmstat->down_factor; + target_derr = lmstat->target_derr; + weight = 1; + derr = newerr = 0; /* to avoid compiler warnings */ + + /* calculate the initial error ("chi-squared") */ + err = error_func(par, ny, y, dysq, func, fdata); + + /* main iteration */ + for (it=0; it 0); + } + if (ill) { + mult = (1 + lambda*up)/(1 + lambda); + lambda *= up; + it++; + } + } + for (i=0; ifinal_it = it; + lmstat->final_err = err; + lmstat->final_derr = derr; - lmstat->final_it = it; - lmstat->final_err = err; - lmstat->final_derr = derr; + if (it == nit) { + DEBUG_PRINT("did not converge"); + return -1; + } - return (it==nit); + return it; } /* calculate the error function (chi-squared) */ float error_func(float *par, int ny, float *y, float *dysq, - float (*func)(float *, int, void *), void *fdata) + float (*func)(float *, int, void *), void *fdata) { - int x; - float res,e=0; - - for (x=0; x=0; i--) { - sum = 0; - for (j=i+1; j=0; i--) { + sum = 0; + for (j=i+1; j +#define DEBUG_PRINTN(...) fprintf(stderr, __VA_ARGS__) +#define DEBUG_PRINTNF(fmt, ...) DEBUG_PRINTN("%s:%d: " fmt, __FILE__, __LINE__, ##__VA_ARGS__) +#define DEBUG_PRINT(fmt, ...) DEBUG_PRINTNF(fmt "\n", ##__VA_ARGS__) +#else +#define DEBUG_PRINT(...) ((void)0) +#define DEBUG_PRINTN(...) ((void)0) +#endif + +#endif /* __SIMULATION_H__ */ diff --git a/controller/fw/tools/freq_meas_test.c b/controller/fw/tools/freq_meas_test.c index 01b4963..df3e39d 100644 --- a/controller/fw/tools/freq_meas_test.c +++ b/controller/fw/tools/freq_meas_test.c @@ -1,4 +1,6 @@ +#include +#include #include #include #include @@ -13,7 +15,7 @@ void print_usage(void); void print_usage() { - fprintf(stderr, "Usage: freq_meas_test [test_data.bin]"); + fprintf(stderr, "Usage: freq_meas_test [test_data.bin]\n"); } int main(int argc, char **argv) { @@ -46,6 +48,7 @@ int main(int argc, char **argv) { 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); @@ -54,41 +57,48 @@ int main(int argc, char **argv) { continue; if (rc < 0) { - fprintf(stderr, "Error reading test data: %s\n", strerror(errno)); + fprintf(stderr, "\nError reading test data: %s\n", strerror(errno)); return 2; } if (rc == 0) { - fprintf(stderr, "Error reading test data: Unexpected end of file\n"); + fprintf(stderr, "\nError reading test data: Unexpected end of file\n"); return 2; } nread += rc; } + fprintf(stderr, " done.\n"); size_t n_samples = st.st_size / sizeof(float); float *buf_f = (float *)buf; - uint16_t *sim_adc_buf = calloc(sizeof(uint16_t), n_samples); + int16_t *sim_adc_buf = calloc(sizeof(int16_t), n_samples); if (!sim_adc_buf) { fprintf(stderr, "Error allocating memory\n"); return 2; } + fprintf(stderr, "Converting and truncating test data..."); for (size_t i=0; i