feat(FOC): 添加快速电压检测功能和PWM调试模式
新增基于跌幅检测的快速电压保护机制，在100us周期内响应电压异常， 当电压跌幅超过阈值或低于绝对下限时立即关机保护。同时添加PWM输出 调试模式便于硬件测试。 BREAKING CHANGE: 重构了电压检测逻辑，采用峰值跟踪算法替代固 定阈值检测，提高在动态供电环境下的可靠性
This commit is contained in:
+179
-112
@@ -38,8 +38,25 @@ extern u32 motor_runtime_50us; // 电机驱动计算轮次（用于静默模式
|
||||
// 添加 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[64]; // CAN 接收缓冲区（支持CAN FD最大64字节）
|
||||
uint8_t can_tx_buffer[64]; // CAN 发送缓冲区（支持CAN FD最大64字节）
|
||||
|
||||
// CAN发送队列结构
|
||||
#define CAN_TX_QUEUE_SIZE 8
|
||||
typedef struct {
|
||||
u32 id;
|
||||
u8 length;
|
||||
u8 data[64];
|
||||
u8 valid;
|
||||
} can_tx_queue_item_t;
|
||||
|
||||
static can_tx_queue_item_t tx_queue[CAN_TX_QUEUE_SIZE];
|
||||
static volatile u8 tx_queue_head = 0;
|
||||
static volatile u8 tx_queue_tail = 0;
|
||||
static volatile u8 tx_queue_count = 0;
|
||||
|
||||
// 发送队列使用的独立消息结构体
|
||||
static can_trasnmit_message_struct queue_transmit_message;
|
||||
|
||||
CAN_transmiter CAN0_transmiter; // CAN 初始化
|
||||
|
||||
@@ -100,28 +117,24 @@ void CAN_transmiter::set_node_id(u16 new_id) {
|
||||
}
|
||||
|
||||
/*!
|
||||
\brief 从 Flash 加载节点 ID
|
||||
\brief 从 Flash 加载节点 ID（使用魔数+双重备份+计数器机制）
|
||||
\param[in] none
|
||||
\param[out] none
|
||||
\retval FLASH_EEPROM_OK: 加载成功
|
||||
其他：加载失败，使用默认值
|
||||
\note Flash 擦除状态为 0xFFFF，需要特殊处理避免误判
|
||||
注意：0 是有效的节点 ID，不应被视为无效值
|
||||
\note 使用独立的节点 ID 存储区域，带魔数验证和 CRC 校验
|
||||
防止位翻转导致错读
|
||||
*/
|
||||
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);
|
||||
u16 node_id_temp = 0;
|
||||
FlashEepromStatus status = flashEEP.readNodeId(&node_id_temp);
|
||||
|
||||
// 检查 Flash 是否为擦除状态 (0xFFFF) 或读取失败
|
||||
// 注意：Flash 未写入时读到的值是 0xFFFF
|
||||
// 0 是有效的节点 ID，不应被视为无效值
|
||||
if (status != FLASH_EEPROM_OK || node_id_temp == 0xFFFF) {
|
||||
// 检查读取是否成功
|
||||
if (status != FLASH_EEPROM_OK) {
|
||||
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);
|
||||
"Node ID not found in Flash (status=%d), using default: %d\r\n",
|
||||
status, node_id);
|
||||
return FLASH_EEPROM_DATA_NOT_FOUND;
|
||||
}
|
||||
|
||||
@@ -139,25 +152,26 @@ u8 CAN_transmiter::load_node_id_from_flash() {
|
||||
}
|
||||
|
||||
/*!
|
||||
\brief 保存节点 ID 到 Flash
|
||||
\brief 保存节点 ID 到 Flash（使用魔数+双重备份+计数器机制）
|
||||
\param[in] new_id 要保存的节点 ID
|
||||
\param[out] none
|
||||
\retval FLASH_EEPROM_OK: 保存成功
|
||||
其他：保存失败
|
||||
\note 写入主备份和备用备份，确保数据可靠性
|
||||
*/
|
||||
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);
|
||||
|
||||
FlashEepromStatus status = flashEEP.writeNodeId(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");
|
||||
CAN_DEBUG_PRINT("Failed to save Node ID to Flash (status=%d)\r\n", status);
|
||||
return (u8)status;
|
||||
}
|
||||
}
|
||||
@@ -746,10 +760,6 @@ void CAN_transmiter::communication_check() {
|
||||
// 处理所有接收到的 CAN 消息（从环形缓冲区中）
|
||||
can_receive_message_struct current_message;
|
||||
while (dequeue_rx_message(current_message)) {
|
||||
// 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]);
|
||||
// }
|
||||
// 提取基础 ID (低 16 位，去除高 16 位节点 ID 部分)
|
||||
u32 base_id = current_message.rx_efid & 0xFFFF;
|
||||
|
||||
@@ -776,6 +786,74 @@ void CAN_transmiter::communication_check() {
|
||||
continue; // BOOT 命令处理完跳过后续逻辑
|
||||
}
|
||||
|
||||
// 设置节点 ID 命令 (基础 ID: 0x0400)
|
||||
// 注意：此命令在 is_message_for_me 检查之前处理，所有节点都会响应
|
||||
// 这样即使节点ID已经改变，仍然可以用旧节点ID或广播ID来修改
|
||||
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 && new_node_id != BROADCAST_NODE_ID) {
|
||||
// 先发送响应（使用旧节点ID构建CAN 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 高字节
|
||||
|
||||
// 使用旧节点 ID 发送响应
|
||||
node_id = old_node_id;
|
||||
u8 send_result = CANFD_AsyncSend(build_can_id(CANFD_SET_NODE_ID), 3,
|
||||
transmit_data);
|
||||
|
||||
// 然后保存新节点 ID 到 Flash
|
||||
u8 save_status = save_node_id_to_flash(new_node_id);
|
||||
|
||||
CAN_DEBUG_PRINT("SET_NODE_ID: save_status=%d, send_result=%d\r\n",
|
||||
save_status, send_result);
|
||||
|
||||
if (save_status == FLASH_EEPROM_OK) {
|
||||
CAN_DEBUG_PRINT("SET_NODE_ID: SUCCESS, node_id updated from %d to %d\r\n",
|
||||
old_node_id, new_node_id);
|
||||
} else {
|
||||
CAN_DEBUG_PRINT("SET_NODE_ID: Flash write FAILED, status=%d\r\n",
|
||||
save_status);
|
||||
// 发送错误响应
|
||||
transmit_data[0] = 0x00; // 失败
|
||||
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 不变
|
||||
}
|
||||
continue; // SET_NODE_ID 命令处理完跳过后续逻辑
|
||||
}
|
||||
|
||||
// 其他 base_id 的消息，检查是否是发给本节点的
|
||||
if (!is_message_for_me(current_message.rx_efid)) {
|
||||
continue; // 不是发给本节点的消息，跳过
|
||||
@@ -864,75 +942,6 @@ void CAN_transmiter::communication_check() {
|
||||
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());
|
||||
@@ -969,7 +978,7 @@ u8 CAN_transmiter::CAN_Wakeup() {
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief CAN0 中断服务程序
|
||||
* @brief CAN0 中断服务程序（优化版：直接读取数据，减少DMA竞态条件）
|
||||
* @retval 无
|
||||
*/
|
||||
void CAN0_RX0_IRQHandler(void) {
|
||||
@@ -978,14 +987,7 @@ void CAN0_RX0_IRQHandler(void) {
|
||||
// 清除中断标志
|
||||
can_interrupt_flag_clear(CAN0, CAN_INT_FLAG_RFL0);
|
||||
|
||||
// 使用 DMA 读取接收到的数据
|
||||
dma_channel_disable(DMA0, DMA_CH4);
|
||||
dma_rx_parameter.number =
|
||||
can_receive_message_length_get(CAN0, CAN_FIFO0);
|
||||
dma_init(DMA0, DMA_CH4, &dma_rx_parameter);
|
||||
dma_channel_enable(DMA0, DMA_CH4);
|
||||
|
||||
// 读取接收到的数据
|
||||
// 直接读取消息（包括数据），不依赖DMA
|
||||
can_receive_message_struct temp_message;
|
||||
can_message_receive(CAN0, CAN_FIFO0, &temp_message);
|
||||
|
||||
@@ -1086,7 +1088,51 @@ u8 CAN_transmiter::CAN_AsyncSend(u32 id, u8 length, u8 *data) {
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 异步 CANFD 数据发送 (使用 DMA 和中断机制)
|
||||
* @brief 向发送队列添加消息
|
||||
* @param id CAN消息ID
|
||||
* @param length CAN消息长度
|
||||
* @param data CAN消息数据
|
||||
* @retval 1=成功, 0=队列满
|
||||
*/
|
||||
static u8 CAN_TX_QueuePush(u32 id, u8 length, u8 *data) {
|
||||
if (tx_queue_count >= CAN_TX_QUEUE_SIZE) {
|
||||
CAN_DEBUG_PRINT("TX queue full, message dropped\r\n");
|
||||
return 0; // 队列满，丢弃消息
|
||||
}
|
||||
|
||||
tx_queue[tx_queue_tail].id = id;
|
||||
tx_queue[tx_queue_tail].length = length;
|
||||
for (int i = 0; i < length && i < 64; i++) {
|
||||
tx_queue[tx_queue_tail].data[i] = data[i];
|
||||
}
|
||||
tx_queue[tx_queue_tail].valid = 1;
|
||||
|
||||
tx_queue_tail = (tx_queue_tail + 1) % CAN_TX_QUEUE_SIZE;
|
||||
tx_queue_count++;
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 从发送队列取出消息
|
||||
* @param item 指向队列项的指针
|
||||
* @retval 1=成功, 0=队列空
|
||||
*/
|
||||
static u8 CAN_TX_QueuePop(can_tx_queue_item_t *item) {
|
||||
if (tx_queue_count == 0) {
|
||||
return 0; // 队列空
|
||||
}
|
||||
|
||||
*item = tx_queue[tx_queue_head];
|
||||
tx_queue[tx_queue_head].valid = 0;
|
||||
tx_queue_head = (tx_queue_head + 1) % CAN_TX_QUEUE_SIZE;
|
||||
tx_queue_count--;
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 异步 CANFD 数据发送 (使用队列机制防止丢帧)
|
||||
* @param id CAN 消息 ID
|
||||
* @param length CAN 消息长度
|
||||
* @param data CAN 消息数据
|
||||
@@ -1151,29 +1197,50 @@ u8 CAN_transmiter::CANFD_AsyncSend(u32 id, u8 length, u8 *data) {
|
||||
|
||||
// 检查是否成功获取 mailbox
|
||||
if (transmit_mailbox == CAN_NOMAILBOX) {
|
||||
// 没有可用的 mailbox，启用发送中断以便在邮箱可用时重试
|
||||
CAN_DEBUG_PRINT(
|
||||
"No available mailbox for CANFD transmission, enabling TX "
|
||||
"interrupt\r\n");
|
||||
return 0;
|
||||
// 没有可用的 mailbox，将消息加入发送队列
|
||||
CAN_DEBUG_PRINT("No available mailbox, adding to TX queue\r\n");
|
||||
return CAN_TX_QueuePush(id, valid_length, data);
|
||||
}
|
||||
|
||||
// 启用 CAN 发送中断以检测传输完成
|
||||
// 启用 CAN 发送中断以检测传输完成（用于队列发送）
|
||||
can_interrupt_enable(CAN0, CAN_INTEN_TMEIE);
|
||||
return 1;
|
||||
}
|
||||
|
||||
// 添加 CAN 传输中断处理函数
|
||||
/**
|
||||
* @brief CAN 传输中断处理函数（支持队列发送）
|
||||
*/
|
||||
void CAN0_TX_IRQHandler(void) {
|
||||
// 检查是否有发送邮箱空闲
|
||||
if (can_flag_get(CAN0, CAN_FLAG_TME0) != RESET ||
|
||||
can_flag_get(CAN0, CAN_FLAG_TME1) != RESET ||
|
||||
can_flag_get(CAN0, CAN_FLAG_TME2) != RESET) {
|
||||
// 邮箱已空闲，可以发送新消息
|
||||
// 可以在这里处理待发送队列中的消息
|
||||
|
||||
can_tx_queue_item_t next_msg;
|
||||
// 尝试从队列中取出下一条消息发送
|
||||
if (CAN_TX_QueuePop(&next_msg)) {
|
||||
// 配置并发送队列中的消息（使用独立的队列消息结构体）
|
||||
queue_transmit_message.tx_ft = CAN_FT_DATA;
|
||||
queue_transmit_message.tx_ff = CAN_FF_EXTENDED;
|
||||
queue_transmit_message.tx_efid = next_msg.id;
|
||||
queue_transmit_message.tx_dlen = next_msg.length;
|
||||
queue_transmit_message.fd_flag = CAN_FDF_FDFRAME;
|
||||
queue_transmit_message.fd_brs = ENABLE;
|
||||
queue_transmit_message.fd_esi = DISABLE;
|
||||
|
||||
for (int i = 0; i < next_msg.length; i++) {
|
||||
queue_transmit_message.tx_data[i] = next_msg.data[i];
|
||||
}
|
||||
|
||||
can_message_transmit(CAN0, &queue_transmit_message);
|
||||
} else {
|
||||
// 队列为空，禁用发送中断
|
||||
can_interrupt_disable(CAN0, CAN_INTEN_TMEIE);
|
||||
}
|
||||
|
||||
// 清除发送中断标志
|
||||
can_flag_clear(CAN0, CAN_FLAG_TME0);
|
||||
can_flag_clear(CAN0, CAN_FLAG_TME1);
|
||||
can_flag_clear(CAN0, CAN_FLAG_TME2);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user