Finishing up freq meas

11 files changed, 250 insertions, 5 deletions

diff --git a/controller/fw/Makefile b/controller/fw/Makefile
index 291f840..c01b01c 100644
--- a/controller/fw/Makefile
+++ b/controller/fw/Makefile
@@ -6,13 +6,17 @@ LIBSODIUM_DIR	?= libsodium
 TINYAES_DIR	?= tinyaes
 MUSL_DIR	?= musl
 
-C_SOURCES		:= src/main.c src/mspdebug_wrapper.c src/spi_flash.c
+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
 
 CXX_SOURCES		+= src/ldpc_wrapper.cpp
 
@@ -24,6 +28,14 @@ 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 	?= 8.0
+
+
 CC		:= $(PREFIX)gcc
 CXX		:= $(PREFIX)g++
 LD		:= $(PREFIX)gcc
@@ -32,6 +44,7 @@ AS		:= $(PREFIX)as
 OBJCOPY		:= $(PREFIX)objcopy
 OBJDUMP		:= $(PREFIX)objdump
 GDB		:= $(PREFIX)gdb
+PYTHON3 ?= python3
 
 OPENCM3_DIR_ABS 	:= $(abspath $(OPENCM3_DIR))
 CMSIS_DIR_ABS 		:= $(abspath $(CMSIS_DIR))
@@ -41,14 +54,17 @@ 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
-CFLAGS		+= -I$(abspath musl_include_shims)
+CFLAGS		+= -I$(CMSIS_DIR_ABS)/CMSIS/DSP/Include -I$(CMSIS_DIR_ABS)/CMSIS/Core/Include -Dhidden=
+CFLAGS		+= -I$(abspath musl_include_shims) -Ilevmarq -D
 
 CFLAGS	+= -Os -std=gnu11 -g -DSTM32F4
 # 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)
 
 INT_CFLAGS	+= -Wall -Wextra -Wpedantic -Wshadow -Wimplicit-function-declaration -Wundef
 INT_CFLAGS	+= -Wredundant-decls -Wmissing-prototypes -Wstrict-prototypes
