working on testing

3 files changed, 172 insertions, 50 deletions

diff --git a/controller/fw/Makefile b/controller/fw/Makefile
index c01b01c..ba22571 100644
--- a/controller/fw/Makefile
+++ b/controller/fw/Makefile
@@ -8,15 +8,28 @@ MUSL_DIR	?= musl
 
 C_SOURCES		:= src/main.c src/mspdebug_wrapper.c src/spi_flash.c src/freq_meas.c
 C_SOURCES		+= $(MSPDEBUG_DIR)/drivers/jtaglib.c
-C_SOURCES		+= $(CMSIS_DIR)/CMSIS/DSP/Source/TransformFunctions/arm_rfft_f32.c
-C_SOURCES		+= $(CMSIS_DIR)/CMSIS/DSP/Source/TransformFunctions/arm_bitreversal.c
-C_SOURCES		+= $(CMSIS_DIR)/CMSIS/DSP/Source/TransformFunctions/arm_cfft_radix4_f32.c
-C_SOURCES		+= $(MUSL_DIR)/src/math/tanhf.c $(MUSL_DIR)/src/math/atanhf.c
-C_SOURCES		+= $(MUSL_DIR)/src/math/expm1f.c $(MUSL_DIR)/src/math/log1pf.c
-C_SOURCES		+= $(MUSL_DIR)/src/math/expf.c $(MUSL_DIR)/src/math/exp2f_data.c
-C_SOURCES		+= $(MUSL_DIR)/src/math/__math_oflowf.c
-C_SOURCES		+= $(MUSL_DIR)/src/math/__math_uflowf.c
-C_SOURCES		+= $(MUSL_DIR)/src/math/__math_xflowf.c
+
+CMSIS_SOURCES		+= $(CMSIS_DIR)/CMSIS/DSP/Source/TransformFunctions/arm_rfft_fast_init_f32.c
+CMSIS_SOURCES		+= $(CMSIS_DIR)/CMSIS/DSP/Source/TransformFunctions/arm_rfft_fast_f32.c
+CMSIS_SOURCES		+= $(CMSIS_DIR)/CMSIS/DSP/Source/TransformFunctions/arm_cfft_init_f32.c
+CMSIS_SOURCES		+= $(CMSIS_DIR)/CMSIS/DSP/Source/TransformFunctions/arm_cfft_f32.c
+CMSIS_SOURCES		+= $(CMSIS_DIR)/CMSIS/DSP/Source/TransformFunctions/arm_cfft_radix8_f32.c
+CMSIS_SOURCES		+= $(CMSIS_DIR)/CMSIS/DSP/Source/CommonTables/arm_const_structs.c
+CMSIS_SOURCES		+= $(CMSIS_DIR)/CMSIS/DSP/Source/CommonTables/arm_common_tables.c
+CMSIS_SOURCES		+= $(CMSIS_DIR)/CMSIS/DSP/Source/TransformFunctions/arm_bitreversal.c
+CMSIS_SOURCES		+= $(CMSIS_DIR)/CMSIS/DSP/Source/TransformFunctions/arm_bitreversal2.c
+
+MUSL_SOURCES		+= $(MUSL_DIR)/src/math/tanhf.c $(MUSL_DIR)/src/math/atanhf.c
+MUSL_SOURCES		+= $(MUSL_DIR)/src/math/expm1f.c $(MUSL_DIR)/src/math/log1pf.c
+MUSL_SOURCES		+= $(MUSL_DIR)/src/math/expf.c $(MUSL_DIR)/src/math/exp2f_data.c
+MUSL_SOURCES		+= $(MUSL_DIR)/src/math/powf.c
+MUSL_SOURCES		+= $(MUSL_DIR)/src/math/__math_oflowf.c
+MUSL_SOURCES		+= $(MUSL_DIR)/src/math/__math_uflowf.c
+MUSL_SOURCES		+= $(MUSL_DIR)/src/math/__math_xflowf.c
+MUSL_SOURCES		+= $(MUSL_DIR)/src/math/__math_invalidf.c
+MUSL_SOURCES		+= $(MUSL_DIR)/src/math/powf_data.c
+
+C_SOURCES += $(CMSIS_SOURCES) $(MUSL_SOURCES)
 
 CXX_SOURCES		+= src/ldpc_wrapper.cpp
 
@@ -53,18 +66,23 @@ LIBSODIUM_DIR_ABS	:= $(abspath $(LIBSODIUM_DIR))
 TINYAES_DIR_ABS		:= $(abspath $(TINYAES_DIR))
 MUSL_DIR_ABS		:= $(abspath $(MUSL_DIR))
 
