125 lines
3.3 KiB
C
125 lines
3.3 KiB
C
#include "sfra_f32.h"
|
||
#include "sfra_test.h"
|
||
#include "lowpass.h"
|
||
#include "sfra_gui_scicomms_driverlib.h"
|
||
#include <stdio.h>
|
||
|
||
|
||
|
||
// 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];
|
||
|
||
|
||
// 通信串口，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
|
||
|
||
|
||
|
||
// lowpass filter
|
||
LowPassFilter_t lowPass_test;
|
||
#define FS CONTROL_ISR_FREQUENCY
|
||
#define FC 10.0f * 1000
|
||
|
||
|
||
void sfra_init()
|
||
{
|
||
CPUTimer_setPeriod(CPUTIMER0_BASE,
|
||
(DEVICE_SYSCLK_FREQ / CONTROL_ISR_FREQUENCY) - 1);
|
||
CPUTimer_startTimer(CPUTIMER0_BASE);
|
||
|
||
LowPassFilter_Init(&lowPass_test, FS, FC);
|
||
|
||
|
||
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);
|
||
|
||
}
|
||
|
||
|
||
void sfra_task_run()
|
||
{
|
||
DEVICE_DELAY_US(1.0f *1000);
|
||
|
||
SFRA_F32_runBackgroundTask(&ti_sfra);
|
||
SFRA_GUI_runSerialHostComms(&ti_sfra);
|
||
|
||
}
|
||
|
||
__interrupt void TIMER0_ISR()
|
||
{
|
||
static float32_t input_dc = 0.8f;
|
||
float32_t plant_input;
|
||
float32_t plant_output;
|
||
|
||
plant_input = SFRA_F32_inject(input_dc);
|
||
|
||
|
||
// 直通，用于测试SFRA，plant扫描结果应为0°，0db
|
||
// plant_output = plant_input;
|
||
|
||
// 注入扫描lowpass
|
||
plant_output = LowPassFilter_Run(&lowPass_test, plant_input);
|
||
|
||
SFRA_F32_collect(&plant_input, &plant_output);
|
||
|
||
Interrupt_clearACKGroup(INTERRUPT_ACK_GROUP1);
|
||
}
|