cbb
4c4e47e9e3
添加了完整的Doxygen文档注释到FOC算法模块的所有函数、类和变量，提升了代码可读性和维护性。同时优化了SVPWM算法实现，将atan2f替换为符号法扇区判断，显著提升计算效率（约15周期vs atan2f 300周期），并改进了arm_sin_cos_f32替代sqrtf提高性能。 feat(FOC): 统一电气角度计算避免重复运算 重构代码以统一电气角度计算，在各个控制循环中只计算一次电气角度并传递给需要的函数，避免了多次重复的fmodf运算和角度归一化操作，提升了实时控制性能。 refactor(Filter): 改进α-β观测器角度处理精度 修改角度卡尔曼滤波器在归一化角度空间[0,360)内工作，避免大累积角度的浮点精度损失问题，确保静止状态下不会产生虚假速度，同时保持与下游代码的兼容性。 refactor: 为算法模块添加完整文档注释 为所有算法模块(FOC、Filter、pid、thermal)添加了详细的Doxygen文档注释，包括函数说明、参数描述、返回值说明等，便于代码理解和API文档生成。
310 lines
8.9 KiB
C++
310 lines
8.9 KiB
C++
/**
|
||
* @file device/ang_sensor.cpp
|
||
* @brief 底层设备驱动与外设封装源文件
|
||
* @note 本文件仅补充 Doxygen 文档注释，不改变代码逻辑。
|
||
*/
|
||
|
||
#include "ang_sensor.h"
|
||
#include "Can.h"
|
||
#include "Filter.h"
|
||
#include "config.h"
|
||
#include "flash.h"
|
||
#include "gd32c10x_dma.h"
|
||
#include "gd32c10x_gpio.h"
|
||
#include "gd32c10x_spi.h"
|
||
#include <cstdio>
|
||
#include <cstring>
|
||
#include <stdint.h>
|
||
|
||
// ANG_SENS 调试宏定义
|
||
#if ANG_SENS_DEBUG
|
||
/** @brief ANG_SENS_DEBUG_PRINT 宏定义 */
|
||
#define ANG_SENS_DEBUG_PRINT(fmt, ...) printf(fmt, ##__VA_ARGS__)
|
||
#else
|
||
/** @brief ANG_SENS_DEBUG_PRINT 宏定义 */
|
||
#define ANG_SENS_DEBUG_PRINT(fmt, ...) ((void)0)
|
||
#endif
|
||
|
||
/** @brief ANG_SPI_CS_ENABLE 宏定义 */
|
||
#define ANG_SPI_CS_ENABLE() gpio_bit_reset(GPIOA, GPIO_PIN_15)
|
||
/** @brief ANG_SPI_CS_DISABLE 宏定义 */
|
||
#define ANG_SPI_CS_DISABLE() gpio_bit_set(GPIOA, GPIO_PIN_15)
|
||
/** @brief DRV_SPI_CS_ENABLE 宏定义 */
|
||
#define DRV_SPI_CS_ENABLE() gpio_bit_reset(GPIOB, GPIO_PIN_14)
|
||
/** @brief DRV_SPI_CS_DISABLE 宏定义 */
|
||
#define DRV_SPI_CS_DISABLE() gpio_bit_set(GPIOB, GPIO_PIN_14)
|
||
|
||
// DMA 传输完成标志
|
||
volatile bool spi_dma_tx_complete = false;
|
||
volatile bool spi_dma_rx_complete = false;
|
||
|
||
angle_sensor KTH7823; // 角度传感器初始化
|
||
extern FlashEEPROM flashEEP;
|
||
extern EncoderAngleKalman Angle_kalman;
|
||
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
|
||
*
|
||
* @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
|
||
*/
|
||
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;
|
||
|
||
// 配置发送 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 传输完成
|
||
}
|
||
|
||
// 检查是否超时
|
||
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;
|
||
}
|
||
|
||
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 无
|
||
*/
|
||
void KTH7823_gpio_init(void) {
|
||
rcu_periph_clock_enable(RCU_GPIOA);
|
||
rcu_periph_clock_enable(RCU_GPIOB);
|
||
rcu_periph_clock_enable(RCU_SPI0);
|
||
rcu_periph_clock_enable(RCU_AF);
|
||
/* 关闭 JTAG，仅保留 SWD（释放 PA15、PB3、PB4） */
|
||
gpio_pin_remap_config(GPIO_SWJ_SWDPENABLE_REMAP, ENABLE);
|
||
|
||
/* -------- SPI0 全 remap：PB3/4/5 + PA15 -------- */
|
||
gpio_pin_remap_config(GPIO_SPI0_REMAP, ENABLE);
|
||
/* PA15 → NSS, 推挽输出 */
|
||
gpio_init(GPIOA, GPIO_MODE_OUT_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_15);
|
||
/* PB3 → SCK, PB5 → MOSI, 推挽输出 */
|
||
gpio_init(GPIOB, GPIO_MODE_AF_PP, GPIO_OSPEED_50MHZ,
|
||
GPIO_PIN_3 | GPIO_PIN_5);
|
||
/* PB4 → MISO, 浮空输入 */
|
||
gpio_init(GPIOB, GPIO_MODE_IN_FLOATING, GPIO_OSPEED_50MHZ,
|
||
GPIO_PIN_4 | GPIO_PIN_10 | GPIO_PIN_11);
|
||
}
|
||
|
||
/**
|
||
* @brief SPI 初始化
|
||
*
|
||
* @details Configures SPI hardware and initializes necessary settings
|
||
*/
|
||
void KTH7823_spi_init(void) {
|
||
spi_parameter_struct spi_init_str;
|
||
spi_struct_para_init(&spi_init_str);
|
||
|
||
/* SPI 参数：16bit, 主机，全双工，CPOL=1 CPHA=1, MSB first */
|
||
spi_init_str.device_mode = SPI_MASTER;
|
||
spi_init_str.trans_mode = SPI_TRANSMODE_FULLDUPLEX;
|
||
spi_init_str.frame_size = SPI_FRAMESIZE_16BIT;
|
||
spi_init_str.clock_polarity_phase = SPI_CK_PL_HIGH_PH_2EDGE;
|
||
spi_init_str.nss = SPI_NSS_SOFT;
|
||
spi_init_str.endian = SPI_ENDIAN_MSB;
|
||
|
||
spi_init_str.prescale = 24; /* 5MHz @ 120MHz */
|
||
spi_init(SPI0, &spi_init_str);
|
||
spi_nss_output_enable(SPI0);
|
||
spi_nssp_mode_enable(SPI0);
|
||
spi_enable(SPI0);
|
||
}
|
||
|
||
/**
|
||
* @ 角度传感器类 初始化
|
||
* @return 0:初始化成功 1:初始化失败
|
||
*/
|
||
u8 angle_sensor::init(void) {
|
||
KTH7823_gpio_init(); // GPIO 初始化
|
||
KTH7823_spi_init(); // SPI 初始化
|
||
spi_dma_init(); // DMA 初始化
|
||
return 0;
|
||
}
|
||
|
||
/**
|
||
* @ 角度传感器类 设置当前角度
|
||
* @param angle 输出级角度值
|
||
* @return 0 成功/1 失败
|
||
*/
|
||
u8 angle_sensor::angle_set(float angle) {
|
||
m_cumulativeAngle = angle * REDUCTION_RATIO;
|
||
m_firstUpdate = true;
|
||
return 0;
|
||
}
|
||
|
||
/**
|
||
* @brief 读取传感器数据
|
||
* @return 传感器数据
|
||
*/
|
||
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);
|
||
ANG_SPI_CS_DISABLE();
|
||
|
||
if (result == 0) {
|
||
// 单次浮点乘法优化：只计算一次度数，然后转换为弧度
|
||
float angle_deg = -(float)raw_data * ANGLESENS_FACTOR + ANGLESENS_OFFSET;
|
||
current_angle_deg = angle_deg;
|
||
current_angle_rad = angle_deg * DEG2RAD; // M_PI/180.0f
|
||
return angle_deg;
|
||
}
|
||
|
||
// 仅在调试模式下输出错误，避免发布版本中的 I/O 开销
|
||
if (result != 0) {
|
||
ANG_SENS_DEBUG_PRINT("Error reading angle from sensor.\n");
|
||
}
|
||
|
||
return (float)FAULT_RECEIVE;
|
||
}
|
||
|
||
/**
|
||
* @brief 更新传感器角度（输出级）
|
||
* @return 累积角度值
|
||
*/
|
||
float angle_sensor::update(void) {
|
||
read_sensorAngle();
|
||
|
||
if (m_firstUpdate) {
|
||
m_lastAngle = current_angle_deg;
|
||
m_firstUpdate = false;
|
||
return m_cumulativeAngle;
|
||
}
|
||
|
||
// 计算角度变化
|
||
float delta = current_angle_deg - m_lastAngle;
|
||
|
||
// 处理角度绕回情况（0 度和 360 度之间的跳变）
|
||
// 如果变化超过 300 度，说明发生了绕回
|
||
if (delta > ANGLE_WRAP_THRESHOLD) {
|
||
// 从接近 360 度绕回到接近 0 度
|
||
delta -= ANGLE_WRAP_VALUE;
|
||
} else if (delta < -ANGLE_WRAP_THRESHOLD) {
|
||
// 从接近 0 度绕到接近 360 度
|
||
delta += ANGLE_WRAP_VALUE;
|
||
}
|
||
|
||
m_cumulativeAngle += delta;
|
||
m_lastAngle = current_angle_deg;
|
||
return m_cumulativeAngle;
|
||
}
|
||
|
||
/**
|
||
* @brief 获取累积角度值 (输出级)
|
||
* @return 累积角度值
|
||
*/
|
||
float angle_sensor::getAngle(void) const {
|
||
return m_cumulativeAngle / REDUCTION_RATIO;
|
||
}
|
||
|
||
/**
|
||
* @brief 获取累积角度值 (原始级)
|
||
* @return 累积角度值
|
||
*/
|
||
float angle_sensor::getRawAngle(void) const { return m_cumulativeAngle; }
|
||
|
||
/**
|
||
* @brief 获取角度值 (原始级)
|
||
* @return 角度值 (原始级)
|
||
*/
|
||
float angle_sensor::getcurrent_sensorangle(void) const{
|
||
return current_angle_deg;
|
||
} |