From 205daf0cab4dc82d4e5d3f4319471d662110e5c5 Mon Sep 17 00:00:00 2001 From: zxc <3269003127@qq.com> Date: Fri, 12 Jun 2026 15:39:15 +0800 Subject: [PATCH] =?UTF-8?q?=E4=B8=8A=E4=BC=A0=E6=96=87=E4=BB=B6=E8=87=B3?= =?UTF-8?q?=20/?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 更新sfra_f32.c --- sfra_f32.c | 454 +++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 454 insertions(+) create mode 100644 sfra_f32.c diff --git a/sfra_f32.c b/sfra_f32.c new file mode 100644 index 0000000..f2eafdd --- /dev/null +++ b/sfra_f32.c @@ -0,0 +1,454 @@ +/** + * @file sfra_f32.c + * @brief 软件频率响应分析器 (SFRA) 库实现，完全兼容 TI SFRA_F32 接口。 + * @details 实现功率级传函 H(s)、系统开环传函 GH(s) 和闭环传函 CL(s) 的测量。 + * 支持浮点运算，自动检测上位机参数变化并更新频率向量。 + */ + +#include "sfra_f32.h" +#include + +/*============================================================================ + * 常量定义 + *============================================================================*/ +#define TWO_PI (2.0f * 3.14159265358979323846f) +#define RAD_TO_DEG (180.0f / 3.14159265358979323846f) +#define INVALID_MAG_DB (-200.0f) + +/*============================================================================ + * 内部静态数据（单实例，非重入） + *============================================================================*/ +typedef struct { + uint16_t active; /**< 扫频是否激活 */ + uint16_t dtft_running; /**< 当前频率点是否正在累积 DTFT */ + uint16_t freqIndex; /**< 当前频率点索引 */ + uint16_t dataIndex; /**< 当前频率点的采样计数器 */ + uint16_t dataCount; /**< 当前频率点总采样数 */ + + float32_t foi_rad; /**< 每个采样的角度增量 (rad) */ + float32_t foi_sin; /**< 当前采样点 sin 值 */ + float32_t foi_cos; /**< 当前采样点 cos 值 */ + + /* DTFT 累加器 (使用 e^{-jwt} 核) */ + float32_t dtft_real_inj; /**< 注入信号 I 的实部 */ + float32_t dtft_imag_inj; /**< 注入信号 I 的虚部 */ + float32_t dtft_real_fb; /**< 反馈信号 Y 的实部 */ + float32_t dtft_imag_fb; /**< 反馈信号 Y 的虚部 */ + float32_t dtft_real_ctrl; /**< 控制输出 U 的实部 */ + float32_t dtft_imag_ctrl; /**< 控制输出 U 的虚部 */ + + /* 影子变量：用于检测上位机参数变更 */ + float32_t shadow_amplitude; /**< 上次同步的注入幅度 */ + float32_t shadow_freqStart; /**< 上次同步的起始频率 */ + float32_t shadow_freqStep; /**< 上次同步的频率步进乘数 */ + int16_t shadow_speed; /**< 上次同步的扫频速度 */ + + int16_t storeH; /**< 是否存储 H 向量 */ + int16_t storeGH; /**< 是否存储 GH 向量 */ + int16_t storeCL; /**< 是否存储 CL 向量 */ +} SFRA_Internal; + +static SFRA_Internal s_sfra = {0}; + +/*============================================================================ + * 辅助函数 + *============================================================================*/ + +/** + * @brief 计算指定频率点需要累积的采样点数（基于 libsfra 的周期对齐策略） + * @param foi_hz 注入频率 (Hz) + * @param isrFreq 控制 ISR 频率 (Hz) + * @param speed 速度因子（越大扫频越慢，累积周期越多） + * @return 采样点数 + */ +static uint16_t calcDataCount(float32_t foi_hz, float32_t isrFreq, int16_t speed) +{ + uint16_t cycles; + if (foi_hz < 10.0f) + cycles = 10; + else if (foi_hz < 100.0f) + cycles = (uint16_t)ceilf(foi_hz); + else + cycles = 100; + + cycles = (uint16_t)((float32_t)cycles * (float32_t)speed); + if (cycles < 4) cycles = 4; + + float32_t samples_per_cycle = isrFreq / foi_hz; + return (uint16_t)ceilf(samples_per_cycle * cycles); +} + +/** + * @brief 复数除法：计算 (num_re + j*num_im) / (den_re + j*den_im) + * @param re_num 分子实部 + * @param im_num 分子虚部 + * @param re_den 分母实部 + * @param im_den 分母虚部 + * @param mag 输出幅值 (dB) + * @param phase_deg 输出相位 (度) + */ +static void complexDiv(float32_t re_num, float32_t im_num, + float32_t re_den, float32_t im_den, + float32_t *mag, float32_t *phase_deg) +{ + float32_t den_sq = re_den * re_den + im_den * im_den; + if (den_sq < 1e-12f) { + *mag = INVALID_MAG_DB; + *phase_deg = 0.0f; + return; + } + float32_t re = (re_num * re_den + im_num * im_den) / den_sq; + float32_t im = (im_num * re_den - re_num * im_den) / den_sq; + *mag = 10.0f * log10f(re * re + im * im); + *phase_deg = atan2f(im, re) * RAD_TO_DEG; +} + +/** + * @brief 从指定索引开始重新生成频率向量（使用当前的 freqStart 和 freqStep） + * @param obj SFRA 对象指针 + * @param startIdx 起始索引（该点之前的频率点保持不变） + */ +static void regenerateFreqVectorFrom(SFRA_F32 *obj, uint16_t startIdx) +{ + if (!obj || !obj->freqVect || obj->vecLength <= 0) return; + if (startIdx >= (uint16_t)obj->vecLength) return; + + for (int16_t i = startIdx; i < obj->vecLength; i++) { + if (i == 0) { + obj->freqVect[0] = obj->freqStart; + } else { + obj->freqVect[i] = obj->freqVect[i-1] * obj->freqStep; + } + } +} + +/*============================================================================ + * 公开 API 实现 + *============================================================================*/ + +void SFRA_F32_reset(SFRA_F32 *obj) +{ + if (!obj) return; + obj->state = 0; + obj->status = 0; + obj->freqIndex = 0; + obj->start = 0; + + s_sfra.active = 0; + s_sfra.dtft_running = 0; + s_sfra.freqIndex = 0; + s_sfra.dataIndex = 0; + s_sfra.dataCount = 0; + s_sfra.foi_rad = 0; + s_sfra.foi_sin = 0; + s_sfra.foi_cos = 0; + s_sfra.dtft_real_inj = 0; + s_sfra.dtft_imag_inj = 0; + s_sfra.dtft_real_fb = 0; + s_sfra.dtft_imag_fb = 0; + s_sfra.dtft_real_ctrl = 0; + s_sfra.dtft_imag_ctrl = 0; + + s_sfra.shadow_amplitude = obj->amplitude; + s_sfra.shadow_freqStart = obj->freqStart; + s_sfra.shadow_freqStep = obj->freqStep; + s_sfra.shadow_speed = obj->speed; +} + +void SFRA_F32_config(SFRA_F32 *obj, + 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, + int16_t speed) +{ + if (!obj) return; + obj->isrFreq = isrFrequency; + obj->amplitude = injectionAmplitude; + obj->vecLength = noFreqPoints; + obj->freqStart = fraSweepStartFreq; + obj->freqStep = freqStep; + obj->speed = speed; + + obj->h_magVect = h_magVect; + obj->h_phaseVect = h_phaseVect; + obj->gh_magVect = gh_magVect; + obj->gh_phaseVect = gh_phaseVect; + obj->cl_magVect = cl_magVect; + obj->cl_phaseVect = cl_phaseVect; + obj->freqVect = freqVect; + + obj->storeH = (h_magVect && h_phaseVect) ? 1 : 0; + obj->storeGH = (gh_magVect && gh_phaseVect) ? 1 : 0; + obj->storeCL = (cl_magVect && cl_phaseVect) ? 1 : 0; + + s_sfra.storeH = obj->storeH; + s_sfra.storeGH = obj->storeGH; + s_sfra.storeCL = obj->storeCL; + + s_sfra.shadow_amplitude = injectionAmplitude; + s_sfra.shadow_freqStart = fraSweepStartFreq; + s_sfra.shadow_freqStep = freqStep; + s_sfra.shadow_speed = speed; + + SFRA_F32_reset(obj); +} + +void SFRA_F32_initFreqArrayWithLogSteps(SFRA_F32 *obj, + float32_t fra_sweep_start_freq, + float32_t freqStep) +{ + if (!obj || !obj->freqVect || obj->vecLength <= 0) return; + obj->freqVect[0] = fra_sweep_start_freq; + for (int16_t i = 1; i < obj->vecLength; i++) { + obj->freqVect[i] = obj->freqVect[i-1] * freqStep; + } + s_sfra.shadow_freqStart = fra_sweep_start_freq; + s_sfra.shadow_freqStep = freqStep; +} + +void SFRA_F32_resetFreqRespArray(SFRA_F32 *obj) +{ + if (!obj) return; + uint16_t len = obj->vecLength; + if (obj->storeH && obj->h_magVect && obj->h_phaseVect) { + for (uint16_t i = 0; i < len; i++) { + obj->h_magVect[i] = 0.0f; + obj->h_phaseVect[i] = 0.0f; + } + } + if (obj->storeGH && obj->gh_magVect && obj->gh_phaseVect) { + for (uint16_t i = 0; i < len; i++) { + obj->gh_magVect[i] = 0.0f; + obj->gh_phaseVect[i] = 0.0f; + } + } + if (obj->storeCL && obj->cl_magVect && obj->cl_phaseVect) { + for (uint16_t i = 0; i < len; i++) { + obj->cl_magVect[i] = 0.0f; + obj->cl_phaseVect[i] = 0.0f; + } + } +} + +void SFRA_F32_updateInjectionAmplitude(SFRA_F32 *obj, float32_t new_injection_amplitude) +{ + if (obj) { + obj->amplitude = new_injection_amplitude; + s_sfra.shadow_amplitude = new_injection_amplitude; + } +} + +/** + * @brief 注入函数：生成正弦扰动并叠加到参考值上。 + * @param ref 原始参考值 + * @return 叠加扰动后的参考值 + */ +float SFRA_F32_inject(float ref) +{ + if (!s_sfra.active || !s_sfra.dtft_running) return ref; + + float32_t angle = s_sfra.foi_rad * s_sfra.dataIndex; + s_sfra.foi_cos = cosf(angle); + s_sfra.foi_sin = sinf(angle); + return ref + s_sfra.shadow_amplitude * s_sfra.foi_cos; +} + +/** + * @brief 收集函数：在控制 ISR 中调用，累积 DTFT 数据。 + * @param control_output 控制输出指针（例如占空比） + * @param feedback 反馈信号指针（例如 ADC 读数） + */ +void SFRA_F32_collect(float *control_output, float *feedback) +{ + if (!s_sfra.active || !s_sfra.dtft_running) return; + if (!control_output || !feedback) return; + + float32_t ctrl = *control_output; + float32_t fb = *feedback; + float32_t cosv = s_sfra.foi_cos; + float32_t sinv = s_sfra.foi_sin; + + /* DTFT 采用 e^{-jwt} 核，虚部为负号 */ + s_sfra.dtft_real_fb += fb * cosv; + s_sfra.dtft_imag_fb -= fb * sinv; + + s_sfra.dtft_real_ctrl += ctrl * cosv; + s_sfra.dtft_imag_ctrl -= ctrl * sinv; + + float32_t inj = s_sfra.shadow_amplitude * cosv; + s_sfra.dtft_real_inj += inj * cosv; + s_sfra.dtft_imag_inj -= inj * sinv; + + s_sfra.dataIndex++; + if (s_sfra.dataIndex >= s_sfra.dataCount) { + s_sfra.dtft_running = 0; /* 当前频率点累积完成 */ + } +} + +/** + * @brief 后台任务：状态机，管理扫频流程，计算传函并存储结果。 + * @param obj SFRA 对象指针 + * @note 传函定义： + * - H(s) = Y / U (功率级传函，用于开环模式) + * - GH(s) = Y / (I - Y) (系统开环传函，闭环模式下测量) + * - CL(s) = Y / I (系统闭环传函) + */ +void SFRA_F32_runBackgroundTask(SFRA_F32 *obj) +{ + if (!obj) return; + + /*------------------------------------------------------------------------ + * 1. 扫频未激活时，检测参数变化并更新频率向量 + *------------------------------------------------------------------------*/ + if (!s_sfra.active) { + if (obj->amplitude != s_sfra.shadow_amplitude) { + s_sfra.shadow_amplitude = obj->amplitude; + } + if (obj->speed != s_sfra.shadow_speed) { + s_sfra.shadow_speed = obj->speed; + } + if (obj->freqStart != s_sfra.shadow_freqStart || + obj->freqStep != s_sfra.shadow_freqStep) { + regenerateFreqVectorFrom(obj, 0); + s_sfra.shadow_freqStart = obj->freqStart; + s_sfra.shadow_freqStep = obj->freqStep; + } + } + + /*------------------------------------------------------------------------ + * 2. 启动新扫频 + *------------------------------------------------------------------------*/ + if (!s_sfra.active && obj->start) { + s_sfra.active = 1; + s_sfra.freqIndex = 0; + obj->start = 0; + obj->state = 1; + obj->status = 1; + obj->freqIndex = 0; + + if (obj->freqVect && obj->vecLength > 0) { + float32_t freq = obj->freqVect[0]; + s_sfra.dataCount = calcDataCount(freq, obj->isrFreq, obj->speed); + + uint16_t cycles_raw; + if (freq < 10.0f) cycles_raw = 10; + else if (freq < 100.0f) cycles_raw = (uint16_t)ceilf(freq); + else cycles_raw = 100; + cycles_raw = (uint16_t)((float32_t)cycles_raw * obj->speed); + if (cycles_raw < 4) cycles_raw = 4; + + s_sfra.foi_rad = TWO_PI * (float32_t)cycles_raw / (float32_t)s_sfra.dataCount; + + s_sfra.dataIndex = 0; + s_sfra.dtft_real_inj = 0; + s_sfra.dtft_imag_inj = 0; + s_sfra.dtft_real_fb = 0; + s_sfra.dtft_imag_fb = 0; + s_sfra.dtft_real_ctrl = 0; + s_sfra.dtft_imag_ctrl = 0; + s_sfra.dtft_running = 1; + } + return; + } + + if (!s_sfra.active) { + obj->state = 0; + obj->status = 0; + return; + } + + if (s_sfra.dtft_running) return; + + /*------------------------------------------------------------------------ + * 3. 当前频率点 DTFT 已完成，计算并存储各类传函 + *------------------------------------------------------------------------*/ + uint16_t idx = s_sfra.freqIndex; + if (idx < (uint16_t)obj->vecLength) { + float32_t mag, phase; + + /* 功率级传函 H(s) = Y / U */ + if (s_sfra.storeH && obj->h_magVect && obj->h_phaseVect) { + complexDiv(s_sfra.dtft_real_fb, s_sfra.dtft_imag_fb, + s_sfra.dtft_real_ctrl, s_sfra.dtft_imag_ctrl, + &mag, &phase); + obj->h_magVect[idx] = mag; + obj->h_phaseVect[idx] = phase; + } + + /* 开环传函 GH(s) = Y / (I - Y) */ + if (s_sfra.storeGH && obj->gh_magVect && obj->gh_phaseVect) { + float32_t re_iy = s_sfra.dtft_real_inj - s_sfra.dtft_real_fb; + float32_t im_iy = s_sfra.dtft_imag_inj - s_sfra.dtft_imag_fb; + complexDiv(s_sfra.dtft_real_fb, s_sfra.dtft_imag_fb, + re_iy, im_iy, &mag, &phase); + obj->gh_magVect[idx] = mag; + obj->gh_phaseVect[idx] = phase; + } + + /* 闭环传函 CL(s) = Y / I */ + if (s_sfra.storeCL && obj->cl_magVect && obj->cl_phaseVect) { + complexDiv(s_sfra.dtft_real_fb, s_sfra.dtft_imag_fb, + s_sfra.dtft_real_inj, s_sfra.dtft_imag_inj, + &mag, &phase); + obj->cl_magVect[idx] = mag; + obj->cl_phaseVect[idx] = phase; + } + } + + /*------------------------------------------------------------------------ + * 4. 准备下一个频率点（如果参数已变化，则重新生成剩余频率向量） + *------------------------------------------------------------------------*/ + s_sfra.freqIndex++; + obj->freqIndex = s_sfra.freqIndex; + + if (s_sfra.freqIndex < (uint16_t)obj->vecLength) { + /* 检测频率参数变化，重新生成剩余频率点 */ + if (obj->freqStart != s_sfra.shadow_freqStart || + obj->freqStep != s_sfra.shadow_freqStep) { + regenerateFreqVectorFrom(obj, s_sfra.freqIndex); + s_sfra.shadow_freqStart = obj->freqStart; + s_sfra.shadow_freqStep = obj->freqStep; + } + if (obj->speed != s_sfra.shadow_speed) { + s_sfra.shadow_speed = obj->speed; + } + + float32_t freq = obj->freqVect[s_sfra.freqIndex]; + s_sfra.dataCount = calcDataCount(freq, obj->isrFreq, obj->speed); + + uint16_t cycles_raw; + if (freq < 10.0f) cycles_raw = 10; + else if (freq < 100.0f) cycles_raw = (uint16_t)ceilf(freq); + else cycles_raw = 100; + cycles_raw = (uint16_t)((float32_t)cycles_raw * obj->speed); + if (cycles_raw < 4) cycles_raw = 4; + + s_sfra.foi_rad = TWO_PI * (float32_t)cycles_raw / (float32_t)s_sfra.dataCount; + + s_sfra.dataIndex = 0; + s_sfra.dtft_real_inj = 0; + s_sfra.dtft_imag_inj = 0; + s_sfra.dtft_real_fb = 0; + s_sfra.dtft_imag_fb = 0; + s_sfra.dtft_real_ctrl = 0; + s_sfra.dtft_imag_ctrl = 0; + s_sfra.dtft_running = 1; + obj->state = 1; + obj->status = 1; + } else { + /* 扫频结束 */ + s_sfra.active = 0; + obj->state = 0; + obj->status = 2; + obj->freqIndex = obj->vecLength; + } +}