feat(FOC): 优化FOC控制算法并修复多个控制环路问题
- 重构cal_Iq_Id函数，移除电角度参数传递，改用预计算的sin/cos值 以避免重复三角函数计算，提升性能约15-300周期 - 修复角度同步Uq前馈学习中的运动方向判断逻辑，解决反转时的 震荡问题和正转正常反转震荡的根本原因 - 实现低速缩放因子函数get_mode3_velocity_scale，改进静摩擦 hunting抑制效果 - 优化电压检测逻辑，将阻塞式ADC读取改为连续模式零等待， 提升实时性能 - 修复初始化期间电压分母为零的问题，添加下限保护防止P增益 无穷大 - 实现PID控制器测量微分功能，消除目标阶跃时的微分冲击 - 改进积分抗饱和策略，添加外部饱和泄放功能，优化到位稳定 性能 perf(Filter): 优化中值滤波算法性能 - 针对ADC_FILTER_SIZE=6的固定场景实现最优排序网络， 用12次比较替代通用std::nth_element算法，显著提升性能 - 保留通用回退路径处理其他尺寸情况
This commit is contained in:
+24
-107
@@ -34,10 +34,6 @@
|
||||
/** @brief DRV_SPI_CS_DISABLE 宏定义 */
|
||||
#define DRV_SPI_CS_DISABLE() gpio_bit_set(GPIOB, GPIO_PIN_14)
|
||||
|
||||
// SPI DMA 完成标志：由 DMA ISR 置位，spi_rw_half_word_dma() 发起传输前清零并等待。
|
||||
volatile bool spi_dma_tx_complete = false;
|
||||
volatile bool spi_dma_rx_complete = false;
|
||||
|
||||
// 全局角度传感器单例：系统只有一个 KTH7823 角度传感器。
|
||||
// 初始化顺序由 main::hardware_init() 调用 KTH7823.init() 保证。
|
||||
angle_sensor KTH7823; // 角度传感器初始化
|
||||
@@ -47,120 +43,42 @@ extern EncoderVelocityKalman Vel_kalman;
|
||||
extern CAN_transmiter CAN0_transmiter; // CAN 初始化
|
||||
|
||||
/**
|
||||
* @brief SPI DMA initialization
|
||||
*/
|
||||
void spi_dma_init(void) {
|
||||
dma_parameter_struct dma_init_struct;
|
||||
|
||||
// 使能 DMA 时钟
|
||||
rcu_periph_clock_enable(RCU_DMA0);
|
||||
|
||||
// 初始化发送 DMA 通道
|
||||
dma_deinit(DMA0, DMA_CH2);
|
||||
dma_struct_para_init(&dma_init_struct);
|
||||
dma_init_struct.direction = DMA_MEMORY_TO_PERIPHERAL;
|
||||
dma_init_struct.memory_addr = (uint32_t)0; // 实际地址将在传输时设置
|
||||
dma_init_struct.memory_inc = DMA_MEMORY_INCREASE_ENABLE;
|
||||
dma_init_struct.memory_width = DMA_MEMORY_WIDTH_16BIT;
|
||||
dma_init_struct.number = 1;
|
||||
dma_init_struct.periph_addr = (uint32_t)(&SPI_DATA(SPI0));
|
||||
dma_init_struct.periph_inc = DMA_PERIPH_INCREASE_DISABLE;
|
||||
dma_init_struct.periph_width = DMA_PERIPHERAL_WIDTH_16BIT;
|
||||
dma_init_struct.priority = DMA_PRIORITY_HIGH;
|
||||
dma_init(DMA0, DMA_CH2, &dma_init_struct);
|
||||
|
||||
// 初始化接收 DMA 通道
|
||||
dma_deinit(DMA0, DMA_CH1);
|
||||
dma_struct_para_init(&dma_init_struct);
|
||||
dma_init_struct.direction = DMA_PERIPHERAL_TO_MEMORY;
|
||||
dma_init_struct.memory_addr = (uint32_t)0; // 实际地址将在传输时设置
|
||||
dma_init_struct.memory_inc = DMA_MEMORY_INCREASE_ENABLE;
|
||||
dma_init_struct.memory_width = DMA_MEMORY_WIDTH_16BIT;
|
||||
dma_init_struct.number = 1;
|
||||
dma_init_struct.periph_addr = (uint32_t)(&SPI_DATA(SPI0));
|
||||
dma_init_struct.periph_inc = DMA_PERIPH_INCREASE_DISABLE;
|
||||
dma_init_struct.periph_width = DMA_PERIPHERAL_WIDTH_16BIT;
|
||||
dma_init_struct.priority = DMA_PRIORITY_HIGH;
|
||||
dma_init(DMA0, DMA_CH1, &dma_init_struct);
|
||||
|
||||
// 使能 SPI 的 DMA 发送和接收功能
|
||||
spi_dma_enable(SPI0, SPI_DMA_TRANSMIT);
|
||||
spi_dma_enable(SPI0, SPI_DMA_RECEIVE);
|
||||
|
||||
// 配置 DMA 中断
|
||||
nvic_irq_enable(DMA0_Channel2_IRQn, 0, 1);
|
||||
nvic_irq_enable(DMA0_Channel1_IRQn, 0, 1);
|
||||
|
||||
// 使能 DMA 传输完成中断
|
||||
dma_interrupt_enable(DMA0, DMA_CH2, DMA_INT_FTF);
|
||||
dma_interrupt_enable(DMA0, DMA_CH1, DMA_INT_FTF);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Send or read half word through SPI using DMA
|
||||
* @brief Send or read half word through SPI by register polling
|
||||
*
|
||||
* @param[in] spi_periph SPI peripheral
|
||||
* @param[in] tx_data Data to transmit
|
||||
* @param[out] rx_data Pointer to store received data
|
||||
* @return 0 if success, -1 if error
|
||||
* @note 单半字轮询收发。DMA 版本（已移除）每次传输需要：重配 2 个 DMA
|
||||
* 通道(约 10 次寄存器写) + 2 次 FTF 中断 + 忙等完成标志，而 16bit
|
||||
* @5MHz 帧本身只有 3.2μs——DMA 的准备与中断开销和传输时间同量级。
|
||||
* 轮询同样忙等帧时间，但砍掉了全部 DMA 开销与 40k 次/秒的中断。
|
||||
*/
|
||||
int spi_rw_half_word_dma(uint32_t spi_periph, uint16_t tx_data,
|
||||
uint16_t *rx_data) {
|
||||
// 重置完成标志
|
||||
spi_dma_tx_complete = false;
|
||||
spi_dma_rx_complete = false;
|
||||
int spi_rw_half_word_polling(uint32_t spi_periph, uint16_t tx_data,
|
||||
uint16_t *rx_data) {
|
||||
u32 timeout = 1000;
|
||||
|
||||
// 配置发送 DMA
|
||||
dma_channel_disable(DMA0, DMA_CH2);
|
||||
dma_memory_address_config(DMA0, DMA_CH2, (uint32_t)&tx_data);
|
||||
dma_transfer_number_config(DMA0, DMA_CH2, 1);
|
||||
dma_channel_enable(DMA0, DMA_CH2);
|
||||
|
||||
// 配置接收 DMA
|
||||
dma_channel_disable(DMA0, DMA_CH1);
|
||||
dma_memory_address_config(DMA0, DMA_CH1, (uint32_t)rx_data);
|
||||
dma_transfer_number_config(DMA0, DMA_CH1, 1);
|
||||
dma_channel_enable(DMA0, DMA_CH1);
|
||||
|
||||
// 等待传输完成，超时时间为 100 个循环
|
||||
uint32_t timeout = 100;
|
||||
while ((!spi_dma_tx_complete || !spi_dma_rx_complete) && --timeout) {
|
||||
// 空循环等待 DMA 传输完成
|
||||
while (spi_i2s_flag_get(spi_periph, SPI_FLAG_TBE) == RESET) {
|
||||
if (--timeout == 0U) {
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
spi_i2s_data_transmit(spi_periph, tx_data);
|
||||
|
||||
// 检查是否超时
|
||||
if (timeout == 0) {
|
||||
// 禁用 DMA 通道
|
||||
dma_channel_disable(DMA0, DMA_CH2);
|
||||
dma_channel_disable(DMA0, DMA_CH1);
|
||||
ANG_SENS_DEBUG_PRINT("SPI DMA transfer timeout\r\n");
|
||||
return -1;
|
||||
timeout = 1000;
|
||||
while (spi_i2s_flag_get(spi_periph, SPI_FLAG_RBNE) == RESET) {
|
||||
if (--timeout == 0U) {
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
*rx_data = spi_i2s_data_receive(spi_periph);
|
||||
|
||||
// 等待移位完成再返回：调用方返回后立即拉高 CS，
|
||||
// 若最后一位仍在移位会截断帧
|
||||
while (spi_i2s_flag_get(spi_periph, SPI_FLAG_TRANS) != RESET) {
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief DMA0_Channel2 中断处理函数
|
||||
* @param 无
|
||||
*/
|
||||
extern "C" void DMA0_Channel2_IRQHandler(void) {
|
||||
if (dma_interrupt_flag_get(DMA0, DMA_CH2, DMA_INT_FLAG_FTF)) {
|
||||
dma_interrupt_flag_clear(DMA0, DMA_CH2, DMA_INT_FLAG_FTF);
|
||||
spi_dma_tx_complete = true;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief DMA0_Channel1 中断处理函数
|
||||
* @param 无
|
||||
*/
|
||||
extern "C" void DMA0_Channel1_IRQHandler(void) {
|
||||
if (dma_interrupt_flag_get(DMA0, DMA_CH1, DMA_INT_FLAG_FTF)) {
|
||||
dma_interrupt_flag_clear(DMA0, DMA_CH1, DMA_INT_FLAG_FTF);
|
||||
spi_dma_rx_complete = true;
|
||||
}
|
||||
}
|
||||
/**
|
||||
* @brief 角度传感器初始化
|
||||
* @param 无
|
||||
@@ -216,7 +134,6 @@ void KTH7823_spi_init(void) {
|
||||
u8 angle_sensor::init(void) {
|
||||
KTH7823_gpio_init(); // GPIO 初始化
|
||||
KTH7823_spi_init(); // SPI 初始化
|
||||
spi_dma_init(); // DMA 初始化
|
||||
return 0;
|
||||
}
|
||||
|
||||
@@ -239,7 +156,7 @@ inline float angle_sensor::read_sensorAngle(void) {
|
||||
uint16_t raw_data;
|
||||
|
||||
ANG_SPI_CS_ENABLE();
|
||||
u8 result = spi_rw_half_word_dma(SPI0, DEFAULT_COMMAND, &raw_data);
|
||||
u8 result = spi_rw_half_word_polling(SPI0, DEFAULT_COMMAND, &raw_data);
|
||||
ANG_SPI_CS_DISABLE();
|
||||
|
||||
if (result == 0) {
|
||||
|
||||
Reference in New Issue
Block a user