-CFLAGS		+= -I$(OPENCM3_DIR_ABS)/include -Imspdebug/util -Imspdebug/drivers
-CFLAGS		+= -I$(CMSIS_DIR_ABS)/CMSIS/DSP/Include -I$(CMSIS_DIR_ABS)/CMSIS/Core/Include -Dhidden=
-CFLAGS		+= -I$(abspath musl_include_shims) -Ilevmarq -D
+COMMON_CFLAGS		+= -I$(OPENCM3_DIR_ABS)/include -Imspdebug/util -Imspdebug/drivers
+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	+= -Os -std=gnu11 -g -DSTM32F4
+COMMON_CFLAGS	+= -Os -std=gnu11 -g -DSTM32F4
+CFLAGS			+= -mthumb -mcpu=cortex-m4 -mfloat-abi=hard -mfpu=fpv4-sp-d16
+SIM_CFLAGS		+= -mthumb -mcpu=cortex-m4 -mfloat-abi=soft
 # Note: libopencm3 requires some standard libc definitions from stdint.h and stdbool.h, so we don't pass -nostdinc here.
-CFLAGS	+= -nostdlib -ffreestanding
-CFLAGS	+= -mthumb -mcpu=cortex-m4 -mfloat-abi=hard -mfpu=fpv4-sp-d16
-CFLAGS	+= -fno-common -ffunction-sections -fdata-sections
-CFLAGS	+= -DGOLD_CODE_NBITS=$(GOLD_CODE_NBITS) -DFMEAS_FFT_LEN=$(FMEAS_FFT_LEN) -DFMEAS_ADC_MAX=$(FMEAS_ADC_MAX)
-CFLAGS	+= -DFMEAS_ADC_SAMPLING_RATE=$(FMEAS_ADC_SAMPLING_RATE) -DFMEAS_FFT_WINDOW=$(FMEAS_FFT_WINDOW)
-CFLAGS	+= -DFMEAS_FFT_WINDOW_SIGMA=$(FMEAS_FFT_WINDOW_SIGMA)
+COMMON_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	+= -DFMEAS_ADC_SAMPLING_RATE=$(FMEAS_ADC_SAMPLING_RATE)
+COMMON_CFLAGS	+= -DFMEAS_FFT_WINDOW_SIGMA=$(FMEAS_FFT_WINDOW_SIGMA)
+# for musl
+CFLAGS			+= -Dhidden=
+
+SIM_CFLAGS	+= -Isrc
 
 INT_CFLAGS	+= -Wall -Wextra -Wpedantic -Wshadow -Wimplicit-function-declaration -Wundef
 INT_CFLAGS	+= -Wredundant-decls -Wmissing-prototypes -Wstrict-prototypes
@@ -76,29 +94,36 @@ CXXFLAGS	+= -fno-common -ffunction-sections -fdata-sections
 CXXFLAGS	+= -Wall -Wextra -Wshadow -Wundef -Wredundant-decls
 CXXFLAGS	+= -I.
 
-LDFLAGS		+= --static -nostartfiles
-LDFLAGS		+= -mthumb -mcpu=cortex-m4 -mfloat-abi=hard -mfpu=fpv4-sp-d16 -g
-LDFLAGS		+= -Wl,--cre
-#LDFLAGS	+= -Wl,--gc-sections
-LDFLAGS		+= -Wl,--start-group -lc -lgcc -lnosys -Wl,--end-group
-LDFLAGS		+= -L$(OPENCM3_DIR_ABS)/lib -l$(OPENCM3_LIB) $(shell $(CC) -print-libgcc-file-name)
+COMMON_LDFLAGS	+= -mthumb -mcpu=cortex-m4 -mfloat-abi=hard -mfpu=fpv4-sp-d16 -g
+COMMON_LDFLAGS	+= --static -nostartfiles
+LDFLAGS			+= -Wl,--cre
+#LDFLAGS		+= -Wl,--gc-sections
+LDFLAGS			+= -Wl,--start-group -lc -lgcc -lnosys -Wl,--end-group
+LDFLAGS			+= -L$(OPENCM3_DIR_ABS)/lib -l$(OPENCM3_LIB) $(shell $(CC) -print-libgcc-file-name)
 
 all: $(BUILDDIR)/$(BINARY)
 
+tests: tools/freq_meas_test.elf
+
 OBJS := $(addprefix $(BUILDDIR)/,$(C_SOURCES:.c=.o) $(CXX_SOURCES:.cpp=.o))
 
