Fix frequency measurement simulation

7 files changed, 289 insertions, 174 deletions

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 <arm_math.h>
 
 #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<nit; it++) {
-
-    /* calculate the approximation to the Hessian and the "derivative" d */
-    for (i=0; i<npar; i++) {
-      d[i] = 0;
-      for (j=0; j<=i; j++)
-	h[i][j] = 0;
-    }
-    for (x=0; x<ny; x++) {
-      if (dysq) weight = 1/dysq[x]; /* for weighted least-squares */
-      grad(g, par, x, fdata);
-      for (i=0; i<npar; i++) {
-	d[i] += (y[x] - func(par, x, fdata))*g[i]*weight;
-	for (j=0; j<=i; j++)
-	  h[i][j] += g[i]*g[j]*weight;
-      }
-    }
-
-    /*  make a step "delta."  If the step is rejected, increase
-       lambda and try again */
-    mult = 1 + lambda;
-    ill = 1; /* ill-conditioned? */
-    while (ill && (it<nit)) {
-      for (i=0; i<npar; i++)
-	h[i][i] = h[i][i]*mult;
-
-      ill = cholesky_decomp(npar, ch, h);
-
-      if (!ill) {
-	solve_axb_cholesky(npar, ch, delta, d);
-	for (i=0; i<npar; i++)
-	  newpar[i] = par[i] + delta[i];
-	newerr = error_func(newpar, ny, y, dysq, func, fdata);
-	derr = newerr - err;
-	ill = (derr > 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<nit; it++) {
+        //DEBUG_PRINT("iteration %d", it);
+
+        /* calculate the approximation to the Hessian and the "derivative" d */
+        for (i=0; i<npar; i++) {
+            d[i] = 0;
+            for (j=0; j<=i; j++)
+                h[i][j] = 0;
+        }
+
+        for (x=0; x<ny; x++) {
+            if (dysq) weight = 1/dysq[x]; /* for weighted least-squares */
+            grad(g, par, x, fdata);
+            for (i=0; i<npar; i++) {
+                d[i] += (y[x] - func(par, x, fdata))*g[i]*weight;
+                for (j=0; j<=i; j++)
+                    h[i][j] += g[i]*g[j]*weight;
+            }
+        }
+
+        /*  make a step "delta."  If the step is rejected, increase
+            lambda and try again */
+        mult = 1 + lambda;
+        ill = 1; /* ill-conditioned? */
+        while (ill && (it<nit)) {
+            for (i=0; i<npar; i++)
+                h[i][i] = h[i][i]*mult;
+
+            ill = cholesky_decomp(npar, ch, h);
+
+            if (!ill) {
+                solve_axb_cholesky(npar, ch, delta, d);
+                for (i=0; i<npar; i++)
+                    newpar[i] = par[i] + delta[i];
+                newerr = error_func(newpar, ny, y, dysq, func, fdata);
+                derr = newerr - err;
+                ill = (derr > 0);
+            } 
+            if (ill) {
+                mult = (1 + lambda*up)/(1 + lambda);
+                lambda *= up;
+                it++;
+            }
+        }
+        for (i=0; i<npar; i++)
+            par[i] = newpar[i];
+        err = newerr;
+        lambda *= down;  
+
+        if ((!ill)&&(-derr<target_derr)) break;
     }
-    for (i=0; i<npar; i++)
-      par[i] = newpar[i];
-    err = newerr;
-    lambda *= down;  
 
-    if ((!ill)&&(-derr<target_derr)) break;
-  }
+    lmstat->final_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<ny; x++) {
-    res = func(par, x, fdata) - y[x];
-    if (dysq)  /* weighted least-squares */
-      e += res*res/dysq[x];
-    else
-      e += res*res;
-  }
-  return e;
+    int x;
+    float res,e=0;
+
+    for (x=0; x<ny; x++) {
+        res = func(par, x, fdata) - y[x];
+        if (dysq)  /* weighted least-squares */
+            e += res*res/dysq[x];
+        else
+            e += res*res;
+    }
+    return e;
 }
 
 
 /* solve the equation Ax=b for a symmetric positive-definite matrix A,
    using the Cholesky decomposition A=LL^T.  The matrix L is passed in "l".
    Elements above the diagonal are ignored.
-*/
+   */
 void solve_axb_cholesky(int n, float l[n][n], float x[n], float b[n])
 {
-  int i,j;
-  float sum;
-
-  /* solve L*y = b for y (where x[] is used to store y) */
-
-  for (i=0; i<n; i++) {
-    sum = 0;
-    for (j=0; j<i; j++)
-      sum += l[i][j] * x[j];
-    x[i] = (b[i] - sum)/l[i][i];      
-  }
-
-  /* solve L^T*x = y for x (where x[] is used to store both y and x) */
-
-  for (i=n-1; i>=0; i--) {
-    sum = 0;
-    for (j=i+1; j<n; j++)
-      sum += l[j][i] * x[j];
-    x[i] = (x[i] - sum)/l[i][i];      
-  }
+    int i,j;
+    float sum;
+
+    /* solve L*y = b for y (where x[] is used to store y) */
+
+    for (i=0; i<n; i++) {
+        sum = 0;
+        for (j=0; j<i; j++)
+            sum += l[i][j] * x[j];
+        x[i] = (b[i] - sum)/l[i][i];      
+    }
+
+    /* solve L^T*x = y for x (where x[] is used to store both y and x) */
+
+    for (i=n-1; i>=0; i--) {
+        sum = 0;
+        for (j=i+1; j<n; j++)
+            sum += l[j][i] * x[j];
+        x[i] = (x[i] - sum)/l[i][i];      
+    }
 }
 
 
