146 lines
4.3 KiB
C
146 lines
4.3 KiB
C
#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;
|
|
}
|