-$(BUILDDIR)/$(BINARY): $(OBJS) \
-	$(OPENCM3_DIR)/lib/lib$(OPENCM3_LIB).a \
-	$(BUILDDIR)/libsodium/src/libsodium/.libs/libsodium.a \
-	$(BUILDDIR)/tinyaes/aes.o \
-	$(BUILDDIR)/levmarq/levmarq.o \
-	$(BUILDDIR)/generated/gold_code_$(GOLD_CODE_NBITS).o \
-	$(BUILDDIR)/generated/fmeas_fft_window.o
-	$(LD) -T$(LDSCRIPT) $(LDFLAGS) -o $@ -Wl,-Map=$(BUILDDIR)/src/$*.map $^
+ALL_OBJS := $(OBJS)
+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/fmeas_fft_window.o
+
+$(BUILDDIR)/$(BINARY): $(ALL_OBJS)
+	$(LD) -T$(LDSCRIPT) $(COMMON_LDFLAGS) $(LDFLAGS) -o $@ -Wl,-Map=$(BUILDDIR)/src/$*.map $^
+
+tools/freq_meas_test.elf: tools/freq_meas_test.c src/freq_meas.c $(CMSIS_SOURCES) $(BUILDDIR)/musl/lib/libc.a
+	$(CC) $(COMMON_CFLAGS) $(SIM_CFLAGS) -lm -lgcc -lnosys -o $@ $^
 
 $(BUILDDIR)/src/%.o: src/%.c
 	mkdir -p $(@D)
-	$(CC) $(CFLAGS) $(INT_CFLAGS) -o $@ -c $<
+	$(CC) $(COMMON_CFLAGS) $(CFLAGS) $(INT_CFLAGS) -o $@ -c $<
 
 $(BUILDDIR)/src/%.o: src/%.cpp
 	mkdir -p $(@D)
@@ -106,22 +131,22 @@ $(BUILDDIR)/src/%.o: src/%.cpp
 
 $(BUILDDIR)/generated/%.o: $(BUILDDIR)/generated/%.c
 	mkdir -p $(@D)
-	$(CC) $(CFLAGS) $(INT_CFLAGS) -o $@ -c $<
+	$(CC) $(COMMON_CFLAGS) $(CFLAGS) $(INT_CFLAGS) -o $@ -c $<
 
 $(BUILDDIR)/%.o: %.c
 	mkdir -p $(@D)
-	$(CC) $(CFLAGS) $(EXT_CFLAGS) -o $@ -c $<
+	$(CC) $(COMMON_CFLAGS) $(CFLAGS) $(EXT_CFLAGS) -o $@ -c $<
 
 $(OPENCM3_DIR)/lib/lib$(OPENCM3_LIB).a:
 	$(MAKE) -C $(OPENCM3_DIR) -j $(shell nproc)
 
 $(BUILDDIR)/libsodium/src/libsodium/.libs/libsodium.a:
 	mkdir -p $(BUILDDIR)/libsodium
-	cd $(BUILDDIR)/libsodium && CFLAGS="$(CFLAGS) -DDEV_MODE=1" $(LIBSODIUM_DIR_ABS)/configure --host=arm-none-eabi && $(MAKE) -j $(shell nproc)
+	cd $(BUILDDIR)/libsodium && CFLAGS="$(COMMON_CFLAGS) $(CFLAGS) -DDEV_MODE=1" $(LIBSODIUM_DIR_ABS)/configure --host=arm-none-eabi && $(MAKE) -j $(shell nproc)
 
 $(BUILDDIR)/tinyaes/aes.o:
 	mkdir -p $(@D)
-	make -C $(@D) -f $(TINYAES_DIR_ABS)/Makefile VPATH=$(TINYAES_DIR_ABS) CFLAGS="$(CFLAGS) -c" CC=$(CC) LD=$(LD) AR=$(AR) aes.o
+	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)
@@ -131,6 +156,9 @@ $(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)
 
 build/ldpc_decoder_test.so: src/ldpc_decoder.c
 	gcc -fPIC -shared -Wall -Wextra -Wpedantic -std=gnu11 -O0 -g -o $@ $^
diff --git a/controller/fw/src/freq_meas.c b/controller/fw/src/freq_meas.c
index e6b3976..627af34 100644
--- a/controller/fw/src/freq_meas.c
+++ b/controller/fw/src/freq_meas.c
@@ -26,7 +26,7 @@ extern arm_status arm_rfft_4096_fast_init_f32(arm_rfft_fast_instance_f32 * S);
 #define CONCAT(A, B, C) A ## B ## C
 #define arm_rfft_init_name(nbits) CONCAT(arm_rfft_, nbits, _fast_init_f32)
 
