455 lines
16 KiB
C
455 lines
16 KiB
C
/**
|
||
* @file sfra_f32.c
|
||
* @brief 软件频率响应分析器 (SFRA) 库实现，完全兼容 TI SFRA_F32 接口。
|
||
* @details 实现功率级传函 H(s)、系统开环传函 GH(s) 和闭环传函 CL(s) 的测量。
|
||
* 支持浮点运算，自动检测上位机参数变化并更新频率向量。
|
||
*/
|
||
|
||
#include "sfra_f32.h"
|
||
#include <math.h>
|
||
|
||
/*============================================================================
|
||
* 常量定义
|
||
*============================================================================*/
|
||
#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;
|
||
}
|
||
}
|