258 lines
7.2 KiB
C
258 lines
7.2 KiB
C
#include "sfra_f32.h"
|
||
#include "sfra_test.h"
|
||
#include "libsfra.h"
|
||
#include "lowpass.h"
|
||
#include "sfra_gui_scicomms_driverlib.h"
|
||
#include <stdio.h>
|
||
#include "libsfra_ti_hal.h"
|
||
|
||
#define TI_SFRA 0
|
||
#define LIBSFRA 1
|
||
#define LIBSFRA_HAL_TI 2
|
||
|
||
#define SFRA_TYPE LIBSFRA_HAL_TI
|
||
|
||
// sfra变量定义
|
||
SFRA_F32 ti_sfra;
|
||
|
||
#define CONTROL_ISR_FREQUENCY ((float32_t)100 * 1000) // 100KHz
|
||
|
||
#define SFRA_ISR_FREQ CONTROL_ISR_FREQUENCY
|
||
#define SFRA_FREQ_START 10
|
||
//
|
||
// SFRA step Multiply = 10^(1/No of steps per decade(40))
|
||
//
|
||
#define SFRA_FREQ_STEP_MULTIPLY (float32_t)1.105
|
||
#define SFRA_AMPLITUDE (float32_t)0.1
|
||
#define SFRA_FREQ_LENGTH 100
|
||
|
||
float32_t plantMagVect[SFRA_FREQ_LENGTH];
|
||
float32_t plantPhaseVect[SFRA_FREQ_LENGTH];
|
||
float32_t olMagVect[SFRA_FREQ_LENGTH];
|
||
float32_t olPhaseVect[SFRA_FREQ_LENGTH];
|
||
float32_t clMagVect[SFRA_FREQ_LENGTH];
|
||
float32_t clPhaseVect[SFRA_FREQ_LENGTH];
|
||
float32_t freqVect[SFRA_FREQ_LENGTH];
|
||
//
|
||
//extern to access tables in ROM
|
||
//
|
||
extern long FPUsinTable[];
|
||
|
||
|
||
// libsfra变量定义
|
||
// Size of each SFRA result buffer field
|
||
#define SFRA_BUF_SIZE 100
|
||
|
||
typedef struct {
|
||
float freq[SFRA_BUF_SIZE]; // In Hz, log-spaced
|
||
float mag[SFRA_BUF_SIZE]; // In dB
|
||
float phase[SFRA_BUF_SIZE]; // In degrees
|
||
} sfra_results_t;
|
||
|
||
// Holds measured bode plot
|
||
sfra_results_t libsfra_results;
|
||
|
||
// Declare the SFRA instance
|
||
sfra_t libsfra = {
|
||
.config.isrFreq = CONTROL_ISR_FREQUENCY, // Sampling frequency
|
||
.config.freqStart = 10, // Start frequency
|
||
.config.freqStep = 1.584893192461113, // Frequency step (log-spaced)
|
||
.config.vecLength = SFRA_BUF_SIZE, // Number of sweeps
|
||
.results.freqVect = libsfra_results.freq, // Points to the frequency vector
|
||
.results.magnitudeVect = libsfra_results.mag, // Points to the magnitude vector
|
||
.results.phaseVect = libsfra_results.phase // Points to the phase vector
|
||
};
|
||
|
||
|
||
// hal层变量
|
||
sfra_t hal_sfra;
|
||
|
||
|
||
|
||
// lowpass filter
|
||
LowPassFilter_t lowPass_test;
|
||
#define FS CONTROL_ISR_FREQUENCY
|
||
#define FC 10.0f * 1000
|
||
|
||
|
||
// 通信串口，LED
|
||
#define SFRA_GUI_SCI_BASE SCIA_BASE
|
||
#define SFRA_GUI_VBUS_CLK DEVICE_LSPCLK_FREQ
|
||
#define SFRA_GUI_SCI_BAUDRATE 115200
|
||
#define SFRA_GUI_SCIRX_GPIO 43
|
||
#define SFRA_GUI_SCITX_GPIO 42
|
||
#define SFRA_GUI_SCIRX_GPIO_PIN_CONFIG GPIO_43_SCIRXDA
|
||
#define SFRA_GUI_SCITX_GPIO_PIN_CONFIG GPIO_42_SCITXDA
|
||
|
||
#define SFRA_GUI_LED_INDICATOR 1
|
||
#define SFRA_GUI_LED_GPIO 31
|
||
#define SFRA_GUI_LED_GPIO_PIN_CONFIG GPIO_31_GPIO31
|
||
|
||
|
||
void sfra_init()
|
||
{
|
||
CPUTimer_setPeriod(CPUTIMER0_BASE,
|
||
(DEVICE_SYSCLK_FREQ / CONTROL_ISR_FREQUENCY) - 1);
|
||
CPUTimer_startTimer(CPUTIMER0_BASE);
|
||
|
||
LowPassFilter_Init(&lowPass_test, FS, FC);
|
||
|
||
|
||
#if SFRA_TYPE == TI_SFRA
|
||
|
||
SFRA_F32_reset(&ti_sfra);
|
||
SFRA_F32_config(&ti_sfra,
|
||
SFRA_ISR_FREQ,
|
||
SFRA_AMPLITUDE,
|
||
SFRA_FREQ_LENGTH,
|
||
SFRA_FREQ_START,
|
||
SFRA_FREQ_STEP_MULTIPLY,
|
||
plantMagVect,
|
||
plantPhaseVect,
|
||
olMagVect,
|
||
olPhaseVect,
|
||
clMagVect,
|
||
clPhaseVect,
|
||
freqVect,
|
||
1);
|
||
SFRA_F32_resetFreqRespArray(&ti_sfra);
|
||
SFRA_F32_initFreqArrayWithLogSteps(&ti_sfra,
|
||
SFRA_FREQ_START,
|
||
SFRA_FREQ_STEP_MULTIPLY);
|
||
SFRA_GUI_config(SFRA_GUI_SCI_BASE,
|
||
SFRA_GUI_VBUS_CLK,
|
||
SFRA_GUI_SCI_BAUDRATE,
|
||
SFRA_GUI_SCIRX_GPIO,
|
||
SFRA_GUI_SCIRX_GPIO_PIN_CONFIG,
|
||
SFRA_GUI_SCITX_GPIO,
|
||
SFRA_GUI_SCITX_GPIO_PIN_CONFIG,
|
||
SFRA_GUI_LED_INDICATOR,
|
||
SFRA_GUI_LED_GPIO,
|
||
SFRA_GUI_LED_GPIO_PIN_CONFIG,
|
||
&ti_sfra,
|
||
SFRA_GUI_PLOT_GH_CL);
|
||
|
||
#elif SFRA_TYPE == LIBSFRA
|
||
|
||
// libsfra
|
||
sfra_init_all();
|
||
|
||
sfra_start(&libsfra);
|
||
|
||
#elif SFRA_TYPE == LIBSFRA_HAL_TI
|
||
|
||
// 初始化 HAL 适配层
|
||
libsfra_ti_hal_init(&hal_sfra,
|
||
SFRA_ISR_FREQ,
|
||
SFRA_AMPLITUDE,
|
||
SFRA_FREQ_LENGTH,
|
||
SFRA_FREQ_START,
|
||
SFRA_FREQ_STEP_MULTIPLY,
|
||
plantMagVect,
|
||
plantPhaseVect,
|
||
olMagVect,
|
||
olPhaseVect,
|
||
clMagVect,
|
||
clPhaseVect,
|
||
freqVect);
|
||
SFRA_GUI_config(SFRA_GUI_SCI_BASE,
|
||
SFRA_GUI_VBUS_CLK,
|
||
SFRA_GUI_SCI_BAUDRATE,
|
||
SFRA_GUI_SCIRX_GPIO,
|
||
SFRA_GUI_SCIRX_GPIO_PIN_CONFIG,
|
||
SFRA_GUI_SCITX_GPIO,
|
||
SFRA_GUI_SCITX_GPIO_PIN_CONFIG,
|
||
SFRA_GUI_LED_INDICATOR,
|
||
SFRA_GUI_LED_GPIO,
|
||
SFRA_GUI_LED_GPIO_PIN_CONFIG,
|
||
libsfra_ti_hal_get_adapter(),
|
||
SFRA_GUI_PLOT_GH_H);
|
||
|
||
#endif
|
||
}
|
||
|
||
|
||
// ==================== 打印 LIBSFRA 结果（CSV 格式）====================
|
||
/**
|
||
* 将 SFRA 测量结果通过串口输出为 CSV 格式
|
||
* @param res 指向 sfra_results_t 结构体的指针
|
||
* @param num_points 结果点数（即 SFRA_FREQ_LENGTH）
|
||
*/
|
||
void sfra_print_results_csv(sfra_results_t *res, int num_points)
|
||
{
|
||
char lineBuf[64];
|
||
int i;
|
||
|
||
UARTprintf("Frequency(Hz),Magnitude(dB),Phase(deg)\n");
|
||
|
||
for (i = 0; i < num_points; i++) {
|
||
int pos = 0;
|
||
// 频率
|
||
pos += floatToStr5(res->freq[i], lineBuf + pos);
|
||
lineBuf[pos++] = ',';
|
||
// 增益 (dB)
|
||
pos += floatToStr5(res->mag[i], lineBuf + pos);
|
||
lineBuf[pos++] = ',';
|
||
// 相位 (度)
|
||
pos += floatToStr5(res->phase[i], lineBuf + pos);
|
||
lineBuf[pos++] = '\n';
|
||
lineBuf[pos] = '\0';
|
||
UARTprintf(lineBuf);
|
||
}
|
||
}
|
||
|
||
|
||
void sfra_task_run()
|
||
{
|
||
DEVICE_DELAY_US(1.0f *1000);
|
||
|
||
#if SFRA_TYPE == TI_SFRA
|
||
|
||
SFRA_F32_runBackgroundTask(&ti_sfra);
|
||
SFRA_GUI_runSerialHostComms(&ti_sfra);
|
||
|
||
#elif SFRA_TYPE == LIBSFRA
|
||
|
||
sfra_background_task(&libsfra);
|
||
if(sfra_is_done(&libsfra))
|
||
{
|
||
sfra_print_results_csv(&libsfra_results, SFRA_BUF_SIZE);
|
||
sfra_clear_done(&libsfra);
|
||
}
|
||
|
||
#elif SFRA_TYPE == LIBSFRA_HAL_TI
|
||
|
||
SFRA_F32_runBackgroundTask(libsfra_ti_hal_get_adapter());
|
||
SFRA_GUI_runSerialHostComms(libsfra_ti_hal_get_adapter());
|
||
|
||
#endif
|
||
|
||
}
|
||
|
||
__interrupt void TIMER0_ISR()
|
||
{
|
||
static float32_t input_dc = 0.8f;
|
||
float32_t plant_input;
|
||
float32_t plant_output;
|
||
|
||
#if (SFRA_TYPE == TI_SFRA) || (SFRA_TYPE == LIBSFRA_HAL_TI)
|
||
plant_input = SFRA_F32_inject(input_dc);
|
||
#elif SFRA_TYPE == LIBSFRA
|
||
plant_input = input_dc + 0.1f * sfra_inject(&libsfra);
|
||
#endif
|
||
|
||
// 直通，用于测试SFRA，plant扫描结果应为0°，0db
|
||
// plant_output = plant_input;
|
||
|
||
// 注入扫描lowpass
|
||
plant_output = LowPassFilter_Run(&lowPass_test, plant_input);
|
||
|
||
#if (SFRA_TYPE == TI_SFRA) || (SFRA_TYPE == LIBSFRA_HAL_TI)
|
||
SFRA_F32_collect(&plant_input, &plant_output);
|
||
#elif SFRA_TYPE == LIBSFRA
|
||
sfra_monitor(&libsfra, plant_input, plant_output);
|
||
#endif
|
||
|
||
Interrupt_clearACKGroup(INTERRUPT_ACK_GROUP1);
|
||
}
|