-float func_gauss_grad(float *out, float *params, int x, void *userdata);
+void func_gauss_grad(float *out, float *params, int x, void *userdata);
 float func_gauss(float *params, int x, void *userdata);
 
 int adc_buf_measure_freq(uint16_t adc_buf[FMEAS_FFT_LEN], float *out) {
@@ -42,12 +42,12 @@ int adc_buf_measure_freq(uint16_t adc_buf[FMEAS_FFT_LEN], float *out) {
 
     arm_rfft_fast_f32(&fft_inst, in_buf, out_buf, 0);
 
-#define FMEAS_FFT_WINDOW_MIN_F 30.0f
-#define FMEAS_FFT_WINDOW_MAX_F 70.0f
-    const float binsize = (float)FMEAS_ADC_SAMPLING_RATE / FMEAS_FFT_LEN;
-    const int first_bin = (int)(FMEAS_FFT_WINDOW_MIN_F / binsize);
-    const int last_bin = (int)(FMEAS_FFT_WINDOW_MAX_F / binsize + 0.5f);
-    const int nbins = last_bin - first_bin + 1;
+#define FMEAS_FFT_WINDOW_MIN_F_HZ 30.0f
+#define FMEAS_FFT_WINDOW_MAX_F_HZ 70.0f
+    const float binsize_hz = (float)FMEAS_ADC_SAMPLING_RATE / FMEAS_FFT_LEN;
+    const size_t first_bin = (int)(FMEAS_FFT_WINDOW_MIN_F_HZ / binsize_hz);
+    const size_t last_bin = (int)(FMEAS_FFT_WINDOW_MAX_F_HZ / binsize_hz + 0.5f);
+    const size_t nbins = last_bin - first_bin + 1;
 
     /* Copy real values of target data to front of output buffer */
     for (size_t i=0; i<nbins; i++)
@@ -69,10 +69,10 @@ int adc_buf_measure_freq(uint16_t adc_buf[FMEAS_FFT_LEN], float *out) {
         a_max, i_max, 1.0f
     };
 
-    if (levmarq(3, &params, nbins, out_buf, NULL, func_gauss, func_gauss_grad, NULL, &lmstat))
+    if (levmarq(3, par, nbins, out_buf, NULL, func_gauss, func_gauss_grad, NULL, &lmstat))
         return -1;
 
-    *out = (params[1] + first_bin) * binsize;
+    *out = (par[1] + first_bin) * binsize_hz;
 
     return 0;
 }
@@ -82,13 +82,13 @@ float func_gauss(float *params, int x, void *userdata) {
     float a = params[0];
     float mu = params[1];
     float sigma = params[2];
-    return a*expf(-arm_power_f32((x-mu), 2.0f/(2.0f*(sigma*sigma))));
+    return a*expf(-powf((x-mu), 2.0f/(2.0f*(sigma*sigma))));
 }
 
-float func_gauss_grad(float *out, float *params, int x, void *userdata) {
+void func_gauss_grad(float *out, float *params, int x, void *userdata) {
     UNUSED(userdata);
     float a = params[0];
     float mu = params[1];
     float sigma = params[2];
-    return -(x-mu) / (  sigma*sigma*sigma * 2.5066282746310002f) * a*expf(-arm_power_f32((x-mu), 2.0f/(2.0f*(sigma*sigma))));
+    *out = -(x-mu) / (  sigma*sigma*sigma * 2.5066282746310002f) * a*expf(-powf((x-mu), 2.0f/(2.0f*(sigma*sigma))));
 }
diff --git a/controller/fw/tools/freq_meas_test.c b/controller/fw/tools/freq_meas_test.c
new file mode 100644
index 0000000..01b4963
--- /dev/null
+++ b/controller/fw/tools/freq_meas_test.c
@@ -0,0 +1,94 @@
+
+#include <unistd.h>
+#include <stdio.h>
+#include <string.h>
+#include <errno.h>
+#include <stdlib.h>
+#include <sys/stat.h>
+#include <sys/types.h>
+#include <sys/fcntl.h>
+
+#include "freq_meas.h"
+
+void print_usage(void);
+
+void print_usage() {
+    fprintf(stderr, "Usage: freq_meas_test [test_data.bin]");
+}
+
+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;
+    }
+
+    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, "Error reading test data: %s\n", strerror(errno));
+            return 2;
+        }
+        
+        if (rc == 0) {
+            fprintf(stderr, "Error reading test data: Unexpected end of file\n");
+            return 2;
+        }
+
+        nread += rc;
+    }
+
+    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);
+    if (!sim_adc_buf) {
+        fprintf(stderr, "Error allocating memory\n");
+        return 2;
+    }
+
+    for (size_t i=0; i<n_samples; i++)
+        sim_adc_buf[i] = 2048 + buf_f[i] * 2047;
+
+    for (size_t i=0; i<n_samples; i+=FMEAS_FFT_LEN) {
+
+        float out;
+        int rc = adc_buf_measure_freq(sim_adc_buf + i, &out);
+        if (rc) {
+            fprintf(stderr, "Simulation error in iteration %zd at position %zd: %d\n", i/FMEAS_FFT_LEN, i, rc);
+            return 3;
+        }
+
+        printf("%09zd %015f\n", i, out);
+    }
+
+    return 0;
+}