Add end-to-end simulation

1 files changed, 27 insertions, 0 deletions

diff --git a/controller/fw/src/freq_meas.c b/controller/fw/src/freq_meas.c
index 83a330c..020e12d 100644
--- a/controller/fw/src/freq_meas.c
+++ b/controller/fw/src/freq_meas.c
@@ -1,4 +1,8 @@
 
+#ifndef SIMULATION
+#include <stm32f407xx.h>
+#endif
+
 #include <unistd.h>
 #include <math.h>
 
@@ -57,7 +61,13 @@ int adc_buf_measure_freq(uint16_t adc_buf[FMEAS_FFT_LEN], float *out) {
         DEBUG_PRINTN("%010f, ", in_buf[i]);
     DEBUG_PRINTN("\n");
     */
+#ifndef SIMULATION
+    GPIOA->BSRR = 1<<12;
+#endif
     arm_rfft_fast_f32(&fft_inst, in_buf, out_buf, 0);
+#ifndef SIMULATION
+    GPIOA->BSRR = 1<<12<<16;
+#endif
 
 #define FMEAS_FFT_WINDOW_MIN_F_HZ 30.0f
 #define FMEAS_FFT_WINDOW_MAX_F_HZ 70.0f
@@ -66,6 +76,7 @@ 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)
@@ -76,6 +87,7 @@ int adc_buf_measure_freq(uint16_t adc_buf[FMEAS_FFT_LEN], float *out) {
     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++) {
         float real = out_buf[2 * (first_bin + i)];
@@ -83,7 +95,9 @@ int adc_buf_measure_freq(uint16_t adc_buf[FMEAS_FFT_LEN], float *out) {
         out_buf[i] = sqrtf(real*real + imag*imag);
     }
 
+    /*
     DEBUG_PRINT("Running Levenberg-Marquardt");
+    */
     LMstat lmstat;
     levmarq_init(&lmstat);
 
@@ -99,16 +113,29 @@ 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]);
+    */
 
+#ifndef SIMULATION
+    GPIOA->BSRR = 1<<12;
+#endif
     if (levmarq(3, par, nbins, out_buf, NULL, func_gauss, func_gauss_grad, NULL, &lmstat) < 0) {
+#ifndef SIMULATION
+        GPIOA->BSRR = 1<<12<<16;
+#endif
         *out = NAN;
         return -1;
     }
+#ifndef SIMULATION
+    GPIOA->BSRR = 1<<12<<16;
+#endif
 
+    /*
     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;
 }