201 lines
6.6 KiB
C
201 lines
6.6 KiB
C
#include "libsfra.h"
|
|
#include <math.h>
|
|
|
|
void sfra_init_all(void) {
|
|
#if(SFRA_INT)
|
|
fast_tri_init();
|
|
#endif
|
|
}
|
|
|
|
void sfra_start(sfra_t* sfra) {
|
|
sfra->internal_state.start = 1;
|
|
}
|
|
|
|
sfra_flag_t sfra_is_running(sfra_t* sfra) {
|
|
return sfra->internal_state.running;
|
|
}
|
|
|
|
sfra_flag_t sfra_is_done(sfra_t* sfra) {
|
|
return sfra->internal_state.done;
|
|
}
|
|
|
|
void sfra_clear_done(sfra_t* sfra) {
|
|
sfra->internal_state.done = 0;
|
|
}
|
|
|
|
|
|
static void set_float_if_nonnull(void* base, sfra_size_t offset, sfra_float_t value) {
|
|
if (base != 0) {
|
|
sfra_float_t* target = (sfra_float_t*) base;
|
|
target += offset;
|
|
*target = value;
|
|
}
|
|
}
|
|
|
|
static void process_foi_data(sfra_t* sfra) {
|
|
sfra_size_t freqIndex = sfra->internal_state.freqIndex;
|
|
sfra_float_t dtft_real_num = sfra->internal_state.dtft_real_num;
|
|
sfra_float_t dtft_nimg_num = sfra->internal_state.dtft_nimg_num;
|
|
sfra_float_t dtft_real_den = sfra->internal_state.dtft_real_den;
|
|
sfra_float_t dtft_nimg_den = sfra->internal_state.dtft_nimg_den;
|
|
|
|
set_float_if_nonnull(sfra->results.ctrl_real, freqIndex, dtft_real_den);
|
|
set_float_if_nonnull(sfra->results.ctrl_nimg, freqIndex, dtft_nimg_den);
|
|
set_float_if_nonnull(sfra->results.fb_real, freqIndex, dtft_real_num);
|
|
set_float_if_nonnull(sfra->results.fb_nimg, freqIndex, dtft_nimg_num);
|
|
|
|
// Calculate gain in dB
|
|
sfra_float_t mag = 10.0F * log10f(
|
|
(dtft_real_num*dtft_real_num + dtft_nimg_num*dtft_nimg_num) /
|
|
(dtft_real_den*dtft_real_den + dtft_nimg_den*dtft_nimg_den)
|
|
);
|
|
|
|
// Calculate phase in degrees
|
|
sfra_float_t re = dtft_real_num*dtft_real_den + dtft_nimg_num*dtft_nimg_den;
|
|
sfra_float_t im = dtft_real_num*dtft_nimg_den - dtft_nimg_num*dtft_real_den;
|
|
sfra_float_t phase = atanf(im/re) * 180.0F / M_PI;
|
|
if (re < 0.0F) {
|
|
if (im < 0.0F) {
|
|
phase = phase - 180.0F;
|
|
} else {
|
|
phase = phase + 180.0F;
|
|
}
|
|
}
|
|
|
|
set_float_if_nonnull(sfra->results.magnitudeVect, freqIndex, mag);
|
|
set_float_if_nonnull(sfra->results.phaseVect, freqIndex, phase);
|
|
}
|
|
|
|
static sfra_size_t calc_cycles(sfra_float_t foi_hz, sfra_float_t isrFreq) {
|
|
if (foi_hz < 10.0) {
|
|
// DC - 10Hz, attempt to reduce time consumption
|
|
return 10;
|
|
} else if (foi_hz < 100.0) {
|
|
// 10Hz - 100Hz, approximately 1 seconds per foi
|
|
return ceilf(foi_hz);
|
|
} else {
|
|
// 100Hz and above, collect 100 cycles
|
|
return 100;
|
|
}
|
|
}
|
|
|
|
static void setup_freq_point(sfra_t* sfra) {
|
|
sfra_float_t foi_hz = sfra->results.freqVect[sfra->internal_state.freqIndex];
|
|
sfra_size_t cycles = calc_cycles(foi_hz, sfra->config.isrFreq);
|
|
|
|
sfra->internal_state.data_count = ceilf(sfra->config.isrFreq * (float)cycles / foi_hz);
|
|
|
|
sfra->internal_state.foi_rad = (float)cycles / (float)sfra->internal_state.data_count *
|
|
#if(SFRA_INT)
|
|
(float) FAST_SIN_MAPPED_PI * (float) (1 << SFRA_OMEGA_DEC_BITS)
|
|
#else
|
|
2.0F * M_PI
|
|
#endif
|
|
;
|
|
|
|
sfra->internal_state.data_index = 0;
|
|
sfra->internal_state.dtft_real_num = 0;
|
|
sfra->internal_state.dtft_nimg_num = 0;
|
|
sfra->internal_state.dtft_real_den = 0;
|
|
sfra->internal_state.dtft_nimg_den = 0;
|
|
sfra->internal_state.dtft_running = 1;
|
|
}
|
|
|
|
void sfra_background_task(sfra_t* sfra) {
|
|
if (!sfra->internal_state.running) {
|
|
if (sfra->internal_state.start) {
|
|
// Start a frequency sweep
|
|
sfra->internal_state.start = 0;
|
|
sfra->internal_state.running = 1;
|
|
sfra->internal_state.freqIndex = 0;
|
|
sfra->results.freqVect[0] = sfra->config.freqStart;
|
|
setup_freq_point(sfra);
|
|
} else {
|
|
return;
|
|
}
|
|
}
|
|
|
|
if (sfra->internal_state.dtft_running) {
|
|
return;
|
|
}
|
|
|
|
process_foi_data(sfra);
|
|
|
|
sfra->internal_state.freqIndex++;
|
|
if (sfra->internal_state.freqIndex < sfra->config.vecLength) {
|
|
sfra->results.freqVect[sfra->internal_state.freqIndex] =
|
|
sfra->results.freqVect[sfra->internal_state.freqIndex - 1] *
|
|
sfra->config.freqStep;
|
|
setup_freq_point(sfra);
|
|
} else {
|
|
// End of frequency sweep
|
|
sfra->internal_state.running = 0;
|
|
sfra->internal_state.done = 1;
|
|
}
|
|
}
|
|
|
|
#if (SFRA_INT)
|
|
SFRA_RAMFUNC(sfra_inject_int32)
|
|
fast_tri_ret_type sfra_inject_int32(sfra_t* sfra) {
|
|
if (sfra->internal_state.dtft_running) {
|
|
fast_tri_omega_type omega = sfra->internal_state.foi_rad * sfra->internal_state.data_index;
|
|
omega >>= SFRA_OMEGA_DEC_BITS-1;
|
|
fast_tri_ret_type foi_cos = fast_cos(omega);
|
|
sfra->internal_state.foi_sin = fast_sin(omega);
|
|
sfra->internal_state.foi_cos = foi_cos;
|
|
return foi_cos;
|
|
} else {
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
SFRA_RAMFUNC(sfra_monitor_int32)
|
|
void sfra_monitor_int32(sfra_t* sfra, sfra_signal_t input, sfra_signal_t output) {
|
|
if (!sfra->internal_state.dtft_running)
|
|
return;
|
|
|
|
fast_tri_ret_type foi_cos = sfra->internal_state.foi_cos;
|
|
fast_tri_ret_type foi_sin = sfra->internal_state.foi_sin;
|
|
sfra->internal_state.dtft_real_num += (sfra_integral_t)output * (sfra_integral_t)foi_cos;
|
|
sfra->internal_state.dtft_nimg_num += (sfra_integral_t)output * (sfra_integral_t)foi_sin;
|
|
sfra->internal_state.dtft_real_den += (sfra_integral_t)input * (sfra_integral_t)foi_cos;
|
|
sfra->internal_state.dtft_nimg_den += (sfra_integral_t)input * (sfra_integral_t)foi_sin;
|
|
|
|
sfra->internal_state.data_index++;
|
|
if (sfra->internal_state.data_index >= sfra->internal_state.data_count) {
|
|
sfra->internal_state.dtft_running = 0;
|
|
}
|
|
}
|
|
#else
|
|
SFRA_RAMFUNC(sfra_inject)
|
|
sfra_float_t sfra_inject(sfra_t* sfra) {
|
|
if (sfra->internal_state.dtft_running) {
|
|
sfra_float_t omega = sfra->internal_state.foi_rad * sfra->internal_state.data_index;
|
|
sfra_float_t foi_cos = SFRA_FLOAT_COS(omega);
|
|
sfra->internal_state.foi_sin = SFRA_FLOAT_SIN(omega);
|
|
sfra->internal_state.foi_cos = foi_cos;
|
|
return foi_cos;
|
|
} else {
|
|
return 0.0F;
|
|
}
|
|
}
|
|
|
|
SFRA_RAMFUNC(sfra_monitor)
|
|
void sfra_monitor(sfra_t* sfra, sfra_float_t input, sfra_float_t output) {
|
|
if (!sfra->internal_state.dtft_running)
|
|
return;
|
|
|
|
sfra_float_t foi_cos = sfra->internal_state.foi_cos;
|
|
sfra_float_t foi_sin = sfra->internal_state.foi_sin;
|
|
sfra->internal_state.dtft_real_num += output * foi_cos;
|
|
sfra->internal_state.dtft_nimg_num += output * foi_sin;
|
|
sfra->internal_state.dtft_real_den += input * foi_cos;
|
|
sfra->internal_state.dtft_nimg_den += input * foi_sin;
|
|
|
|
sfra->internal_state.data_index++;
|
|
if (sfra->internal_state.data_index >= sfra->internal_state.data_count) {
|
|
sfra->internal_state.dtft_running = 0;
|
|
}
|
|
}
|
|
#endif
|