jaseg/master-thesis
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Fix frequency measurement simulation

Browse source
This commit is contained in:
jaseg 2020-03-04 17:10:31 +01:00
parent 4d80111cad
commit d9b26d16c0
22 changed files with 1201 additions and 164 deletions
Download patch file Download diff file Expand all files Collapse all files

71
controller/fw/src/freq_meas.c
View file

@ -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;
/* 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)];
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++) {
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;
}
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;
}

2
controller/fw/src/freq_meas.h
View file

@ -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__ */

14
controller/fw/src/simulation.h Normal file
View file

@ -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__ */