cbb/Motor_v2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat(CAN): 实现多节点CAN总线通信与节点ID管理功能

Browse Source 
- 添加CAN调试宏定义，支持条件编译调试信息输出
- 实现节点ID相关功能，包括构建CAN ID、消息过滤、获取/设置节点ID
- 支持Flash存储节点ID，提供加载和保存功能
- 实现节点ID设置和查询命令处理
- 更新CANFD发送函数使用节点ID构建发送ID
- 添加消息过滤逻辑，只处理目标节点的消息
- 重构DMA中断服务程序，优化数据传输处理
- 替换printf为调试宏，便于生产环境控制调试输出
This commit is contained in:
cbb
2026-03-15 02:38:50 +08:00
parent 2b93a4537c
commit 9f56ab394e
10 changed files with 751 additions and 461 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Robot_V2/device/Can.cpp
+348 -124
View File
@@ -18,6 +18,13 @@
#include <cstring>
#include <random>
// CAN 调试宏定义
#if CAN_DEBUG
#define CAN_DEBUG_PRINT(fmt, ...) printf(fmt, ##__VA_ARGS__)
#else
#define CAN_DEBUG_PRINT(fmt, ...) ((void)0)
#endif
constexpr u16 CANX = CAN0;
extern FOC_Controller FOC;
extern sampler sampler;
@@ -27,33 +34,151 @@ extern EncoderVelocityKalman Vel_kalman; // 速度滤波
extern FlashEEPROM flashEEP;
extern VersionManager Version; // 声明外部版本管理器实例
// 添加DMA相关变量
// 添加 DMA 相关变量
dma_parameter_struct dma_rx_parameter;
dma_parameter_struct dma_tx_parameter;
uint8_t can_rx_buffer[8]; // CAN接收缓冲区
uint8_t can_tx_buffer[8]; // CAN发送缓冲区
uint8_t can_rx_buffer[8]; // CAN 接收缓冲区
uint8_t can_tx_buffer[8]; // CAN 发送缓冲区
CAN_transmiter CAN0_transmiter; // CAN初始化
CAN_transmiter CAN0_transmiter; // CAN 初始化
// ============================================================================
// 节点 ID 相关函数实现 (多电机 CAN 总线隔离)
// 使用高 16 位 (bits 31-16) 存储节点 ID，低 16 位 (bits 15-0) 存储基础 ID
// 格式：[节点 ID(16 位)][基础 ID(16 位)]
// 例如：node_id=1, base_id=0x0200 → 0x00010200
// ============================================================================
// CAN接收缓冲区相关函数实现
/*!
\brief 检查CAN接收缓冲区是否已满
\brief 根据节点 ID 构建 CAN ID
\param[in] base_id 基础 CAN ID(低 16 位)
\param[out] none
\retval 构建后的完整 CAN ID
\note 将节点 ID 放在高 16 位，基础 ID 放在低 16 位
格式：[节点 ID(16 位)][基础 ID(16 位)]
例如：node_id=1, base_id=0x0200 → 0x00010200
*/
u32 CAN_transmiter::build_can_id(u32 base_id) {
return ((u32)node_id << 16) | (base_id & 0xFFFF);
}
/*!
\brief 判断接收到的消息是否是发给本节点的
\param[in] received_id 接收到的 CAN ID
\param[out] none
\retval true: 消息是发给本节点的 (或广播消息)
false: 消息不是发给本节点的
\note 支持广播地址 (0xFFFF)，广播消息所有节点都会响应
提取高 16 位进行节点 ID 匹配
*/
bool CAN_transmiter::is_message_for_me(u32 received_id) {
u16 rx_node_id = (u16)((received_id >> 16) & 0xFFFF);
return (rx_node_id == BROADCAST_NODE_ID) || (rx_node_id == node_id);
}
/*!
\brief 获取当前节点 ID
\param[in] none
\param[out] none
\retval bool 返回true表示缓冲区已满，false表示未满
\retval 当前节点 ID 值 (0-65534)
*/
u16 CAN_transmiter::get_node_id() { return node_id; }
/*!
\brief 设置节点 ID
\param[in] new_id 新的节点 ID 值
\param[out] none
\retval none
\note 节点 ID 范围：0-65534，0xFFFF 保留为广播地址
*/
void CAN_transmiter::set_node_id(u16 new_id) {
if (new_id <= MAX_NODE_ID) {
node_id = new_id;
CAN_DEBUG_PRINT("Node ID set to: %d\r\n", node_id);
}
}
/*!
\brief 从 Flash 加载节点 ID
\param[in] none
\param[out] none
\retval FLASH_EEPROM_OK: 加载成功
其他：加载失败，使用默认值
\note Flash 擦除状态为 0xFFFF，需要特殊处理避免误判
*/
u8 CAN_transmiter::load_node_id_from_flash() {
u16 node_id_temp = 0xFFFF; // 初始化为 Flash 擦除值
FlashEepromStatus status =
flashEEP.read(FlashEEPROM::ADDR_NODE_ID, &node_id_temp);
// 检查 Flash 是否为擦除状态 (0xFFFF 或 0) 或读取失败
// 注意：Flash 未写入时读到的值是 0xFFFF，但有些情况可能读到 0
if (status != FLASH_EEPROM_OK || node_id_temp == 0xFFFF ||
node_id_temp == 0) {
node_id = DEFAULT_NODE_ID;
CAN_DEBUG_PRINT(
"Node ID not found in Flash (status=%d, val=0x%04X), using "
"default: %d\r\n",
status, node_id_temp, node_id);
return FLASH_EEPROM_DATA_NOT_FOUND;
}
// 检查节点 ID 是否有效
if (node_id_temp <= MAX_NODE_ID) {
node_id = node_id_temp;
CAN_DEBUG_PRINT("Node ID loaded from Flash: %d\r\n", node_id);
return FLASH_EEPROM_OK;
} else {
node_id = DEFAULT_NODE_ID;
CAN_DEBUG_PRINT("Invalid Node ID in Flash (%d), using default: %d\r\n",
node_id_temp, node_id);
return FLASH_EEPROM_DATA_NOT_FOUND;
}
}
/*!
\brief 保存节点 ID 到 Flash
\param[in] new_id 要保存的节点 ID
\param[out] none
\retval FLASH_EEPROM_OK: 保存成功
其他：保存失败
*/
u8 CAN_transmiter::save_node_id_to_flash(u16 new_id) {
if (new_id > MAX_NODE_ID) {
CAN_DEBUG_PRINT("Invalid Node ID: %d\r\n", new_id);
return FLASH_EEPROM_INVALID_ADDR;
}
FlashEepromStatus status =
flashEEP.write(FlashEEPROM::ADDR_NODE_ID, new_id);
if (status == FLASH_EEPROM_OK) {
node_id = new_id;
CAN_DEBUG_PRINT("Node ID saved to Flash: %d\r\n", node_id);
return FLASH_EEPROM_OK;
} else {
CAN_DEBUG_PRINT("Failed to save Node ID to Flash\r\n");
return (u8)status;
}
}
// CAN 接收缓冲区相关函数实现
/*!
\brief 检查 CAN 接收缓冲区是否已满
\param[in] none
\param[out] none
\retval bool 返回 true 表示缓冲区已满，false 表示未满
\note 通过比较头指针的下一个位置和尾指针来判断缓冲区是否已满
使用循环缓冲区策略，当(rx_buffer_head + 1) %
CAN_RX_BUFFER_SIZE等于rx_buffer_tail时， 表示缓冲区已达到最大容量
使用循环缓冲区策略，当(rx_buffer_head + 1) % CAN_RX_BUFFER_SIZE
等于 rx_buffer_tail 时， 表示缓冲区已达到最大容量
*/
bool CAN_transmiter::is_rx_buffer_full() {
return ((rx_buffer_head + 1) % CAN_RX_BUFFER_SIZE) == rx_buffer_tail;
}
/*!
\brief 检查CAN接收缓冲区是否为空
\brief 检查 CAN 接收缓冲区是否为空
\param[in] none
\param[out] none
\retval bool 返回true表示缓冲区为空，false表示缓冲区不为空
\retval bool 返回 true 表示缓冲区为空，false 表示缓冲区不为空
\note 通过比较头指针和尾指针是否相等来判断缓冲区是否为空
*/
bool CAN_transmiter::is_rx_buffer_empty() {
@@ -61,13 +186,13 @@ bool CAN_transmiter::is_rx_buffer_empty() {
}
/**
* @brief 从接收缓冲区中取出一条CAN消息
* @brief 从接收缓冲区中取出一条 CAN 消息
*
* 该函数检查接收缓冲区是否为空，如果不为空则取出缓冲区队列头部的消息，
* 并更新队列尾部索引。如果缓冲区为空，则返回false。
* 并更新队列尾部索引。如果缓冲区为空，则返回 false。
*
* @param message 引用类型的输出参数，用于存储取出的CAN接收消息结构体
* @return bool 返回操作是否成功，true表示成功取出消息，false表示缓冲区为空
* @param message 引用类型的输出参数，用于存储取出的 CAN 接收消息结构体
* @return bool 返回操作是否成功，true 表示成功取出消息，false 表示缓冲区为空
*/
bool CAN_transmiter::dequeue_rx_message(can_receive_message_struct &message) {
if (!is_rx_buffer_empty()) {
@@ -103,60 +228,60 @@ u8 can_gpio_init() {
}
/*!
\brief 初始化CAN DMA
\brief 初始化 CAN DMA
\param[in] none
\param[out] none
\retval none
*/
void can_dma_init(void) {
// 使能DMA时钟
// 使能 DMA 时钟
rcu_periph_clock_enable(RCU_DMA0);
// 配置接收DMA
// 配置接收 DMA
dma_struct_para_init(&dma_rx_parameter);
dma_rx_parameter.periph_addr =
(uint32_t)(&CAN_RFIFOMDATA0(CAN0, CAN_FIFO0)); // CAN接收数据寄存器地址
(uint32_t)(&CAN_RFIFOMDATA0(CAN0, CAN_FIFO0)); // CAN 接收数据寄存器地址
dma_rx_parameter.memory_addr = (uint32_t)can_rx_buffer; // 接收缓冲区地址
dma_rx_parameter.direction = DMA_PERIPHERAL_TO_MEMORY; // 外设到内存
dma_rx_parameter.memory_width = DMA_MEMORY_WIDTH_8BIT; // 内存宽度8位
dma_rx_parameter.periph_width = DMA_PERIPHERAL_WIDTH_8BIT; // 外设宽度8位
dma_rx_parameter.memory_width = DMA_MEMORY_WIDTH_8BIT; // 内存宽度 8 位
dma_rx_parameter.periph_width = DMA_PERIPHERAL_WIDTH_8BIT; // 外设宽度 8 位
dma_rx_parameter.priority = DMA_PRIORITY_HIGH; // 高优先级
dma_rx_parameter.number = 8; // 传输字节数
dma_rx_parameter.periph_inc = DMA_PERIPH_INCREASE_DISABLE; // 外设地址不递增
dma_rx_parameter.memory_inc = DMA_MEMORY_INCREASE_ENABLE; // 内存地址递增
dma_init(DMA0, DMA_CH4, &dma_rx_parameter); // 使用DMA通道4
dma_init(DMA0, DMA_CH4, &dma_rx_parameter); // 使用 DMA 通道 4
// 配置发送DMA
// 配置发送 DMA
dma_struct_para_init(&dma_tx_parameter);
dma_tx_parameter.periph_addr =
(uint32_t)(&CAN_TMI0(CAN0)); // CAN发送数据寄存器地址
(uint32_t)(&CAN_TMI0(CAN0)); // CAN 发送数据寄存器地址
dma_tx_parameter.memory_addr = (uint32_t)can_tx_buffer; // 发送缓冲区地址
dma_tx_parameter.direction = DMA_MEMORY_TO_PERIPHERAL; // 内存到外设
dma_tx_parameter.memory_width = DMA_MEMORY_WIDTH_8BIT; // 内存宽度8位
dma_tx_parameter.periph_width = DMA_PERIPHERAL_WIDTH_8BIT; // 外设宽度8位
dma_tx_parameter.memory_width = DMA_MEMORY_WIDTH_8BIT; // 内存宽度 8 位
dma_tx_parameter.periph_width = DMA_PERIPHERAL_WIDTH_8BIT; // 外设宽度 8 位
dma_tx_parameter.priority = DMA_PRIORITY_HIGH; // 高优先级
dma_tx_parameter.number = 8; // 传输字节数
dma_tx_parameter.periph_inc = DMA_PERIPH_INCREASE_DISABLE; // 外设地址不递增
dma_tx_parameter.memory_inc = DMA_MEMORY_INCREASE_ENABLE; // 内存地址递增
dma_init(DMA0, DMA_CH5, &dma_tx_parameter); // 使用DMA通道5
dma_init(DMA0, DMA_CH5, &dma_tx_parameter); // 使用 DMA 通道 5
// 使能DMA通道
// 使能 DMA 通道
dma_channel_enable(DMA0, DMA_CH4);
dma_channel_enable(DMA0, DMA_CH5);
// 使能CAN接收DMA
// 使能 CAN 接收 DMA
// can_dma_enable(CAN0, CAN_DMA_RECEIVE);
}
/*!
\brief 初始化CAN0 2.0B 模式
\brief 初始化 CAN0 2.0B 模式
\param[in] none
\param[out] none
\retval none
*/
void can_config(void) {
can_gpio_init();
can_dma_init(); // 初始化DMA
can_dma_init(); // 初始化 DMA
can_parameter_struct can_parameter;
can_filter_parameter_struct can_filter;
@@ -180,7 +305,7 @@ void can_config(void) {
/* initialize CAN */
if (can_init(CAN0, &can_parameter) != SUCCESS) {
while (1)
printf("can init error \r\n");
CAN_DEBUG_PRINT("can init error \r\n");
}
/* initialize filter */
@@ -205,20 +330,20 @@ void can_config(void) {
/* enable can transmit mailbox empty interrupt */
can_interrupt_enable(CAN0, CAN_INTEN_TMEIE);
// 使能DMA中断
// 使能 DMA 中断
nvic_irq_enable(DMA0_Channel4_IRQn, 1, 1);
dma_interrupt_enable(DMA0, DMA_CH4, DMA_INT_FTF);
}
/*!
\brief 初始化CAN0 CANFD模式
\brief 初始化 CAN0 CANFD 模式
\param[in] none
\param[out] none
\retval none
*/
void canfd_config(void) {
can_gpio_init();
can_dma_init(); // 初始化DMA
can_dma_init(); // 初始化 DMA
can_parameter_struct can_parameter;
can_fdframe_struct can_fd_parameter;
can_fd_tdc_struct can_fd_tdc_parameter;
@@ -281,13 +406,13 @@ void canfd_config(void) {
nvic_irq_enable(CAN0_RX0_IRQn, 1, 0);
/* enable can receive FIFO0 not empty interrupt */
can_interrupt_enable(CAN0, CAN_INTEN_RFNEIE0);
// 使能DMA中断
// 使能 DMA 中断
nvic_irq_enable(DMA0_Channel4_IRQn, 1, 1);
dma_interrupt_enable(DMA0, DMA_CH4, DMA_INT_FTF);
}
/*!
\brief 初始化CAN0回环测试
\brief 初始化 CAN0 回环测试
\param[in] none
\param[out] none
\retval none
@@ -316,7 +441,7 @@ void can_loopback_init(void) {
u16 i = 200;
while (--i > 0) {
delay_1ms(10);
printf("can init error \r\n");
CAN_DEBUG_PRINT("can init error \r\n");
}
system_soft_reset();
}
@@ -341,7 +466,7 @@ void can_loopback_init(void) {
}
/*!
\brief CAN0 FD回环测试
\brief CAN0 FD 回环测试
\param[in] none
\param[out] none
\retval ErrStatus
@@ -368,7 +493,7 @@ void canfd_loopback_test(void) {
int i = 0;
while (i++ < transmit_message.tx_dlen) {
transmit_message.tx_data[i] = dis(gen);
std::printf("%x\n", transmit_message.tx_data[i]);
CAN_DEBUG_PRINT("%x\n", transmit_message.tx_data[i]);
}
/* transmit message */
@@ -392,10 +517,10 @@ void canfd_loopback_test(void) {
(transmit_message.tx_ff == receive_message.rx_ff) &&
(transmit_message.tx_dlen == receive_message.rx_dlen)) {
/* loopback test is success */
std::printf("CAN LO0PBACK SUCCESS\r\n");
CAN_DEBUG_PRINT("CAN LO0PBACK SUCCESS\r\n");
} else {
/* loopback test is failed */
std::printf("CAN LO0PBACK FAIL\r\n");
CAN_DEBUG_PRINT("CAN LO0PBACK FAIL\r\n");
}
}
@@ -430,7 +555,8 @@ void can_loopback_test(void) {
/* waiting for transmit completed */
while ((CAN_TRANSMIT_OK != can_transmit_states(CANX, transmit_mailbox)) &&
(0 != timeout)) {
printf("sending:%x...\n", can_transmit_states(CANX, transmit_mailbox));
CAN_DEBUG_PRINT("sending:%x...\n",
can_transmit_states(CANX, transmit_mailbox));
timeout--;
}
timeout = 0xFF;
@@ -447,28 +573,29 @@ void can_loopback_test(void) {
(transmit_message.tx_dlen == receive_message.rx_dlen)) {
i = transmit_message.tx_dlen;
while (i-- > 0) {
std::printf("rx %d:%x:%x\n", i, transmit_message.tx_data[i],
receive_message.rx_data[i]);
CAN_DEBUG_PRINT("rx %d:%x:%x\n", i, transmit_message.tx_data[i],
receive_message.rx_data[i]);
}
if (std::memcmp(receive_message.rx_data, transmit_message.tx_data,
transmit_message.tx_dlen) == 0) {
/* loopback test is success */
std::printf("loopback success\n");
CAN_DEBUG_PRINT("loopback success\n");
} else {
std::printf("loopback failed\n");
/* loopback test is failed */
CAN_DEBUG_PRINT("loopback failed\n");
}
} else {
/* loopback test is failed */
std::printf("loopback failed\n");
CAN_DEBUG_PRINT("loopback failed\n");
}
}
/*!
\brief CAN_transmiter构造函数
\note 初始化CAN通信相关参数和硬件配置
\brief CAN_transmiter 构造函数
\note 初始化 CAN 通信相关参数和硬件配置
*/
u8 CAN_transmiter::init() {
// 初始化CAN硬件
// 初始化 CAN 硬件
// can_config();
canfd_config();
@@ -477,9 +604,15 @@ u8 CAN_transmiter::init() {
/* initialize receive message */
can_struct_para_init(CAN_RX_MESSAGE_STRUCT, &receive_message);
// 初始化DMA缓冲区
// 初始化 DMA 缓冲区
memset(can_rx_buffer, 0, sizeof(can_rx_buffer));
memset(can_tx_buffer, 0, sizeof(can_tx_buffer));
// 初始化节点 ID 为默认值
node_id = DEFAULT_NODE_ID;
// 从 Flash 加载节点 ID (如果 Flash 中有有效值则覆盖默认值)
load_node_id_from_flash();
return 0;
}
@@ -511,14 +644,14 @@ void CAN_transmiter::update(void) {
}
/**
* @brief CANFD发送函数
* @brief CANFD 发送函数
* @retval 0: 发送失败
* 1: 发送成功
*/
u8 CAN_transmiter::CANFD_all_Send() {
u32 success_flag = 0;
// 打包status数据 - 只清零实际使用的32字节
// 打包 status 数据 - 只清零实际使用的 32 字节
memset(transmit_data, 0, 32);
pack_u16_to_bytes(transmit_data, 0, mcu_temp);
pack_u16_to_bytes(transmit_data, 2, motor_temp);
@@ -535,43 +668,56 @@ u8 CAN_transmiter::CANFD_all_Send() {
pack_float_to_bytes(transmit_data, 21, rt_current_union);
pack_float_to_bytes(transmit_data, 25, testfloat_union);
pack_u16_to_bytes(transmit_data, 29, test_u16);
success_flag += CANFD_AsyncSend(CANFD_STATUS_ID, 32, transmit_data);
// 使用节点 ID 构建发送 ID (高 16 位为节点 ID)
success_flag +=
CANFD_AsyncSend(build_can_id(CANFD_STATUS_ID), 32, transmit_data);
// 打包config数据 - 只清零实际使用的24字节
// 打包 config 数据 - 只清零实际使用的 24 字节
memset(transmit_data, 0, 24);
transmit_data[0] = controller_Mode;
pack_u16_to_bytes(transmit_data, 1, limit_voltage);
pack_float_to_bytes(transmit_data, 3, target_current_union);
pack_float_to_bytes(transmit_data, 7, target_velocity_union);
pack_float_to_bytes(transmit_data, 11, target_angle_union);
success_flag += CANFD_AsyncSend(CANFD_CONFIG_ID, 24, transmit_data);
// 使用节点 ID 构建发送 ID (高 16 位为节点 ID)
success_flag +=
CANFD_AsyncSend(build_can_id(CANFD_CONFIG_ID), 24, transmit_data);
return success_flag;
}
/**
* @brief CAN通信检查函数
* @brief CAN 通信检查函数
* @retval 0: 无新消息
* 1: 有新消息
*/
void CAN_transmiter::communication_check() {
// CAN0 发送数据标志
if (SET == can0_send_flag) {
// printf("\r\n can0 %X receive data:", receive_message.rx_efid);
// CAN_DEBUG_PRINT("\r\n can0 %X receive data:",
// receive_message.rx_efid);
update();
// CAN0_transmiter.CAN_all_Send();
CANFD_all_Send();
can0_send_flag = RESET;
}
// 处理所有接收到的CAN消息（从环形缓冲区中）
// 处理所有接收到的 CAN 消息（从环形缓冲区中）
can_receive_message_struct current_message;
while (dequeue_rx_message(current_message)) {
// printf("\r\n can0 %X receive data:", current_message.rx_efid);
// for (i = 0; i < current_message.rx_dlen; i++) {
// printf(" %02x", current_message.rx_data[i]);
// CAN_DEBUG_PRINT("\r\n can0 %X receive data:",
// current_message.rx_efid); for (i = 0; i < current_message.rx_dlen;
// i++) { CAN_DEBUG_PRINT(" %02x", current_message.rx_data[i]);
// }
if (current_message.rx_efid == CANFD_CMD_ID) {
// 检查消息是否是发给本节点的 (检查高 16 位)
if (!is_message_for_me(current_message.rx_efid)) {
continue; // 不是发给本节点的消息，跳过
}
// 提取基础 ID (低 16 位，去除高 16 位节点 ID 部分)
u32 base_id = current_message.rx_efid & 0xFFFF;
if (base_id == CANFD_CMD_ID) {
// 控制模式
FOC.control_mode = current_message.rx_data[0];
// 目标电压限制
@@ -595,9 +741,9 @@ void CAN_transmiter::communication_check() {
FOC.target_velocity = target_velocity_union.f;
FOC.target_current = target_current_union.f;
// 解析PID参数
// 解析 PID 参数
float voltage = FOC.limit_voltage;
// PID参数
// PID 参数
u16 P_config = unpack_bytes_to_u16(current_message.rx_data, 15);
FOC.cur_loop_p = DEFAULT_current_loop_p * P_config / voltage;
FOC.cur_MIT_p = DEFAULT_current_MIT_p * P_config / voltage;
@@ -611,7 +757,7 @@ void CAN_transmiter::communication_check() {
FOC.ang_loop_p = DEFAULT_angle_loop_p * P_config / voltage;
FOC.ang_angloop_p = DEFAULT_angle_angloop_p * P_config / voltage;
FOC.ang_MIT_p = DEFAULT_angle_MIT_p * P_config / voltage;
} else if (current_message.rx_efid == CANFD_RESET_ID) {
} else if (base_id == CANFD_RESET_ID) {
u8 first_byte = current_message.rx_data[0];
u8 eigth_byte = current_message.rx_data[7];
// 电机重置
@@ -619,7 +765,7 @@ void CAN_transmiter::communication_check() {
motor_reset();
testfloat_union.f = KTH7823.getcurrent_sensorangle();
}
// EEPROM重置
// EEPROM 重置
if (first_byte >> 1 & 0x01) {
// flashEEP.format();
}
@@ -638,17 +784,17 @@ void CAN_transmiter::communication_check() {
app_prepare_deep_sleep(true);
// CAN_Sleep();
}
} else if (current_message.rx_efid == CANFD_VERSION_ID) {
} else if (base_id == CANFD_VERSION_ID) {
// 处理版本号请求，发送当前系统版本信息
Version.sendVersionInfoViaCan();
} else if (current_message.rx_efid == CANFD_BOOT_ID) {
} else if (base_id == CANFD_BOOT_ID) {
motor_sleep();
flashEEP.save_system_config(false);
fmc_unlock();
OTA_SET(n_APP_LATEST_EN_ADDR, OTA_NOT_AVAL); // 设置APP为不激活
OTA_SET(n_APP_LATEST_EN_ADDR, OTA_NOT_AVAL); // 设置 APP 为不激活
fmc_lock();
jumpToBOOT();
} else if (current_message.rx_efid == 0x00000102) {
} else if (base_id == CANFD_OTA_ACK_ID) {
volatile u32 *flash_ptr = (volatile u32 *)n_APP_LATEST_EN_ADDR;
u32 app_enable = *flash_ptr;
flash_ptr = (volatile u32 *)n_APP_LATEST_CHECK_ADDR;
@@ -657,16 +803,92 @@ void CAN_transmiter::communication_check() {
transmit_data[1] = 0xAC;
delay_1ms(1);
if (app_enable == OTA_AVAL && app_check == OTA_AVAL) {
CANFD_AsyncSend(CANFD_OTA_ACK_ID, 2, transmit_data);
CANFD_AsyncSend(build_can_id(CANFD_OTA_ACK_ID), 2,
transmit_data);
}
}
// 设置节点 ID 命令 (基础 ID: 0x0400)
else if (base_id == CANFD_SET_NODE_ID) {
// 打印接收到的原始数据
CAN_DEBUG_PRINT(
"SET_NODE_ID: rx_dlen=%d, data[0]=%02X data[1]=%02X\r\n",
current_message.rx_dlen, current_message.rx_data[0],
current_message.rx_data[1]);
// 解析新节点 ID - 支持 1 字节或 2 字节格式
u16 new_node_id;
if (current_message.rx_dlen >= 2) {
new_node_id = unpack_bytes_to_u16(current_message.rx_data, 0);
} else {
new_node_id =
current_message.rx_data[0]; // 只发送 1 字节时直接使用
}
u16 old_node_id = node_id; // 保存旧节点 ID 用于构建响应 ID
CAN_DEBUG_PRINT(
"SET_NODE_ID: recv new_id=%d (0x%04X), old_id=%d\r\n",
new_node_id, new_node_id, old_node_id);
if (new_node_id <= MAX_NODE_ID) {
u8 save_status = save_node_id_to_flash(new_node_id);
CAN_DEBUG_PRINT(
"SET_NODE_ID: save_status=%d, current node_id=%d\r\n",
save_status, node_id);
if (save_status == FLASH_EEPROM_OK) {
// 发送确认响应 - 使用旧节点 ID 构建响应 ID
// 响应数据格式：[status:1byte][new_node_id:2byte] = 3 字节
transmit_data[0] = 0x01; // 成功
transmit_data[1] = (u8)(new_node_id & 0xFF); // 新 ID 低字节
transmit_data[2] =
(u8)((new_node_id >> 8) & 0xFF); // 新 ID 高字节
CAN_DEBUG_PRINT("SET_NODE_ID: SUCCESS, send response with "
"old_id=%d, new_id_bytes=%02X %02X\r\n",
old_node_id, transmit_data[1],
transmit_data[2]);
// 注意：这里不修改 node_id，因为 save_node_id_to_flash
// 已经更新了 但构建响应时需要用旧 ID，所以临时修改
node_id = old_node_id;
CANFD_AsyncSend(build_can_id(CANFD_SET_NODE_ID), 3,
transmit_data); // 发送 3 字节
node_id = new_node_id; // 恢复新 ID
CAN_DEBUG_PRINT("SET_NODE_ID: node_id updated to %d\r\n",
node_id);
} else {
// Flash 写入失败，发送错误响应
CAN_DEBUG_PRINT(
"SET_NODE_ID: Flash write FAILED, status=%d\r\n",
save_status);
transmit_data[0] = 0x00; // 失败
transmit_data[1] = (u8)(new_node_id & 0xFF);
transmit_data[2] = (u8)((new_node_id >> 8) & 0xFF);
node_id = old_node_id;
CANFD_AsyncSend(build_can_id(CANFD_SET_NODE_ID), 3,
transmit_data);
}
} else {
// 发送错误响应 - 使用旧节点 ID 构建响应 ID
CAN_DEBUG_PRINT("SET_NODE_ID: invalid new_id=%d (max=%d)\r\n",
new_node_id, MAX_NODE_ID);
transmit_data[0] = 0x00; // 失败
transmit_data[1] = (u8)(new_node_id & 0xFF);
transmit_data[2] = (u8)((new_node_id >> 8) & 0xFF);
node_id = old_node_id; // 临时恢复旧 ID 用于构建响应
CANFD_AsyncSend(build_can_id(CANFD_SET_NODE_ID), 3,
transmit_data);
// 保持原 ID 不变
}
}
// 查询节点 ID 命令 (基础 ID: 0x0402)
else if (base_id == CANFD_GET_NODE_ID) {
pack_u16_to_bytes(transmit_data, 0, get_node_id());
CANFD_AsyncSend(build_can_id(CANFD_GET_NODE_ID), 2, transmit_data);
} else {
printf("Unknown CAN ID: %X\n", current_message.rx_efid);
CAN_DEBUG_PRINT("Unknown CAN ID: %X (node_id=%d)\n",
current_message.rx_efid, node_id);
}
}
}
/**
* @brief CAN睡眠
* @brief CAN 睡眠
* @param None
* @retval None
*/
@@ -677,12 +899,12 @@ u8 CAN_transmiter::CAN_Sleep() {
}
/**
* @brief CAN唤醒
* @brief CAN 唤醒
* @param None
* @retval None
*/
u8 CAN_transmiter::CAN_Wakeup() {
printf("can wake\r\n");
CAN_DEBUG_PRINT("can wake\r\n");
is_sleep = false;
canfd_config();
@@ -690,16 +912,16 @@ u8 CAN_transmiter::CAN_Wakeup() {
}
/**
* @brief CAN0中断服务程序
* @brief CAN0 中断服务程序
* @retval 无
*/
void CAN0_RX0_IRQHandler(void) {
// 检查是否是FIFO0消息挂起中断
// 检查是否是 FIFO0 消息挂起中断
if (can_interrupt_flag_get(CAN0, CAN_INT_FLAG_RFL0) != RESET) {
// 清除中断标志
can_interrupt_flag_clear(CAN0, CAN_INT_FLAG_RFL0);
// 使用DMA读取接收到的数据
// 使用 DMA 读取接收到的数据
dma_channel_disable(DMA0, DMA_CH4);
dma_rx_parameter.number =
can_receive_message_length_get(CAN0, CAN_FIFO0);
@@ -716,27 +938,27 @@ void CAN0_RX0_IRQHandler(void) {
// 检查其他可能的中断源
if (can_interrupt_flag_get(CAN0, CAN_INT_FLAG_RFF0) != RESET) {
// FIFO0满中断
// FIFO0 满中断
can_interrupt_flag_clear(CAN0, CAN_INT_FLAG_RFF0);
printf("CAN FIFO0 full\n");
CAN_DEBUG_PRINT("CAN FIFO0 full\n");
}
if (can_interrupt_flag_get(CAN0, CAN_INT_FLAG_RFO0) != RESET) {
// FIFO0溢出中断
// FIFO0 溢出中断
can_interrupt_flag_clear(CAN0, CAN_INT_FLAG_RFO0);
printf("CAN FIFO0 overflow\n");
CAN_DEBUG_PRINT("CAN FIFO0 overflow\n");
}
}
/**
* @brief DMA通道4中断服务程序(CAN接收DMA)
* @brief DMA 通道 4 中断服务程序 (CAN 接收 DMA)
* @retval 无
*/
void DMA0_Channel4_IRQHandler(void) {
if (dma_interrupt_flag_get(DMA0, DMA_CH4, DMA_INT_FLAG_FTF)) {
// 清除DMA传输完成中断标志
// 清除 DMA 传输完成中断标志
dma_interrupt_flag_clear(DMA0, DMA_CH4, DMA_INT_FLAG_G);
// 将DMA缓冲区的数据复制到接收消息结构体
// 将 DMA 缓冲区的数据复制到接收消息结构体
for (int i = 0; i < CAN0_transmiter.receive_message.rx_dlen && i < 8;
i++) {
CAN0_transmiter.receive_message.rx_data[i] = can_rx_buffer[i];
@@ -745,41 +967,41 @@ void DMA0_Channel4_IRQHandler(void) {
}
/**
* @brief DMA通道5中断服务程序(CAN/CANFD发送DMA)
* @brief DMA 通道 5 中断服务程序 (CAN/CANFD 发送 DMA)
* @retval 无
*/
void DMA0_Channel5_IRQHandler(void) {
if (dma_interrupt_flag_get(DMA0, DMA_CH5, DMA_INT_FLAG_FTF)) {
// DMA传输完成，可以在此处执行后续处理
// 清除DMA传输完成中断标志
// DMA 传输完成，可以在此处执行后续处理
// 清除 DMA 传输完成中断标志
dma_interrupt_flag_clear(DMA0, DMA_CH5, DMA_INT_FLAG_FTF);
// 可以在此添加后续处理逻辑，如发送下一个数据包等
}
}
/**
* @brief 异步CAN数据发送(使用DMA和中断机制)
* @param id CAN消息ID
* @param length CAN消息长度
* @param data CAN消息数据
* @brief 异步 CAN 数据发送 (使用 DMA 和中断机制)
* @param id CAN 消息 ID
* @param length CAN 消息长度
* @param data CAN 消息数据
*/
u8 CAN_transmiter::CAN_AsyncSend(u32 id, u8 length, u8 *data) {
// 检查数据长度是否符合CAN规范
// 检查数据长度是否符合 CAN 规范
if (length > 8) {
printf("CAN data length exceeds maximum of 8 bytes\r\n");
CAN_DEBUG_PRINT("CAN data length exceeds maximum of 8 bytes\r\n");
return 0;
}
// 检查数据指针是否有效
if (data == nullptr) {
printf("Invalid data pointer\r\n");
CAN_DEBUG_PRINT("Invalid data pointer\r\n");
return 0;
}
/* 配置发送消息参数 */
transmit_message.tx_ft = CAN_FT_DATA; // 数据帧
transmit_message.tx_ff = CAN_FF_STANDARD; // 标准帧格式
transmit_message.tx_sfid = id; // 设置帧ID
transmit_message.tx_sfid = id; // 设置帧 ID
transmit_message.tx_dlen = length; // 设置数据长度
// 复制数据到发送缓冲区
@@ -787,45 +1009,46 @@ u8 CAN_transmiter::CAN_AsyncSend(u32 id, u8 length, u8 *data) {
transmit_message.tx_data[i] = *(data + i);
}
// 自动选择空闲mailbox发送消息
// 自动选择空闲 mailbox 发送消息
transmit_mailbox = can_message_transmit(CAN0, &transmit_message);
// 检查是否成功获取mailbox
// 检查是否成功获取 mailbox
if (transmit_mailbox == CAN_NOMAILBOX) {
// 没有可用的mailbox，启用发送中断以便在邮箱可用时重试
printf("No available mailbox for CAN transmission, enabling TX "
"interrupt\r\n");
// 没有可用的 mailbox，启用发送中断以便在邮箱可用时重试
CAN_DEBUG_PRINT(
"No available mailbox for CAN transmission, enabling TX "
"interrupt\r\n");
// 可以设置一个标志位，当邮箱可用时发送
// 在实际应用中，可以维护一个待发送队列
return 0;
}
// 启用CAN发送中断以检测传输完成
// 启用 CAN 发送中断以检测传输完成
can_interrupt_enable(CAN0, CAN_INTEN_TMEIE);
return 1;
}
/**
* @brief 异步CANFD数据发送(使用DMA和中断机制)
* @param id CAN消息ID
* @param length CAN消息长度
* @param data CAN消息数据
* @brief 异步 CANFD 数据发送 (使用 DMA 和中断机制)
* @param id CAN 消息 ID
* @param length CAN 消息长度
* @param data CAN 消息数据
*/
u8 CAN_transmiter::CANFD_AsyncSend(u32 id, u8 length, u8 *data) {
u8 valid_length = length;
// 检查CANFD模式是否启用
// 检查 CANFD 模式是否启用
if (!(CAN_CTL(CAN0) & CAN_CTL_DFZ)) {
printf("CANFD mode not enabled\r\n");
CAN_DEBUG_PRINT("CANFD mode not enabled\r\n");
return 0;
}
// 检查数据指针是否有效
if (data == nullptr) {
printf("Invalid data pointer\r\n");
CAN_DEBUG_PRINT("Invalid data pointer\r\n");
return 0;
}
// 检查数据长度是否符合CANFD规范
// 检查数据长度是否符合 CANFD 规范
if (length > 8) {
if (length > 8 and length < 13) {
valid_length = 12;
@@ -842,7 +1065,7 @@ u8 CAN_transmiter::CANFD_AsyncSend(u32 id, u8 length, u8 *data) {
} else if (length > 48 and length < 65) {
valid_length = 64;
} else {
printf("CANFD data length error\r\n");
CAN_DEBUG_PRINT("CANFD data length error\r\n");
return 0;
}
}
@@ -850,11 +1073,11 @@ u8 CAN_transmiter::CANFD_AsyncSend(u32 id, u8 length, u8 *data) {
/* 配置发送消息参数 */
transmit_message.tx_ft = CAN_FT_DATA; // 数据帧
transmit_message.tx_ff = CAN_FF_EXTENDED; // 扩展帧格式
transmit_message.tx_efid = id; // 扩展帧ID
transmit_message.tx_efid = id; // 扩展帧 ID
transmit_message.tx_dlen = valid_length; // 设置数据长度
// 设置CANFD特定参数
transmit_message.fd_flag = CAN_FDF_FDFRAME; // CANFD帧格式
// 设置 CANFD 特定参数
transmit_message.fd_flag = CAN_FDF_FDFRAME; // CANFD 帧格式
transmit_message.fd_brs = ENABLE; // 使能比特率切换
transmit_message.fd_esi = DISABLE; // 错误状态指示
// 复制数据到发送缓冲区
@@ -866,23 +1089,24 @@ u8 CAN_transmiter::CANFD_AsyncSend(u32 id, u8 length, u8 *data) {
}
}
// 自动选择空闲mailbox发送消息
// 自动选择空闲 mailbox 发送消息
transmit_mailbox = can_message_transmit(CAN0, &transmit_message);
// 检查是否成功获取mailbox
// 检查是否成功获取 mailbox
if (transmit_mailbox == CAN_NOMAILBOX) {
// 没有可用的mailbox，启用发送中断以便在邮箱可用时重试
printf("No available mailbox for CANFD transmission, enabling TX "
"interrupt\r\n");
// 没有可用的 mailbox，启用发送中断以便在邮箱可用时重试
CAN_DEBUG_PRINT(
"No available mailbox for CANFD transmission, enabling TX "
"interrupt\r\n");
return 0;
}
// 启用CAN发送中断以检测传输完成
// 启用 CAN 发送中断以检测传输完成
can_interrupt_enable(CAN0, CAN_INTEN_TMEIE);
return 1;
}
// 添加CAN传输中断处理函数
// 添加 CAN 传输中断处理函数
void CAN0_TX_IRQHandler(void) {
// 检查是否有发送邮箱空闲
if (can_flag_get(CAN0, CAN_FLAG_TME0) != RESET ||
@@ -895,4 +1119,4 @@ void CAN0_TX_IRQHandler(void) {
can_flag_clear(CAN0, CAN_FLAG_TME1);
can_flag_clear(CAN0, CAN_FLAG_TME2);
}
}
}
Robot_V2/device/Can.h
+46 -33
View File
@@ -8,20 +8,21 @@ extern "C" {
#include <cstdint>
#include "gd32c10x_can.h"
#include "config.h"
// 定义一个联合体类型用于float和byte数组之间的转换
// 定义一个联合体类型用于 float 和 byte 数组之间的转换
typedef union {
float f;
u8 b[4];
} float_union_t;
// 添加辅助函数用于将u16类型数据打包到u8数组中
// 添加辅助函数用于将 u16 类型数据打包到 u8 数组中
static inline void pack_u16_to_bytes(u8 *buffer, u16 offset, u16 value) {
buffer[offset] = (u8)(value & 0xFF);
buffer[offset + 1] = (u8)((value >> 8) & 0xFF);
}
// 添加辅助函数用于将float类型数据打包到u8数组中
// 添加辅助函数用于将 float 类型数据打包到 u8 数组中
static inline void pack_float_to_bytes(u8 *buffer, u16 offset,
float_union_t &float_union) {
buffer[offset] = float_union.b[0];
@@ -30,19 +31,19 @@ static inline void pack_float_to_bytes(u8 *buffer, u16 offset,
buffer[offset + 3] = float_union.b[3];
}
// 添加辅助函数用于从u8数组中解包short类型数据
// 添加辅助函数用于从 u8 数组中解包 short 类型数据
static inline short unpack_bytes_to_short(u8 *buffer, u16 offset) {
short value = (short)(buffer[offset] | (buffer[offset + 1] << 8));
return value;
}
// 添加辅助函数用于从u8数组中解包u16类型数据
// 添加辅助函数用于从 u8 数组中解包 u16 类型数据
static inline u16 unpack_bytes_to_u16(u8 *buffer, u16 offset) {
u16 value = (u16)(buffer[offset] | (buffer[offset + 1] << 8));
return value;
}
// 添加辅助函数用于从u8数组中解包float类型数据
// 添加辅助函数用于从 u8 数组中解包 float 类型数据
static inline float_union_t unpack_bytes_to_float(u8 *buffer, u16 offset) {
float_union_t float_union;
float_union.b[0] = buffer[offset];
@@ -52,32 +53,33 @@ static inline float_union_t unpack_bytes_to_float(u8 *buffer, u16 offset) {
return float_union;
}
u8 can_gpio_init(void); // CAN引脚初始化
void can_config(void); // CAN初始化
void canfd_config(void); // CANFD初始化
void can_dma_init(void); // CAN DMA初始化
void CAN0_RX0_IRQHandler(void); // CAN接收中断处理函数
void DMA0_Channel4_IRQHandler(void); // DMA通道4中断处理函数(CAN接收)
void DMA0_Channel5_IRQHandler(void); // DMA通道5中断处理函数(CAN发送)
u8 can_gpio_init(void); // CAN 引脚初始化
void can_config(void); // CAN 初始化
void canfd_config(void); // CANFD 初始化
void can_dma_init(void); // CAN DMA 初始化
void CAN0_RX0_IRQHandler(void); // CAN 接收中断处理函数
void DMA0_Channel4_IRQHandler(void); // DMA 通道 4 中断处理函数 (CAN 接收)
void DMA0_Channel5_IRQHandler(void); // DMA 通道 5 中断处理函数 (CAN 发送)
void jumpToBOOT();
void can_loopback_init(void);
void can_loopback_test(void);
/*!
\brief CAN通信类
\note 封装CAN发送和接收功能
\brief CAN 通信类
\note 封装 CAN 发送和接收功能，支持多电机节点 ID 隔离
*/
class CAN_transmiter {
private:
static const uint8_t CAN_RX_BUFFER_SIZE = 8; // CAN接收缓冲区大小
static const uint8_t CAN_RX_BUFFER_SIZE = 8; // CAN 接收缓冲区大小
bool is_sleep = false; // 是否休眠
u16 node_id; // 本节点 ID(0-254)，使用 CAN ID 高 8 位存储 (在 init() 中初始化为 DEFAULT_NODE_ID)
u8 Target_Control_Mode = 0; // 目标控制模式
u8 controller_Mode = 0; // 当前模式
short mcu_temp = 0; // MCU温度
short mcu_temp = 0; // MCU 温度
short motor_temp = 0; // 电机温度
u16 Power_Voltage = 0; // 电源电压
@@ -91,47 +93,58 @@ class CAN_transmiter {
float_union_t target_velocity_union = {0}; // 目标速度
float_union_t target_current_union = {0}; // 目标电流
float_union_t u_current_union = {0}; // U相电流
float_union_t w_current_union = {0}; // W相电流
float_union_t v_current_union = {0}; // V相电流
float_union_t u_current_union = {0}; // U 相电流
float_union_t w_current_union = {0}; // W 相电流
float_union_t v_current_union = {0}; // V 相电流
bool MotorStatus = 0; // 电机状态码
u8 EEPROMStatus = 0; // EEPROM状态码
u8 EEPROMStatus = 0; // EEPROM 状态码
can_trasnmit_message_struct transmit_message; // 发送消息结构体
u8 transmit_mailbox = CAN_NOMAILBOX; // 初始化为无mailbox状态
u8 transmit_data[64]; // CANFD最大64字节
u8 transmit_mailbox = CAN_NOMAILBOX; // 初始化为无 mailbox 状态
u8 transmit_data[64]; // CANFD 最大 64 字节
// CAN接收消息队列
// CAN 接收消息队列
can_receive_message_struct rx_message_buffer[CAN_RX_BUFFER_SIZE];
volatile uint8_t rx_buffer_head = 0;
volatile uint8_t rx_buffer_tail = 0;
void update(); // 更新数据
u8 CAN_Receive(); // CAN接收
u8 CANFD_Receive(); // CANFD接收
u8 CAN_Receive(); // CAN 接收
u8 CANFD_Receive(); // CANFD 接收
bool is_rx_buffer_full();
bool is_rx_buffer_empty();
bool dequeue_rx_message(can_receive_message_struct& message);
public:
// 节点 ID 相关辅助函数 (使用高 16 位存储节点 ID)
u32 build_can_id(u32 base_id); // 根据节点 ID 构建 CAN ID，格式：[节点 ID(16 位)][基础 ID(16 位)]
bool is_message_for_me(u32 received_id); // 判断消息是否发给本节点
public:
// 添加构造函数声明
u8 init(void);
can_receive_message_struct receive_message; // 接收消息结构体
FlagStatus can0_send_flag; // CAN0发送标志
FlagStatus can0_send_flag; // CAN0 发送标志
float_union_t testfloat_union = {0}; // 测试用
u16 test_u16 = 0;
bool MagStatus = 0; // 磁状态码
bool ThermalStatus = 0; // 热能状态码
void communication_check(); // 通信检查
u8 CAN_Sleep(); // CAN进入睡眠
u8 CAN_Wakeup(); // CAN进入睡眠
u8 CAN_AsyncSend(u32 ID, u8 length, u8 *data); // CAN异步发送(使用DMA)
u8 CANFD_AsyncSend(u32 ID, u8 length, u8 *data); // CANFD异步发送(使用DMA)
u8 CAN_all_Send(); // CAN发送
u8 CANFD_all_Send(); // CANFD发送
u8 CAN_Sleep(); // CAN 进入睡眠
u8 CAN_Wakeup(); // CAN 唤醒
u8 CAN_AsyncSend(u32 ID, u8 length, u8 *data); // CAN 异步发送 (使用 DMA)
u8 CANFD_AsyncSend(u32 ID, u8 length, u8 *data); // CANFD 异步发送 (使用 DMA)
u8 CAN_all_Send(); // CAN 发送
u8 CANFD_all_Send(); // CANFD 发送
// 节点 ID 管理接口
u16 get_node_id(); // 获取当前节点 ID
void set_node_id(u16 new_id); // 设置节点 ID
u8 load_node_id_from_flash(); // 从 Flash 加载节点 ID
u8 save_node_id_to_flash(u16 new_id); // 保存节点 ID 到 Flash
// 中断处理函数需要访问的公共接口
void enqueue_rx_message_from_isr(const can_receive_message_struct& message) {
Robot_V2/device/ang_sensor.cpp
+39 -33
View File
@@ -11,12 +11,19 @@
#include <cstring>
#include <stdint.h>
// ANG_SENS 调试宏定义
#if ANG_SENS_DEBUG
#define ANG_SENS_DEBUG_PRINT(fmt, ...) printf(fmt, ##__VA_ARGS__)
#else
#define ANG_SENS_DEBUG_PRINT(fmt, ...) ((void)0)
#endif
#define ANG_SPI_CS_ENABLE() gpio_bit_reset(GPIOA, GPIO_PIN_15)
#define ANG_SPI_CS_DISABLE() gpio_bit_set(GPIOA, GPIO_PIN_15)
#define DRV_SPI_CS_ENABLE() gpio_bit_reset(GPIOB, GPIO_PIN_14)
#define DRV_SPI_CS_DISABLE() gpio_bit_set(GPIOB, GPIO_PIN_14)
// DMA传输完成标志
// DMA 传输完成标志
volatile bool spi_dma_tx_complete = false;
volatile bool spi_dma_rx_complete = false;
@@ -24,7 +31,7 @@ angle_sensor KTH7823; // 角度传感器初始化
extern FlashEEPROM flashEEP;
extern EncoderAngleKalman Angle_kalman;
extern EncoderVelocityKalman Vel_kalman;
extern CAN_transmiter CAN0_transmiter; // CAN初始化
extern CAN_transmiter CAN0_transmiter; // CAN 初始化
/**
* @brief SPI DMA initialization
@@ -32,10 +39,10 @@ extern CAN_transmiter CAN0_transmiter; // CAN初始化
void spi_dma_init(void) {
dma_parameter_struct dma_init_struct;
// 使能DMA时钟
// 使能 DMA 时钟
rcu_periph_clock_enable(RCU_DMA0);
// 初始化发送DMA通道
// 初始化发送 DMA 通道
dma_deinit(DMA0, DMA_CH2);
dma_struct_para_init(&dma_init_struct);
dma_init_struct.direction = DMA_MEMORY_TO_PERIPHERAL;
@@ -49,7 +56,7 @@ void spi_dma_init(void) {
dma_init_struct.priority = DMA_PRIORITY_HIGH;
dma_init(DMA0, DMA_CH2, &dma_init_struct);
// 初始化接收DMA通道
// 初始化接收 DMA 通道
dma_deinit(DMA0, DMA_CH1);
dma_struct_para_init(&dma_init_struct);
dma_init_struct.direction = DMA_PERIPHERAL_TO_MEMORY;
@@ -63,15 +70,15 @@ void spi_dma_init(void) {
dma_init_struct.priority = DMA_PRIORITY_HIGH;
dma_init(DMA0, DMA_CH1, &dma_init_struct);
// 使能SPI的DMA发送和接收功能
// 使能 SPI 的 DMA 发送和接收功能
spi_dma_enable(SPI0, SPI_DMA_TRANSMIT);
spi_dma_enable(SPI0, SPI_DMA_RECEIVE);
// 配置DMA中断
// 配置 DMA 中断
nvic_irq_enable(DMA0_Channel2_IRQn, 0, 1);
nvic_irq_enable(DMA0_Channel1_IRQn, 0, 1);
// 使能DMA传输完成中断
// 使能 DMA 传输完成中断
dma_interrupt_enable(DMA0, DMA_CH2, DMA_INT_FTF);
dma_interrupt_enable(DMA0, DMA_CH1, DMA_INT_FTF);
}
@@ -90,30 +97,30 @@ int spi_rw_half_word_dma(uint32_t spi_periph, uint16_t tx_data,
spi_dma_tx_complete = false;
spi_dma_rx_complete = false;
// 配置发送DMA
// 配置发送 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
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个循环
// 等待传输完成，超时时间为 100 个循环
uint32_t timeout = 100;
while ((!spi_dma_tx_complete || !spi_dma_rx_complete) && --timeout) {
// 空循环等待DMA传输完成
// 空循环等待 DMA 传输完成
}
// 检查是否超时
if (timeout == 0) {
// 禁用DMA通道
// 禁用 DMA 通道
dma_channel_disable(DMA0, DMA_CH2);
dma_channel_disable(DMA0, DMA_CH1);
printf("SPI DMA transfer timeout\r\n");
ANG_SENS_DEBUG_PRINT("SPI DMA transfer timeout\r\n");
return -1;
}
@@ -121,7 +128,7 @@ int spi_rw_half_word_dma(uint32_t spi_periph, uint16_t tx_data,
}
/**
* @brief DMA0_Channel2中断处理函数
* @brief DMA0_Channel2 中断处理函数
* @param 无
*/
extern "C" void DMA0_Channel2_IRQHandler(void) {
@@ -132,7 +139,7 @@ extern "C" void DMA0_Channel2_IRQHandler(void) {
}
/**
* @brief DMA0_Channel1中断处理函数
* @brief DMA0_Channel1 中断处理函数
* @param 无
*/
extern "C" void DMA0_Channel1_IRQHandler(void) {
@@ -174,7 +181,7 @@ 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 参数：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;
@@ -194,16 +201,16 @@ void KTH7823_spi_init(void) {
* @return 0:初始化成功 1:初始化失败
*/
u8 angle_sensor::init(void) {
KTH7823_gpio_init(); // GPIO初始化
KTH7823_spi_init(); // SPI初始化
spi_dma_init(); // DMA初始化
KTH7823_gpio_init(); // GPIO 初始化
KTH7823_spi_init(); // SPI 初始化
spi_dma_init(); // DMA 初始化
return 0;
}
/**
* @ 角度传感器类 设置当前角度
* @param angle 输出级角度值
* @return 0成功/1失败
* @return 0 成功/1 失败
*/
u8 angle_sensor::angle_set(float angle) {
m_cumulativeAngle = angle * REDUCTION_RATIO;
@@ -230,10 +237,9 @@ inline float angle_sensor::read_sensorAngle(void) {
return angle_deg;
}
// 仅在调试模式下输出错误，避免发布版本中的I/O开销
// 仅在调试模式下输出错误，避免发布版本中的 I/O 开销
if (result != 0) {
// 使用更高效的串口输出替代printf（如果可用）
printf("Error reading angle from sensor.\n");
ANG_SENS_DEBUG_PRINT("Error reading angle from sensor.\n");
}
return (float)FAULT_RECEIVE;
@@ -255,13 +261,13 @@ float angle_sensor::update(void) {
// 计算角度变化
float delta = current_angle_deg - m_lastAngle;
// 处理角度绕回情况（0度和360度之间的跳变）
// 如果变化超过300度，说明发生了绕回
// 处理角度绕回情况（0 度和 360 度之间的跳变）
// 如果变化超过 300 度，说明发生了绕回
if (delta > ANGLE_WRAP_THRESHOLD) {
// 从接近360度绕回到接近0度
// 从接近 360 度绕回到接近 0 度
delta -= ANGLE_WRAP_VALUE;
} else if (delta < -ANGLE_WRAP_THRESHOLD) {
// 从接近0度绕到接近360度
// 从接近 0 度绕到接近 360 度
delta += ANGLE_WRAP_VALUE;
}
@@ -271,7 +277,7 @@ float angle_sensor::update(void) {
}
/**
* @brief 获取累积角度值(输出级)
* @brief 获取累积角度值 (输出级)
* @return 累积角度值
*/
float angle_sensor::getAngle(void) const {
@@ -279,15 +285,15 @@ float angle_sensor::getAngle(void) const {
}
/**
* @brief 获取累积角度值(原始级)
* @brief 获取累积角度值 (原始级)
* @return 累积角度值
*/
float angle_sensor::getRawAngle(void) const { return m_cumulativeAngle; }
/**
* @brief 获取角度值(原始级)
* @return 角度值(原始级)
* @brief 获取角度值 (原始级)
* @return 角度值 (原始级)
*/
float angle_sensor::getcurrent_sensorangle(void) const{
return current_angle_deg;
}
}
Robot_V2/device/test.cpp
+17 -9
View File
@@ -1,25 +1,33 @@
#include "adc.h"
#include "config.h"
#include <cstdio>
// TEST 调试宏定义
#if TEST_DEBUG
#define TEST_DEBUG_PRINT(fmt, ...) printf(fmt, ##__VA_ARGS__)
#else
#define TEST_DEBUG_PRINT(fmt, ...) ((void)0)
#endif
constexpr int status_print = 0;
extern sampler sampler;
void test_1111() {
// 初始化CAN回环测试
// 初始化 CAN 回环测试
// can_loopback_init();
// can_config();
if (status_print) {
/* value print */
printf(" the temperature data is %2.2f degrees Celsius\r\n",
TEST_DEBUG_PRINT(" the temperature data is %2.2f degrees Celsius\r\n",
sampler.get_mcu_temp());
printf(" the drv_temperature data is %2.2f degrees Celsius\r\n",
TEST_DEBUG_PRINT(" the drv_temperature data is %2.2f degrees Celsius\r\n",
sampler.get_drv_temp());
printf(" the U current data is %2.2f mA\r\n", sampler.get_current('U'));
printf(" the V current data is %2.2f mA\r\n", sampler.get_current('V'));
printf(" the W current data is %2.2f mA\r\n", sampler.get_current('W'));
printf(" the VBUS is %2.2f V\r\n", sampler.get_vbus_voltage());
// printf(" the angle is %2.2f\r\n", tle5012b_read());
TEST_DEBUG_PRINT(" the U current data is %2.2f mA\r\n", sampler.get_current('U'));
TEST_DEBUG_PRINT(" the V current data is %2.2f mA\r\n", sampler.get_current('V'));
TEST_DEBUG_PRINT(" the W current data is %2.2f mA\r\n", sampler.get_current('W'));
TEST_DEBUG_PRINT(" the VBUS is %2.2f V\r\n", sampler.get_vbus_voltage());
// TEST_DEBUG_PRINT(" the angle is %2.2f\r\n", tle5012b_read());
printf("\n----------------------------------------------------\n");
TEST_DEBUG_PRINT("\n----------------------------------------------------\n");
}
// can_loopback_test();
}