@@ -71,7 +87,13 @@ all: $(BUILDDIR)/$(BINARY)
 
 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)/$(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 $^
 
 $(BUILDDIR)/src/%.o: src/%.c
@@ -82,6 +104,10 @@ $(BUILDDIR)/src/%.o: src/%.cpp
 	mkdir -p $(@D)
 	$(CXX) $(CXXFLAGS) -o $@ -c $<
 
+$(BUILDDIR)/generated/%.o: $(BUILDDIR)/generated/%.c
+	mkdir -p $(@D)
+	$(CC) $(CFLAGS) $(INT_CFLAGS) -o $@ -c $<
+
 $(BUILDDIR)/%.o: %.c
 	mkdir -p $(@D)
 	$(CC) $(CFLAGS) $(EXT_CFLAGS) -o $@ -c $<
@@ -97,11 +123,21 @@ $(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
 
-ldpc_decoder_test.so: ldpc_decoder.c
+$(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) > $@
+
+
+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)
 
 mrproper: clean
diff --git a/controller/fw/levmarq b/controller/fw/levmarq
new file mode 160000
+Subproject d7734ce5a436b891d00dd3445dd0b57b1dfbe77
diff --git a/controller/fw/src/freq_meas.c b/controller/fw/src/freq_meas.c
new file mode 100644
index 0000000..e6b3976
--- /dev/null
+++ b/controller/fw/src/freq_meas.c
@@ -0,0 +1,94 @@
+
+#include <unistd.h>
+#include <math.h>
+
+#include <arm_math.h>
+#include <levmarq.h>
+
+#include "freq_meas.h"
+#include "sr_global.h"
+
+
+/* FTT window lookup table defined in generated/fmeas_fft_window.c */
+extern const float * const fmeas_fft_window_table;
+
+/* jury-rig some definitions for these functions since the ARM headers only export an over-generalized variable bin size
+ * variant. */
+extern arm_status arm_rfft_32_fast_init_f32(arm_rfft_fast_instance_f32 * S);
+extern arm_status arm_rfft_64_fast_init_f32(arm_rfft_fast_instance_f32 * S);
+extern arm_status arm_rfft_128_fast_init_f32(arm_rfft_fast_instance_f32 * S);
+extern arm_status arm_rfft_256_fast_init_f32(arm_rfft_fast_instance_f32 * S);
+extern arm_status arm_rfft_512_fast_init_f32(arm_rfft_fast_instance_f32 * S);
+extern arm_status arm_rfft_1024_fast_init_f32(arm_rfft_fast_instance_f32 * S);
+extern arm_status arm_rfft_2048_fast_init_f32(arm_rfft_fast_instance_f32 * S);
+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);
+float func_gauss(float *params, int x, void *userdata);
+
+int adc_buf_measure_freq(uint16_t adc_buf[FMEAS_FFT_LEN], float *out) {
+    int rc;
+    float in_buf[FMEAS_FFT_LEN];
+    float out_buf[FMEAS_FFT_LEN];
+    for (size_t i=0; i<FMEAS_FFT_LEN; i++)
+        in_buf[i] = (float)adc_buf[i] / (float)FMEAS_ADC_MAX * fmeas_fft_window_table[i];
+
+    arm_rfft_fast_instance_f32 fft_inst;
+    if ((rc = arm_rfft_init_name(FMEAS_FFT_LEN)(&fft_inst)) != ARM_MATH_SUCCESS)
+        return rc;
+
+    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;
+
+    /* Copy real values of target data to front of output buffer */
+    for (size_t i=0; i<nbins; i++)
+        out_buf[i] = out_buf[2 * (first_bin + i)];
+
+    LMstat lmstat;
+    levmarq_init(&lmstat);
+
+    float a_max = 0.0f;
+    int i_max = 0;
+    for (size_t i=0; i<nbins; i++) {
+        if (out_buf[i] > a_max) {
+            a_max = out_buf[i];
+            i_max = i;
+        }
+    }
+
+    float par[3] = {
+        a_max, i_max, 1.0f
+    };
+
+    if (levmarq(3, &params, nbins, out_buf, NULL, func_gauss, func_gauss_grad, NULL, &lmstat))
+        return -1;
+
+    *out = (params[1] + first_bin) * binsize;
+
+    return 0;
+}
+
+float func_gauss(float *params, int x, void *userdata) {
+    UNUSED(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))));
+}
+
+float 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))));
+}
diff --git a/controller/fw/src/freq_meas.h b/controller/fw/src/freq_meas.h
new file mode 100644
index 0000000..1c083f8
--- /dev/null
+++ b/controller/fw/src/freq_meas.h
@@ -0,0 +1,7 @@
+
+#ifndef __FREQ_MEAS_H__
+#define __FREQ_MEAS_H__
+
+int adc_buf_measure_freq(uint16_t adc_buf[FMEAS_FFT_LEN], float *out);
+
+#endif /* __FREQ_MEAS_H__ */
diff --git a/controller/fw/src/gold_code.h b/controller/fw/src/gold_code.h
new file mode 100644
index 0000000..739b477
--- /dev/null
+++ b/controller/fw/src/gold_code.h
@@ -0,0 +1,4 @@
+
+/* header file for generated gold code tables */
+
+extern const uint8_t * const gold_code_table;
diff --git a/controller/fw/ldpc_decoder.c b/controller/fw/src/ldpc_decoder.c
index fe59d77..fe59d77 100644
--- a/controller/fw/ldpc_decoder.c
+++ b/controller/fw/src/ldpc_decoder.c
diff --git a/controller/fw/ldpc_decoder_test.py b/controller/fw/src/ldpc_decoder_test.py
index 3b91bba..3b91bba 100644
--- a/controller/fw/ldpc_decoder_test.py
+++ b/controller/fw/src/ldpc_decoder_test.py
diff --git a/controller/fw/test_decoder.py b/controller/fw/src/test_decoder.py
index 8be5b02..8be5b02 100644
--- a/controller/fw/test_decoder.py
+++ b/controller/fw/src/test_decoder.py
diff --git a/controller/fw/test_pyldpc_utils.py b/controller/fw/src/test_pyldpc_utils.py
index 6b14532..6b14532 100644
--- a/controller/fw/test_pyldpc_utils.py
+++ b/controller/fw/src/test_pyldpc_utils.py
diff --git a/controller/fw/tools/fft_window_header_gen.py b/controller/fw/tools/fft_window_header_gen.py
new file mode 100644
index 0000000..7df2ee3
--- /dev/null
+++ b/controller/fw/tools/fft_window_header_gen.py
@@ -0,0 +1,59 @@
+#!/usr/bin/env python3
+
+import textwrap
+
+import scipy.signal as sig
+import numpy as np
+
+WINDOW_TYPES = [
+        'boxcar',
+        'triang',
+        'blackman',
+        'hamming',
+        'hann',
+        'bartlett',
+        'flattop',
+        'parzen',
+        'bohman',
+        'blackmanharris',
+        'nuttall',
+        'barthann',
+        'kaiser',
+        'gaussian',
+        'general_gaussian',
+        'slepian',
+        'dpss',
+        'chebwin',
+        'exponential',
+        'tukey',
+        ]
+
+if __name__ == '__main__':
+    import argparse
+    parser = argparse.ArgumentParser()
+    parser.add_argument('window', choices=WINDOW_TYPES, help='Type of window function to use')
+    parser.add_argument('n', type=int, help='Width of window in samples')
+    parser.add_argument('window_args', nargs='*', type=float,
+            help='''Window argument(s) if required. See https://docs.scipy.org/doc/scipy/reference/generated/scipy.signal.get_window.html#scipy.signal.get_window for details.''')
+    parser.add_argument('-v', '--variable', default='fft_window_table', help='Name for alias variable pointing to generated window')
+    args = parser.parse_args()
+    
+    print(f'/* FTT window table for {args.n} sample {args.window} window.')
+    if args.window_args:
+        print(f' * Window arguments were: ({" ,".join(str(arg) for arg in args.window_args)})')
+    print(f' */')
+    winargs = ''.join(f'_{arg:.4g}'.replace('.', 'F') for arg in args.window_args)
+    varname = f'fft_{args.n}_window_{args.window}{winargs}'
+    print(f'const float {varname}[{args.n}] = {{')
+
+    win = sig.get_window(args.window if not args.window_args else (args.window, *args.window_args),
+            Nx=args.n, fftbins=True)
+    par = ' '.join(f'{f:>013.8g},' 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/gold_code_header_gen.py b/controller/fw/tools/gold_code_header_gen.py
new file mode 100644
index 0000000..e2bc210
--- /dev/null
+++ b/controller/fw/tools/gold_code_header_gen.py
@@ -0,0 +1,45 @@
+#!/usr/bin/env python3
+
+import textwrap
+
+import numpy as np
+import scipy.signal as sig
+
+# From https://github.com/mubeta06/python/blob/master/signal_processing/sp/gold.py
+preferred_pairs = {5:[[2],[1,2,3]], 6:[[5],[1,4,5]], 7:[[4],[4,5,6]],
+                        8:[[1,2,3,6,7],[1,2,7]], 9:[[5],[3,5,6]], 
+                        10:[[2,5,9],[3,4,6,8,9]], 11:[[9],[3,6,9]]}
+
+def gen_gold(seq1, seq2):
+    gold = [seq1, seq2]
+    for shift in range(len(seq1)):
+        gold.append(seq1 ^ np.roll(seq2, -shift))
+    return gold
+
+def gold(n):
+    n = int(n)
+    if not n in preferred_pairs:
+        raise KeyError('preferred pairs for %s bits unknown' % str(n))
+    t0, t1 = preferred_pairs[n]
+    (seq0, _st0), (seq1, _st1) = sig.max_len_seq(n, taps=t0), sig.max_len_seq(n, taps=t1)
+    return gen_gold(seq0, seq1)
+
+if __name__ == '__main__':
+    import argparse
+    parser = argparse.ArgumentParser()
+    parser.add_argument('n', type=int, choices=preferred_pairs, help='bit width of shift register. Generate 2**n + 1 sequences of length 2**n - 1.')
+    args = parser.parse_args()
+
+    print('#include <unistd.h>')
+    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;')