更新README

This commit is contained in:
ZXCLI
2026-06-12 16:22:17 +08:00
parent 205daf0cab
commit c0c195331c
693 changed files with 243421 additions and 1 deletions
+200
View File
@@ -0,0 +1,200 @@
#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
+131
View File
@@ -0,0 +1,131 @@
#ifndef INC_LIBSFRA_H_
#define INC_LIBSFRA_H_
#include <stdint.h>
#include "libsfra_config.h"
/*
* In some MCU/DSP, code runs much faster in RAM.
* This is particular for the ISR routine and the function it may call.
* Placing them in the memory yields short execution time and less penalty.
*
* On STM32 GCC + Makefile:
* 1. Declare macro in "libsfra_config.h":
* "#define SFRA_RAMFUNC(functionName) __attribute__ ((long_call, section (".ramfunc")))"
* 2. Add "*(.ramfunc)" to ".data" section.
*
*/
#ifndef SFRA_RAMFUNC
#define SFRA_RAMFUNC(functionName)
#endif
#if(SFRA_INT)
#include "fast_tri.h"
#define SFRA_OMEGA_DEC_BITS 8
typedef int32_t sfra_signal_t;
typedef int64_t sfra_integral_t;
#else // #if(SFRA_INT)
#include <math.h>
// Use float accelerated trigonometric functions.
#define SFRA_FLOAT_SIN(omega) sinf(omega)
#define SFRA_FLOAT_COS(omega) cosf(omega)
#endif // #if(SFRA_INT)
typedef float sfra_float_t;
typedef uint32_t sfra_size_t;
typedef int_least8_t sfra_flag_t;
typedef struct {
sfra_float_t *ctrl_real;
sfra_float_t *ctrl_nimg;
sfra_float_t *fb_real;
sfra_float_t *fb_nimg;
sfra_float_t *magnitudeVect;
sfra_float_t *phaseVect;
sfra_float_t *freqVect; //!< Frequency Vector
} sfra_result;
typedef struct {
sfra_float_t isrFreq; //!< SFRA ISR frequency
sfra_float_t freqStart; //!< Start frequency of SFRA sweep
sfra_float_t freqStep; //!< Log space between frequency points (optional)
sfra_size_t vecLength; //!< No. of Points in the SFRA
} sfra_setup;
typedef struct {
// Outer loop & state machine
sfra_flag_t start;
sfra_flag_t running;
sfra_flag_t done;
sfra_size_t freqIndex; //!< Index of the frequency vector
// Inner loop & state machine
volatile sfra_flag_t dtft_running; // Set and monitored by main(), clear by sfra_monitor() [in interrupt]
sfra_size_t data_count; // Set by main(), read by sfra_monitor() [in interrupt]
sfra_size_t data_index; // Clear by main(), read and write by sfra_monitor() [in interrupt]
#if(SFRA_INT)
fast_tri_omega_type foi_rad; // Set by main(), read by sfra_inject() [in interrupt]
fast_tri_ret_type foi_sin; // Pass intermediate value from sfra_inject() to sfra_monitor()
fast_tri_ret_type foi_cos; // Pass intermediate value from sfra_inject() to sfra_monitor()
volatile sfra_integral_t dtft_real_num; // Real part of x_out
volatile sfra_integral_t dtft_nimg_num; // Negative of imaginary part of x_out
volatile sfra_integral_t dtft_real_den; // Real part of x_control
volatile sfra_integral_t dtft_nimg_den; // Negative of imaginary part of x_control
#else
sfra_float_t foi_rad; // Set by main(), read by sfra_inject() [in interrupt]
sfra_float_t foi_sin; // Pass intermediate value from sfra_inject() to sfra_monitor()
sfra_float_t foi_cos; // Pass intermediate value from sfra_inject() to sfra_monitor()
volatile sfra_float_t dtft_real_num; // Real part of x_out
volatile sfra_float_t dtft_nimg_num; // Negative of imaginary part of x_out
volatile sfra_float_t dtft_real_den; // Real part of x_control
volatile sfra_float_t dtft_nimg_den; // Negative of imaginary part of x_control
#endif
} sfra_internal_state;
typedef struct {
sfra_result results;
sfra_setup config;
sfra_internal_state internal_state;
} sfra_t;
void sfra_init_all(void);
void sfra_start(sfra_t* sfra);
sfra_flag_t sfra_is_running(sfra_t* sfra);
sfra_flag_t sfra_is_done(sfra_t* sfra);
void sfra_clear_done(sfra_t* sfra);
void sfra_background_task(sfra_t* sfra);
#if(SFRA_INT)
/*
* Generate a sinusoidal perturbation with unit magnitude.
*
* @param sfra a pointer to an initialized sfra_t struct
* @return the sinusoidal perturbation, [-1,1] mapped to [-FAST_SIN_TABLE_SCALE, FAST_SIN_TABLE_SCALE]
*/
fast_tri_ret_type sfra_inject_int32(sfra_t* sfra);
/*
* Pass the signals to the "bode plotter". When measuring the open-loop transfer function of a closed-loop system,
* "control" and "feedback" should use the same base as the return value of sfra_inject_int32().
*
* @param sfra a pointer to an initialized sfra_t struct
* @param control the control output if measuring the open-loop transfer function of a closed-loop system.
* In a plant transfer function measurement, the perturbed reference signal (usually the duty-cycle) should be passed.
* @param feedback the feedback signal, usually is the ADC reading of the output voltage or current.
*/
void sfra_monitor_int32(sfra_t* sfra, sfra_signal_t control, sfra_signal_t feedback);
#else
sfra_float_t sfra_inject(sfra_t* sfra);
void sfra_monitor(sfra_t* sfra, sfra_float_t input, sfra_float_t output);
#endif
#if(SFRA_HAS_TEST)
void sfra_test_run(void);
void sfra_test_background_task(void);
#endif
#endif /* INC_LIBSFRA_H_ */
+15
View File
@@ -0,0 +1,15 @@
#ifndef INC_LIBSFRA_CONFIG_H_
#define INC_LIBSFRA_CONFIG_H_
// 使用浮点模式
#define SFRA_INT 0
// 不需要内置的测试代码
#define SFRA_HAS_TEST 0
#ifndef SFRA_RAMFUNC
#define SFRA_RAMFUNC(functionName)
#endif
#endif /* INC_LIBSFRA_CONFIG_H_ */
+145
View File
@@ -0,0 +1,145 @@
#include "libsfra_ti_hal.h"
#include <string.h>
// 静态适配器实例
static SFRA_F32 g_sfra_adapter;
static sfra_t *g_libsfra = NULL;
// 标志,用于检测 start 边沿
static int16_t g_last_start = 0;
// 需要同步的参数(上位机通过命令修改)
static float32_t g_last_amplitude;
// 内部函数:同步 libsfra 参数
static void sync_parameters(void) {
if (g_sfra_adapter.amplitude != g_last_amplitude) {
g_last_amplitude = g_sfra_adapter.amplitude;
}
if(g_sfra_adapter.freqStart != g_libsfra->config.freqStart){
g_libsfra->config.freqStart = g_sfra_adapter.freqStart;
}
if(g_sfra_adapter.freqStep != g_libsfra->config.freqStep){
g_libsfra->config.freqStep = g_sfra_adapter.freqStep;
}
}
// 检查 start 标志并启动扫频
static void check_start_flag(void) {
if (g_sfra_adapter.start == 1 && g_last_start == 0) {
sfra_start(g_libsfra);
g_sfra_adapter.start = 0; // 清除,避免重复触发
}
g_last_start = g_sfra_adapter.start;
}
// 更新状态和频率索引
static void update_status(void) {
g_sfra_adapter.freqIndex = g_libsfra->internal_state.freqIndex;
if (sfra_is_running(g_libsfra)) {
g_sfra_adapter.status = 1; // 运行中
} else if (sfra_is_done(g_libsfra)) {
g_sfra_adapter.status = 2; // 完成
sfra_clear_done(g_libsfra);
} else {
g_sfra_adapter.status = 0; // 空闲
}
}
// 注入函数,返回 ref + 扰动
float SFRA_F32_inject(float ref) {
float perturb = sfra_inject(g_libsfra); // 返回 -1..1
return ref + g_sfra_adapter.amplitude * perturb;
}
// 收集函数
void SFRA_F32_collect(float *control_output, float *feedback) {
sfra_monitor(g_libsfra, *control_output, *feedback);
}
// 后台任务
void SFRA_F32_runBackgroundTask(SFRA_F32 *obj) {
(void)obj; // 不使用参数,直接用全局
sync_parameters();
check_start_flag();
sfra_background_task(g_libsfra);
update_status();
}
// ------------------------------------------------------------
// 初始化适配器
// ------------------------------------------------------------
void libsfra_ti_hal_init(sfra_t *libsfra,
float32_t isrFrequency,
float32_t injectionAmplitude,
int16_t noFreqPoints,
float32_t fraSweepStartFreq,
float32_t freqStep,
float32_t *h_magVect,
float32_t *h_phaseVect,
float32_t *gh_magVect,
float32_t *gh_phaseVect,
float32_t *cl_magVect,
float32_t *cl_phaseVect,
float32_t *freqVect
)
{
g_libsfra = libsfra;
g_libsfra->config.isrFreq = isrFrequency;
g_libsfra->config.freqStart = fraSweepStartFreq;
g_libsfra->config.freqStep = freqStep;
g_libsfra->config.vecLength = noFreqPoints;
g_libsfra->results.freqVect = freqVect;
g_libsfra->results.magnitudeVect = h_magVect;
g_libsfra->results.phaseVect = h_phaseVect;
// 清空适配器结构体
memset(&g_sfra_adapter, 0, sizeof(SFRA_F32));
// 配置指针
g_sfra_adapter.freqVect = freqVect;
g_sfra_adapter.h_magVect = h_magVect;
g_sfra_adapter.h_phaseVect = h_phaseVect;
// H(s) 和 G(s)H(s) 的数据存储在同一个数组中
g_sfra_adapter.gh_magVect = h_magVect;
g_sfra_adapter.gh_phaseVect= h_phaseVect;
g_sfra_adapter.cl_magVect = cl_magVect;
g_sfra_adapter.cl_phaseVect= cl_phaseVect;
g_sfra_adapter.vecLength = noFreqPoints;
g_sfra_adapter.isrFreq = isrFrequency;
g_sfra_adapter.amplitude = injectionAmplitude;
g_sfra_adapter.freqStart = fraSweepStartFreq;
g_sfra_adapter.freqStep = freqStep;
g_sfra_adapter.speed = 1; // 默认速度
g_sfra_adapter.storeH = 1; // 存储 plant 数据
g_sfra_adapter.storeGH = 0;
g_sfra_adapter.storeCL = 0;
g_sfra_adapter.start = 0;
g_sfra_adapter.state = 0;
g_sfra_adapter.status = 0;
g_sfra_adapter.freqIndex = 0;
// 同步内部影子变量
g_last_amplitude = injectionAmplitude;
g_last_start = 0;
}
SFRA_F32* libsfra_ti_hal_get_adapter(void) {
return &g_sfra_adapter;
}
+36
View File
@@ -0,0 +1,36 @@
#ifndef LIBSFRA_TI_HAL_H
#define LIBSFRA_TI_HAL_H
#include "sfra_f32.h"
#include "libsfra.h"
#ifdef __cplusplus
extern "C" {
#endif
void libsfra_ti_hal_init(sfra_t *libsfra,
float32_t isrFrequency,
float32_t injectionAmplitude,
int16_t noFreqPoints,
float32_t fraSweepStartFreq,
float32_t freqStep,
float32_t *h_magVect,
float32_t *h_phaseVect,
float32_t *gh_magVect,
float32_t *gh_phaseVect,
float32_t *cl_magVect,
float32_t *cl_phaseVect,
float32_t *freqVect
);
/**
* 获取 TI 适配器结构体指针,用于传给 SFRA_GUI_config
*/
SFRA_F32* libsfra_ti_hal_get_adapter(void);
#ifdef __cplusplus
}
#endif
#endif