@@ -190,26 +200,26 @@ void solve_axb_cholesky(int n, float l[n][n], float x[n], float b[n])
    its (lower-triangular) Cholesky factor in "l".  Elements above the
    diagonal are neither used nor modified.  The same array may be passed
    as both l and a, in which case the decomposition is performed in place.
-*/
+   */
 int cholesky_decomp(int n, float l[n][n], float a[n][n])
 {
-  int i,j,k;
-  float sum;
-
-  for (i=0; i<n; i++) {
-    for (j=0; j<i; j++) {
-      sum = 0;
-      for (k=0; k<j; k++)
-	sum += l[i][k] * l[j][k];
-      l[i][j] = (a[i][j] - sum)/l[j][j];
+    int i,j,k;
+    float sum;
+
+    for (i=0; i<n; i++) {
+        for (j=0; j<i; j++) {
+            sum = 0;
+            for (k=0; k<j; k++)
+                sum += l[i][k] * l[j][k];
+            l[i][j] = (a[i][j] - sum)/l[j][j];
+        }
+
+        sum = 0;
+        for (k=0; k<i; k++)
+            sum += l[i][k] * l[i][k];
+        sum = a[i][i] - sum;
+        if (sum<TOL) return 1; /* not positive-definite */
+        l[i][i] = sqrtf(sum);
     }
-
-    sum = 0;
-    for (k=0; k<i; k++)
-      sum += l[i][k] * l[i][k];
-    sum = a[i][i] - sum;
-    if (sum<TOL) return 1; /* not positive-definite */
-    l[i][i] = sqrtf(sum);
-  }
-  return 0;
+    return 0;
 }
diff --git a/controller/fw/src/freq_meas.c b/controller/fw/src/freq_meas.c
index 627af34..ba7801e 100644
--- a/controller/fw/src/freq_meas.c
+++ b/controller/fw/src/freq_meas.c
@@ -7,6 +7,7 @@
 
 #include "freq_meas.h"
 #include "sr_global.h"
+#include "simulation.h"
 
 
 /* FTT window lookup table defined in generated/fmeas_fft_window.c */
@@ -29,17 +30,33 @@ extern arm_status arm_rfft_4096_fast_init_f32(arm_rfft_fast_instance_f32 * S);
 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) {
+int adc_buf_measure_freq(int16_t adc_buf[FMEAS_FFT_LEN], float *out) {
     int rc;
     float in_buf[FMEAS_FFT_LEN];
     float out_buf[FMEAS_FFT_LEN];
+    /*
+    DEBUG_PRINTN("    [emulated adc buf] ");
+    for (size_t i=0; i<FMEAS_FFT_LEN; i++)
+        DEBUG_PRINTN("%5d, ", adc_buf[i]);
+    DEBUG_PRINTN("\n");
+    */
+    //DEBUG_PRINT("Applying window function");
     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];
 
+    //DEBUG_PRINT("Running FFT");
     arm_rfft_fast_instance_f32 fft_inst;
-    if ((rc = arm_rfft_init_name(FMEAS_FFT_LEN)(&fft_inst)) != ARM_MATH_SUCCESS)
+    if ((rc = arm_rfft_init_name(FMEAS_FFT_LEN)(&fft_inst)) != ARM_MATH_SUCCESS) {
+        *out = NAN;
         return rc;
+    }
 
+    /*
+    DEBUG_PRINTN("    [input] ");
+    for (size_t i=0; i<FMEAS_FFT_LEN; i++)
+        DEBUG_PRINTN("%010f, ", in_buf[i]);
+    DEBUG_PRINTN("\n");
+    */
     arm_rfft_fast_f32(&fft_inst, in_buf, out_buf, 0);
 
 #define FMEAS_FFT_WINDOW_MIN_F_HZ 30.0f
@@ -49,10 +66,24 @@ int adc_buf_measure_freq(uint16_t adc_buf[FMEAS_FFT_LEN], float *out) {
     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;
 
+    DEBUG_PRINT("binsize_hz=%f first_bin=%zd last_bin=%zd nbins=%zd", binsize_hz, first_bin, last_bin, nbins);
+    DEBUG_PRINTN("    [bins real] ");
+    for (size_t i=0; i<FMEAS_FFT_LEN/2; i+=2)
+        DEBUG_PRINTN("%010f, ", out_buf[i]);
+    DEBUG_PRINTN("\n    [bins imag] ");
+    for (size_t i=1; i<FMEAS_FFT_LEN/2; i+=2)
+        DEBUG_PRINTN("%010f, ", out_buf[i]);
+    DEBUG_PRINT("\n");
+
+    DEBUG_PRINT("Repacking FFT results");
     /* 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)];
+    for (size_t i=0; i<nbins; i++) {
+        float real = out_buf[2 * (first_bin + i)];
+        float imag = out_buf[2 * (first_bin + i) + 1];
+        out_buf[i] = sqrtf(real*real + imag*imag);
+    }
 
+    DEBUG_PRINT("Running Levenberg-Marquardt");
     LMstat lmstat;
     levmarq_init(&lmstat);
 
@@ -68,27 +99,37 @@ int adc_buf_measure_freq(uint16_t adc_buf[FMEAS_FFT_LEN], float *out) {
     float par[3] = {
         a_max, i_max, 1.0f
     };
+    DEBUG_PRINT("    par_pre={%010f, %010f, %010f}", par[0], par[1], par[2]);
 
-    if (levmarq(3, par, 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) < 0) {
+        *out = NAN;
         return -1;
+    }
 
-    *out = (par[1] + first_bin) * binsize_hz;
+    DEBUG_PRINT("    par_post={%010f, %010f, %010f}", par[0], par[1], par[2]);
 
+    DEBUG_PRINT("done.");
+    *out = (par[1] + first_bin) * binsize_hz;
     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(-powf((x-mu), 2.0f/(2.0f*(sigma*sigma))));
+    float a = params[0], b = params[1], c = params[2];
+    float n = x-b;
+    return a*expf(-n*n / (2.0f* c*c));
 }
 
 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];
-    *out = -(x-mu) / (  sigma*sigma*sigma * 2.5066282746310002f) * a*expf(-powf((x-mu), 2.0f/(2.0f*(sigma*sigma))));
+    float a = params[0], b = params[1], c = params[2];
+    float n = x-b;
+    float e = expf(-n*n / (2.0f * c*c));
+    
+    /* d/da */
+    out[0] = e;
+    /* d/db */
+    out[1] = a*n/(c*c) * e;
+    /* d/dc */
+    out[2] = a*n*n/(c*c*c) * e;
 }
diff --git a/controller/fw/src/freq_meas.h b/controller/fw/src/freq_meas.h
index 1c083f8..4786c0e 100644
--- a/controller/fw/src/freq_meas.h
+++ b/controller/fw/src/freq_meas.h
@@ -2,6 +2,6 @@
 #ifndef __FREQ_MEAS_H__
 #define __FREQ_MEAS_H__
 
-int adc_buf_measure_freq(uint16_t adc_buf[FMEAS_FFT_LEN], float *out);
+int adc_buf_measure_freq(int16_t adc_buf[FMEAS_FFT_LEN], float *out);
 
 #endif /* __FREQ_MEAS_H__ */
diff --git a/controller/fw/src/simulation.h b/controller/fw/src/simulation.h
new file mode 100644
index 0000000..e813de7
--- /dev/null
+++ b/controller/fw/src/simulation.h
@@ -0,0 +1,14 @@
+#ifndef __SIMULATION_H__
+#define __SIMULATION_H__
+
+#ifdef SIMULATION
+#include <stdio.h>
+#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 <stdint.h>
+#include <math.h>
 #include <unistd.h>
 #include <stdio.h>
 #include <string.h>
@@ -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<n_samples; i++)
-        sim_adc_buf[i] = 2048 + buf_f[i] * 2047;
+        /* Note on scaling: We can't simply scale by 0x8000 (1/2 full range) here. Our test data is nominally 1Vp-p but
+         * certain tests such as the interharmonics one can have some samples exceeding that range. */
+        sim_adc_buf[i] = buf_f[i] * (0x4000-1);
+    fprintf(stderr, " done.\n");
 
-    for (size_t i=0; i<n_samples; i+=FMEAS_FFT_LEN) {
+    fprintf(stderr, "Starting simulation.\n");
 
-        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;
-        }
+    size_t iterations = (n_samples-FMEAS_FFT_LEN)/(FMEAS_FFT_LEN/2);
+    for (size_t i=0; i<iterations; i++) {
 
-        printf("%09zd %015f\n", i, out);
-    }
+        fprintf(stderr, "Iteration %zd/%zd\n", i, iterations);
+        float res = NAN;
+        int rc = adc_buf_measure_freq(sim_adc_buf + i*(FMEAS_FFT_LEN/2), &res);
+        if (rc)
+            printf("ERROR: Simulation error in iteration %zd at position %zd: %d\n", i, i*(FMEAS_FFT_LEN/2), rc);
 
+        printf("%09zd %12f\n", i, res);
+    }
+    
     return 0;
 }
diff --git a/controller/fw/tools/freq_meas_test_runner.py b/controller/fw/tools/freq_meas_test_runner.py
new file mode 100644
index 0000000..779922a
--- /dev/null
+++ b/controller/fw/tools/freq_meas_test_runner.py
@@ -0,0 +1,39 @@
+#!/usr/bin/env python3
+
+import os
+from os import path
+import subprocess
+import json
+
+import numpy as np
+np.set_printoptions(linewidth=240)
+
+
+if __name__ == '__main__':
+    import argparse
+    parser = argparse.ArgumentParser()
+    parser.add_argument(metavar='test_data_directory', dest='dir', help='Directory with test data .bin files')
+    default_binary = path.abspath(path.join(path.dirname(__file__), '../build/tools/freq_meas_test'))
+    parser.add_argument(metavar='test_binary', dest='binary', nargs='?', default=default_binary)
+    parser.add_argument('-d', '--dump', help='Write raw measurements to JSON file')
+    args = parser.parse_args()
+
+    bin_files = [ path.join(args.dir, d) for d in os.listdir(args.dir) if d.lower().endswith('.bin') ]
+
+    savedata = {}
+    for p in bin_files:
+        output = subprocess.check_output([args.binary, p], stderr=subprocess.DEVNULL)
+        measurements = np.array([ float(value) for _offset, value in [ line.split() for line in output.splitlines() ] ])
+        savedata[p] = list(measurements)
+
+        # Cut off first and last sample for mean and RMS calculations as these show boundary effects.
+        measurements = measurements[1:-1]
+        mean = np.mean(measurements)
+        rms = np.sqrt(np.mean(np.square(measurements - mean)))
+
+        print(f'{path.basename(p):<60}:    mean={mean:<8.4f}Hz rms={rms*1000:.3f}mHz')
+
+    if args.dump:
+        with open(args.dump, 'w') as f:
+            json.dump(savedata, f)
+