cbb
7c4aaa5ee6
- 重构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算法，显著提升性能 - 保留通用回退路径处理其他尺寸情况
1267 lines
39 KiB
C++
1267 lines
39 KiB
C++
/**
|
||
* @file device/Can.cpp
|
||
* @brief 底层设备驱动与外设封装源文件
|
||
* @note 本文件仅补充 Doxygen 文档注释，不改变代码逻辑。
|
||
*/
|
||
|
||
#include "Can.h"
|
||
#include "FOC.h"
|
||
#include "Filter.h"
|
||
#include "adc.h"
|
||
#include "ang_sensor.h"
|
||
#include "config.h"
|
||
#include "flash.h"
|
||
#include "gd32c10x.h"
|
||
#include "gd32c10x_can.h"
|
||
#include "gd32c10x_dma.h"
|
||
#include "gd32c10x_fmc.h"
|
||
#include "gd32c10x_gpio.h"
|
||
#include "initial.h"
|
||
#include "motor.h"
|
||
#include "systick.h"
|
||
#include <cstdio>
|
||
#include <cstdlib>
|
||
#include <cstring>
|
||
|
||
// CAN 调试宏定义
|
||
#if CAN_DEBUG
|
||
/** @brief CAN_DEBUG_PRINT 宏定义 */
|
||
#define CAN_DEBUG_PRINT(fmt, ...) printf(fmt, ##__VA_ARGS__)
|
||
#else
|
||
/** @brief CAN_DEBUG_PRINT 宏定义 */
|
||
#define CAN_DEBUG_PRINT(fmt, ...) ((void)0)
|
||
#endif
|
||
|
||
extern FOC_Controller FOC;
|
||
extern sampler sampler;
|
||
extern angle_sensor KTH7823;
|
||
extern EncoderAngleKalman Angle_kalman; // 角度滤波
|
||
extern EncoderVelocityKalman Vel_kalman; // 速度滤波
|
||
extern FlashEEPROM flashEEP;
|
||
extern VersionManager Version; // 声明外部版本管理器实例
|
||
extern u32 motor_runtime_50us; // 电机驱动计算轮次（用于静默模式计时）
|
||
|
||
// CAN RX DMA 与收发队列模块状态：
|
||
// - DMA 元数据/busy 标志由 CAN RX 中断与 DMA 完成中断协同维护；
|
||
// - rx/tx 队列 head/tail/count 同时被主循环和中断访问，队列操作通过 CAN_EnterCritical()/CAN_ExitCritical() 保护。
|
||
dma_parameter_struct dma_rx_parameter;
|
||
uint8_t can_rx_buffer[64]; // CAN 接收缓冲区（支持CAN FD最大64字节）
|
||
static uint32_t can_rx_dma_words[16]; // CAN FD接收DMA按32位搬运，最多64字节
|
||
static volatile bool can_rx_dma_busy = false;
|
||
static can_receive_message_struct can_rx_dma_message;
|
||
|
||
// CAN接收/发送队列结构
|
||
/** @brief CAN_RX_QUEUE_SIZE 宏定义 */
|
||
#define CAN_RX_QUEUE_SIZE 32
|
||
/** @brief CAN_TX_QUEUE_SIZE 宏定义 */
|
||
#define CAN_TX_QUEUE_SIZE 32
|
||
|
||
typedef struct {
|
||
u32 id;
|
||
u8 length;
|
||
u8 data[64];
|
||
} can_rx_queue_item_t;
|
||
|
||
typedef struct {
|
||
can_trasnmit_message_struct message;
|
||
u8 valid;
|
||
} can_tx_queue_item_t;
|
||
|
||
static can_rx_queue_item_t rx_queue[CAN_RX_QUEUE_SIZE];
|
||
static volatile u8 rx_queue_head = 0;
|
||
static volatile u8 rx_queue_tail = 0;
|
||
static volatile u8 rx_queue_count = 0;
|
||
|
||
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;
|
||
|
||
// CAN FD DLC 9~15对应的非线性payload长度表
|
||
static constexpr u8 CANFD_DLC_LENGTH_TABLE[7] = {12, 16, 20, 24, 32, 48, 64};
|
||
|
||
// 全局 CAN 发送器单例：系统只有一个 CAN0 通信对象。
|
||
// 初始化顺序由 main::hardware_init() 调用 CAN0_transmiter.init() 保证。
|
||
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
|
||
// ============================================================================
|
||
|
||
/*!
|
||
\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 当前节点 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 使用独立的节点 ID 双备份存储区（主/备独立 Flash 页），
|
||
单份损坏自动回退另一份；带魔数验证和 CRC 校验
|
||
防止位翻转导致错读
|
||
*/
|
||
u8 CAN_transmiter::load_node_id_from_flash() {
|
||
u16 node_id_temp = 0;
|
||
FlashEepromStatus status = flashEEP.readNodeId(&node_id_temp);
|
||
|
||
// 检查读取是否成功
|
||
if (status != FLASH_EEPROM_OK) {
|
||
node_id = DEFAULT_NODE_ID;
|
||
CAN_DEBUG_PRINT(
|
||
"Node ID not found in Flash (status=%d), using default: %d\r\n",
|
||
status, node_id);
|
||
return FLASH_EEPROM_DATA_NOT_FOUND;
|
||
}
|
||
|
||
// 检查节点 ID 是否有效 (0-65534 都是有效值)
|
||
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: 保存成功
|
||
其他：保存失败
|
||
\note 乒乓式写入主/备两个独立 Flash 页之一，另一页始终保有
|
||
旧的有效记录，掉电时至少保留一份有效数据
|
||
*/
|
||
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.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 (status=%d)\r\n", status);
|
||
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 时， 表示缓冲区已达到最大容量
|
||
*/
|
||
bool CAN_transmiter::is_rx_buffer_full() {
|
||
return ((rx_buffer_head + 1) % CAN_RX_BUFFER_SIZE) == rx_buffer_tail;
|
||
}
|
||
|
||
/*!
|
||
\brief 检查 CAN 接收缓冲区是否为空
|
||
\param[in] none
|
||
\param[out] none
|
||
\retval bool 返回 true 表示缓冲区为空，false 表示缓冲区不为空
|
||
\note 通过比较头指针和尾指针是否相等来判断缓冲区是否为空
|
||
*/
|
||
bool CAN_transmiter::is_rx_buffer_empty() {
|
||
return rx_buffer_head == rx_buffer_tail;
|
||
}
|
||
|
||
/**
|
||
* @brief 从接收缓冲区中取出一条 CAN 消息
|
||
*
|
||
* 该函数检查接收缓冲区是否为空，如果不为空则取出缓冲区队列头部的消息，
|
||
* 并更新队列尾部索引。如果缓冲区为空，则返回 false。
|
||
*
|
||
* @param message 引用类型的输出参数，用于存储取出的 CAN 接收消息结构体
|
||
* @return bool 返回操作是否成功，true 表示成功取出消息，false 表示缓冲区为空
|
||
*/
|
||
bool CAN_transmiter::dequeue_rx_message(can_receive_message_struct &message) {
|
||
if (!is_rx_buffer_empty()) {
|
||
message = rx_message_buffer[rx_buffer_tail];
|
||
rx_buffer_tail = (rx_buffer_tail + 1) % CAN_RX_BUFFER_SIZE;
|
||
return true;
|
||
}
|
||
return false;
|
||
}
|
||
|
||
// 静默模式相关函数实现
|
||
// 使用 systick 获取系统时间（ms）
|
||
/**
|
||
* @brief 获取系统毫秒计数
|
||
* @return extern u32 返回值
|
||
*/
|
||
extern u32 get_systick_ms(void);
|
||
|
||
/*!
|
||
\brief 进入静默模式（停发 10 秒）
|
||
*/
|
||
void CAN_transmiter::enter_silent_mode() {
|
||
is_silent = true;
|
||
silent_start_time = get_systick_ms(); // 记录当前时间（ms）
|
||
CAN_DEBUG_PRINT("Enter silent mode for %d ms, start_time=%d\r\n", CAN_SILENT_DURATION_MS, silent_start_time);
|
||
}
|
||
|
||
/*!
|
||
\brief 检查是否在静默模式中
|
||
*/
|
||
bool CAN_transmiter::is_in_silent_mode() { return is_silent; }
|
||
|
||
/*!
|
||
\brief 检查静默模式是否结束
|
||
*/
|
||
void CAN_transmiter::check_silent_mode() {
|
||
if (is_silent) {
|
||
u32 current_time = get_systick_ms(); // 当前时间（ms）
|
||
if (current_time - silent_start_time >= CAN_SILENT_DURATION_MS) {
|
||
is_silent = false;
|
||
CAN_DEBUG_PRINT("Exit silent mode, elapsed=%d ms\r\n", current_time - silent_start_time);
|
||
}
|
||
}
|
||
}
|
||
|
||
/*!
|
||
\brief initialize CAN GPIO
|
||
\param[in] none
|
||
\param[out] none
|
||
\retval ErrStatus: SUCCESS or ERROR
|
||
*/
|
||
u8 can_gpio_init() {
|
||
/* enable CAN clock */
|
||
rcu_periph_clock_enable(RCU_CAN0);
|
||
rcu_periph_clock_enable(RCU_GPIOB);
|
||
rcu_periph_clock_enable(RCU_AF);
|
||
|
||
/* configure CAN0 GPIO */
|
||
gpio_init(GPIOB, GPIO_MODE_IPU, GPIO_OSPEED_50MHZ, GPIO_PIN_8); // rx
|
||
gpio_init(GPIOB, GPIO_MODE_AF_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_9); // tx
|
||
gpio_pin_remap_config(GPIO_CAN0_PARTIAL_REMAP, ENABLE);
|
||
/* enable the EN GPIO clock */
|
||
gpio_init(GPIOB, GPIO_MODE_OUT_PP, GPIO_OSPEED_50MHZ,
|
||
GPIO_PIN_13 | GPIO_PIN_14);
|
||
gpio_bit_reset(GPIOB, GPIO_PIN_13);
|
||
gpio_bit_reset(GPIOB, GPIO_PIN_14);
|
||
return 0;
|
||
}
|
||
|
||
/*!
|
||
\brief 初始化 CAN DMA
|
||
\param[in] none
|
||
\param[out] none
|
||
\retval none
|
||
*/
|
||
void can_dma_init(void) {
|
||
// 使能 DMA 时钟
|
||
rcu_periph_clock_enable(RCU_DMA0);
|
||
|
||
// 配置接收 DMA
|
||
dma_struct_para_init(&dma_rx_parameter);
|
||
dma_rx_parameter.periph_addr =
|
||
(uint32_t)(&CAN_RFIFOMDATA0(CAN0, CAN_FIFO0)); // CAN 接收数据寄存器地址
|
||
dma_rx_parameter.memory_addr = (uint32_t)can_rx_dma_words; // 接收 DMA 缓冲区地址
|
||
dma_rx_parameter.direction = DMA_PERIPHERAL_TO_MEMORY; // 外设到内存
|
||
dma_rx_parameter.memory_width = DMA_MEMORY_WIDTH_32BIT; // 内存宽度 32 位
|
||
dma_rx_parameter.periph_width = DMA_PERIPHERAL_WIDTH_32BIT; // 外设宽度 32 位
|
||
dma_rx_parameter.priority = DMA_PRIORITY_HIGH; // 高优先级
|
||
dma_rx_parameter.number = 16; // 最多 16 个 word = 64 字节
|
||
dma_rx_parameter.periph_inc = DMA_PERIPH_INCREASE_DISABLE; // CAN FIFO 数据端口不递增
|
||
dma_rx_parameter.memory_inc = DMA_MEMORY_INCREASE_ENABLE; // 内存地址递增
|
||
dma_init(DMA0, DMA_CH4, &dma_rx_parameter); // 使用 DMA 通道 4
|
||
dma_memory_to_memory_enable(DMA0, DMA_CH4); // CAN 无独立 DMA request 时，按帧启动 M2M 读取 FIFO 数据端口
|
||
|
||
// RX DMA 按帧启动，初始化时不使能 CH4，避免无有效帧时搬运垃圾数据
|
||
dma_channel_disable(DMA0, DMA_CH4);
|
||
can_rx_dma_busy = false;
|
||
memset(can_rx_dma_words, 0, sizeof(can_rx_dma_words));
|
||
}
|
||
|
||
|
||
/*!
|
||
\brief 初始化 CAN0 CANFD 模式
|
||
\param[in] none
|
||
\param[out] none
|
||
\retval none
|
||
*/
|
||
void canfd_config(void) {
|
||
can_gpio_init();
|
||
can_dma_init();
|
||
can_parameter_struct can_parameter;
|
||
can_fdframe_struct can_fd_parameter;
|
||
can_fd_tdc_struct can_fd_tdc_parameter;
|
||
can_filter_parameter_struct can_filter;
|
||
|
||
/* initialize CAN register */
|
||
can_deinit(CAN0);
|
||
|
||
/* initialize CAN */
|
||
can_parameter.time_triggered = DISABLE;
|
||
can_parameter.auto_bus_off_recovery = ENABLE;
|
||
can_parameter.auto_wake_up = DISABLE;
|
||
can_parameter.auto_retrans = ENABLE;
|
||
can_parameter.rec_fifo_overwrite = ENABLE;
|
||
can_parameter.trans_fifo_order = ENABLE;
|
||
can_parameter.working_mode = CAN_NORMAL_MODE;
|
||
/* baudrate 1Mbps, sample piont at 80% */
|
||
can_parameter.resync_jump_width = CAN_BT_SJW_1TQ;
|
||
can_parameter.time_segment_1 = CAN_BT_BS1_7TQ;
|
||
can_parameter.time_segment_2 = CAN_BT_BS2_2TQ;
|
||
can_parameter.prescaler = 6;
|
||
can_init(CAN0, &can_parameter);
|
||
|
||
can_struct_para_init(CAN_FD_FRAME_STRUCT, &can_fd_parameter);
|
||
can_fd_parameter.fd_frame = ENABLE;
|
||
can_fd_parameter.excp_event_detect = ENABLE;
|
||
can_fd_parameter.delay_compensation = ENABLE;
|
||
can_fd_tdc_parameter.tdc_filter = 0x04;
|
||
can_fd_tdc_parameter.tdc_mode = CAN_TDCMOD_CALC_AND_OFFSET;
|
||
can_fd_tdc_parameter.tdc_offset = 0x04;
|
||
can_fd_parameter.p_delay_compensation = &can_fd_tdc_parameter;
|
||
can_fd_parameter.iso_bosch = CAN_FDMOD_ISO;
|
||
can_fd_parameter.esi_mode = CAN_ESIMOD_HARDWARE;
|
||
/* FD mode data baudrate 2Mbps, sample piont at 80% */
|
||
can_fd_parameter.data_resync_jump_width = CAN_BT_SJW_1TQ;
|
||
can_fd_parameter.data_time_segment_1 = CAN_BT_BS1_3TQ;
|
||
can_fd_parameter.data_time_segment_2 = CAN_BT_BS2_1TQ;
|
||
/* CAN-FD data segement prescaler should be the same as non-data segement
|
||
* prescaler */
|
||
can_fd_parameter.data_prescaler = 6;
|
||
can_fd_init(CAN0, &can_fd_parameter);
|
||
|
||
/* initialize filter */
|
||
can_filter.filter_mode = CAN_FILTERMODE_MASK;
|
||
can_filter.filter_bits = CAN_FILTERBITS_32BIT;
|
||
/* configure filter ID */
|
||
can_filter.filter_list_high = 0x0000;
|
||
can_filter.filter_list_low = 0x0000;
|
||
/* configure filter mask */
|
||
can_filter.filter_mask_high = 0x0000;
|
||
can_filter.filter_mask_low = 0x0000;
|
||
/* select receiver fifo */
|
||
can_filter.filter_fifo_number = CAN_FIFO0;
|
||
can_filter.filter_enable = ENABLE;
|
||
/* configure filter number */
|
||
can_filter.filter_number = 0;
|
||
can_filter_init(&can_filter);
|
||
|
||
/* configure CAN0 NVIC */
|
||
nvic_irq_enable(CAN0_RX0_IRQn, 1, 0);
|
||
nvic_irq_enable(CAN0_TX_IRQn, 1, 1);
|
||
/* enable can receive FIFO0 not empty interrupt */
|
||
can_interrupt_enable(CAN0, CAN_INTEN_RFNEIE0);
|
||
// 使能 DMA 中断
|
||
nvic_irq_enable(DMA0_Channel4_IRQn, 1, 1);
|
||
dma_interrupt_enable(DMA0, DMA_CH4, DMA_INT_FTF | DMA_INT_ERR);
|
||
}
|
||
|
||
|
||
/*!
|
||
\brief CAN_transmiter 构造函数
|
||
\note 初始化 CAN 通信相关参数和硬件配置
|
||
*/
|
||
u8 CAN_transmiter::init() {
|
||
// 初始化 CAN 硬件
|
||
canfd_config();
|
||
|
||
// 初始化发送消息结构体
|
||
can_struct_para_init(CAN_TX_MESSAGE_STRUCT, &transmit_message);
|
||
/* initialize receive message */
|
||
can_struct_para_init(CAN_RX_MESSAGE_STRUCT, &receive_message);
|
||
|
||
// 初始化收发队列和标志位
|
||
rx_queue_head = 0;
|
||
rx_queue_tail = 0;
|
||
rx_queue_count = 0;
|
||
tx_queue_head = 0;
|
||
tx_queue_tail = 0;
|
||
tx_queue_count = 0;
|
||
can0_send_flag = RESET;
|
||
|
||
// 初始化 DMA 缓冲区
|
||
memset(can_rx_buffer, 0, sizeof(can_rx_buffer));
|
||
memset(can_rx_dma_words, 0, sizeof(can_rx_dma_words));
|
||
can_rx_dma_busy = false;
|
||
|
||
// 初始化节点 ID 为默认值
|
||
node_id = DEFAULT_NODE_ID;
|
||
|
||
// 从 Flash 加载节点 ID (如果 Flash 中有有效值则覆盖默认值)
|
||
load_node_id_from_flash();
|
||
return 0;
|
||
}
|
||
|
||
/**
|
||
* @brief 进入 CAN 临界区并保存原 PRIMASK 状态
|
||
* @return uint32_t 进入临界区前的 PRIMASK，用于退出时恢复原中断状态
|
||
* @note 退出时使用 CAN_ExitCritical() 调用 __set_PRIMASK() 恢复，
|
||
* 因此这里不会直接与 __enable_irq() 成对出现。
|
||
*/
|
||
static uint32_t CAN_EnterCritical(void) {
|
||
uint32_t primask = __get_PRIMASK();
|
||
__disable_irq();
|
||
return primask;
|
||
}
|
||
|
||
/**
|
||
* @brief 退出 CAN 临界区并恢复进入前的中断屏蔽状态
|
||
* @param[in] primask CAN_EnterCritical() 返回的原 PRIMASK 状态
|
||
* @return void
|
||
* @note 不直接调用 __enable_irq()，避免误开启外层已经关闭的中断。
|
||
*/
|
||
static void CAN_ExitCritical(uint32_t primask) { __set_PRIMASK(primask); }
|
||
|
||
/**
|
||
* @brief CANFD_DlcToLength 函数
|
||
* @param[in] dlc 参数
|
||
* @return u8 返回值
|
||
*/
|
||
static u8 CANFD_DlcToLength(u8 dlc) {
|
||
return (dlc <= 8U) ? dlc
|
||
: ((dlc <= 15U) ? CANFD_DLC_LENGTH_TABLE[dlc - 9U] : 0U);
|
||
}
|
||
|
||
/**
|
||
* @brief CANFD_NormalizeLength 函数
|
||
* @param[in] length 参数
|
||
* @return u8 返回值
|
||
*/
|
||
static u8 CANFD_NormalizeLength(u8 length) {
|
||
if (length <= 8U)
|
||
return length;
|
||
for (u8 i = 0; i < sizeof(CANFD_DLC_LENGTH_TABLE); i++) {
|
||
if (length <= CANFD_DLC_LENGTH_TABLE[i]) {
|
||
return CANFD_DLC_LENGTH_TABLE[i];
|
||
}
|
||
}
|
||
return 0U;
|
||
}
|
||
|
||
/**
|
||
* @brief CANFD_WordCount 函数
|
||
* @param[in] length 参数
|
||
* @return u8 返回值
|
||
*/
|
||
static u8 CANFD_WordCount(u8 length) { return (length + 3U) / 4U; }
|
||
|
||
/**
|
||
* @brief CAN_RX_QueuePushFromIsr 函数
|
||
* @param[in] message 参数
|
||
* @return void
|
||
*/
|
||
static void CAN_RX_QueuePushFromIsr(const can_receive_message_struct &message) {
|
||
if (rx_queue_count < CAN_RX_QUEUE_SIZE) {
|
||
rx_queue[rx_queue_tail].id = message.rx_efid;
|
||
rx_queue[rx_queue_tail].length = message.rx_dlen;
|
||
if (rx_queue[rx_queue_tail].length > 64) {
|
||
rx_queue[rx_queue_tail].length = 64;
|
||
}
|
||
for (int i = 0; i < rx_queue[rx_queue_tail].length; i++) {
|
||
rx_queue[rx_queue_tail].data[i] = message.rx_data[i];
|
||
}
|
||
rx_queue_tail = (rx_queue_tail + 1) % CAN_RX_QUEUE_SIZE;
|
||
rx_queue_count++;
|
||
} else {
|
||
CAN_DEBUG_PRINT("RX queue full, message dropped\r\n");
|
||
}
|
||
}
|
||
|
||
static void CAN_RX_ReleaseFifo0(void) { CAN_RFIFO0(CAN0) |= CAN_RFIFO0_RFD0; }
|
||
|
||
/**
|
||
* @brief CAN_RX_Fifo0PendingCount 函数
|
||
* @return u8 返回值
|
||
*/
|
||
static u8 CAN_RX_Fifo0PendingCount(void) {
|
||
return can_receive_message_length_get(CAN0, CAN_FIFO0);
|
||
}
|
||
|
||
/**
|
||
* @brief CAN_TX_HasEmptyMailbox 函数
|
||
* @return bool 返回值
|
||
*/
|
||
static bool CAN_TX_HasEmptyMailbox(void) {
|
||
return (can_flag_get(CAN0, CAN_FLAG_TME0) != RESET) ||
|
||
(can_flag_get(CAN0, CAN_FLAG_TME1) != RESET) ||
|
||
(can_flag_get(CAN0, CAN_FLAG_TME2) != RESET);
|
||
}
|
||
|
||
/**
|
||
* @brief CAN_RX_ReadMetadata 函数
|
||
* @param[in] message 参数
|
||
* @return void
|
||
*/
|
||
static void CAN_RX_ReadMetadata(can_receive_message_struct *message) {
|
||
uint8_t dlc;
|
||
uint32_t rfifomi = CAN_RFIFOMI(CAN0, CAN_FIFO0);
|
||
uint32_t rfifomp = CAN_RFIFOMP(CAN0, CAN_FIFO0);
|
||
uint32_t canfd_en = CAN_FDCTL(CAN0) & CAN_FDCTL_FDEN;
|
||
|
||
can_struct_para_init(CAN_RX_MESSAGE_STRUCT, message);
|
||
message->rx_ff = (uint8_t)(CAN_RFIFOMI_FF & rfifomi);
|
||
if (CAN_FF_STANDARD == message->rx_ff) {
|
||
message->rx_sfid = (uint32_t)(GET_RFIFOMI_SFID(rfifomi));
|
||
} else {
|
||
message->rx_efid = (uint32_t)(GET_RFIFOMI_EFID(rfifomi));
|
||
}
|
||
message->rx_ft = (uint8_t)(CAN_RFIFOMI_FT & rfifomi);
|
||
message->rx_fi = (uint8_t)(GET_RFIFOMP_FI(rfifomp));
|
||
message->fd_flag = (uint8_t)((CAN_RFIFOMP_FDF & rfifomp) >> 7);
|
||
dlc = (uint8_t)(GET_RFIFOMP_DLENC(rfifomp));
|
||
if ((canfd_en != 0U) && (message->fd_flag != CAN_FDF_CLASSIC)) {
|
||
message->rx_dlen = CANFD_DlcToLength(dlc);
|
||
message->fd_brs = (uint8_t)((rfifomp & CAN_RFIFOMP_BRS) >> 5);
|
||
message->fd_esi = (uint8_t)((rfifomp & CAN_RFIFOMP_ESI) >> 4);
|
||
} else {
|
||
message->rx_dlen = (dlc <= 8U) ? dlc : 8U;
|
||
}
|
||
}
|
||
|
||
/**
|
||
* @brief CAN_RX_StartPayloadDma 函数
|
||
* @param[in] metadata 参数
|
||
* @return void
|
||
*/
|
||
static void CAN_RX_StartPayloadDma(const can_receive_message_struct &metadata) {
|
||
can_rx_dma_message = metadata;
|
||
if (can_rx_dma_message.rx_dlen > 64) {
|
||
can_rx_dma_message.rx_dlen = 64;
|
||
}
|
||
|
||
u8 word_count = CANFD_WordCount(can_rx_dma_message.rx_dlen);
|
||
if (word_count == 0) {
|
||
CAN_RX_QueuePushFromIsr(can_rx_dma_message);
|
||
CAN_RX_ReleaseFifo0();
|
||
return;
|
||
}
|
||
|
||
can_rx_dma_busy = true;
|
||
dma_channel_disable(DMA0, DMA_CH4);
|
||
dma_interrupt_flag_clear(DMA0, DMA_CH4, DMA_INT_FLAG_G);
|
||
dma_periph_address_config(DMA0, DMA_CH4,
|
||
(uint32_t)(&CAN_RFIFOMDATA0(CAN0, CAN_FIFO0)));
|
||
dma_memory_address_config(DMA0, DMA_CH4, (uint32_t)can_rx_dma_words);
|
||
dma_transfer_number_config(DMA0, DMA_CH4, word_count);
|
||
dma_channel_enable(DMA0, DMA_CH4);
|
||
}
|
||
|
||
/**
|
||
* @brief CAN_RX_PumpFifo0 函数
|
||
* @return void
|
||
*/
|
||
static void CAN_RX_PumpFifo0(void) {
|
||
while (!can_rx_dma_busy) {
|
||
if (CAN_RX_Fifo0PendingCount() == 0U) {
|
||
can_interrupt_enable(CAN0, CAN_INTEN_RFNEIE0);
|
||
return;
|
||
}
|
||
|
||
can_receive_message_struct temp_message;
|
||
CAN_RX_ReadMetadata(&temp_message);
|
||
|
||
if (temp_message.rx_ff == CAN_FF_EXTENDED) {
|
||
CAN_RX_StartPayloadDma(temp_message);
|
||
} else {
|
||
CAN_RX_ReleaseFifo0();
|
||
CAN_DEBUG_PRINT("Drop standard CAN frame in CANFD path\r\n");
|
||
}
|
||
}
|
||
}
|
||
|
||
/**
|
||
* @brief CAN_RX_QueuePop 函数
|
||
* @param[in] item 参数
|
||
* @return u8 返回值
|
||
*/
|
||
static u8 CAN_RX_QueuePop(can_rx_queue_item_t *item) {
|
||
u8 popped = 0U;
|
||
uint32_t primask = CAN_EnterCritical();
|
||
if (rx_queue_count == 0) {
|
||
CAN_ExitCritical(primask);
|
||
return 0U;
|
||
}
|
||
*item = rx_queue[rx_queue_head];
|
||
rx_queue_head = (rx_queue_head + 1) % CAN_RX_QUEUE_SIZE;
|
||
rx_queue_count--;
|
||
popped = 1U;
|
||
CAN_ExitCritical(primask);
|
||
|
||
return popped;
|
||
}
|
||
|
||
/**
|
||
* @brief CAN_TX_QueuePush 函数
|
||
* @param[in] message 参数
|
||
* @return u8 返回值
|
||
*/
|
||
static u8 CAN_TX_QueuePush(const can_trasnmit_message_struct &message) {
|
||
u8 queued = 0U;
|
||
uint32_t primask = CAN_EnterCritical();
|
||
if (tx_queue_count < CAN_TX_QUEUE_SIZE) {
|
||
tx_queue[tx_queue_tail].message = message;
|
||
tx_queue[tx_queue_tail].valid = 1U;
|
||
tx_queue_tail = (tx_queue_tail + 1U) % CAN_TX_QUEUE_SIZE;
|
||
tx_queue_count++;
|
||
queued = 1U;
|
||
}
|
||
CAN_ExitCritical(primask);
|
||
|
||
if (queued != 0U) {
|
||
can_interrupt_enable(CAN0, CAN_INTEN_TMEIE);
|
||
return 1U;
|
||
}
|
||
|
||
CAN_DEBUG_PRINT("TX queue full, message dropped\r\n");
|
||
return 0U;
|
||
}
|
||
|
||
/**
|
||
* @brief CAN_TX_QueuePeek 函数
|
||
* @param[in] message 参数
|
||
* @return u8 返回值
|
||
*/
|
||
static u8 CAN_TX_QueuePeek(can_trasnmit_message_struct *message) {
|
||
u8 has_message = 0U;
|
||
uint32_t primask = CAN_EnterCritical();
|
||
if (tx_queue_count != 0U) {
|
||
*message = tx_queue[tx_queue_head].message;
|
||
has_message = 1U;
|
||
}
|
||
CAN_ExitCritical(primask);
|
||
return has_message;
|
||
}
|
||
|
||
/**
|
||
* @brief CAN_TX_QueuePop 函数
|
||
* @param[in] message 参数
|
||
* @return u8 返回值
|
||
*/
|
||
static u8 CAN_TX_QueuePop(can_trasnmit_message_struct *message) {
|
||
u8 popped = 0U;
|
||
uint32_t primask = CAN_EnterCritical();
|
||
if (tx_queue_count != 0U) {
|
||
*message = tx_queue[tx_queue_head].message;
|
||
tx_queue[tx_queue_head].valid = 0U;
|
||
tx_queue_head = (tx_queue_head + 1U) % CAN_TX_QUEUE_SIZE;
|
||
tx_queue_count--;
|
||
popped = 1U;
|
||
}
|
||
CAN_ExitCritical(primask);
|
||
return popped;
|
||
}
|
||
|
||
/**
|
||
* @brief CAN_TX_DisableTmeIfQueueEmpty 函数
|
||
* @return void
|
||
*/
|
||
static void CAN_TX_DisableTmeIfQueueEmpty(void) {
|
||
uint32_t primask = CAN_EnterCritical();
|
||
if (tx_queue_count == 0U) {
|
||
can_interrupt_disable(CAN0, CAN_INTEN_TMEIE);
|
||
}
|
||
CAN_ExitCritical(primask);
|
||
}
|
||
|
||
/**
|
||
* @brief CAN_TX_ServiceQueue 函数
|
||
* @return void
|
||
*/
|
||
static void CAN_TX_ServiceQueue(void) {
|
||
can_trasnmit_message_struct queued_message;
|
||
while (CAN_TX_HasEmptyMailbox()) {
|
||
if (CAN_TX_QueuePeek(&queued_message) == 0U) {
|
||
CAN_TX_DisableTmeIfQueueEmpty();
|
||
return;
|
||
}
|
||
if (can_message_transmit(CAN0, &queued_message) == CAN_NOMAILBOX) {
|
||
CAN_DEBUG_PRINT(
|
||
"Mailbox became unavailable while draining TX queue\r\n");
|
||
can_interrupt_enable(CAN0, CAN_INTEN_TMEIE);
|
||
return;
|
||
}
|
||
CAN_TX_QueuePop(&queued_message);
|
||
}
|
||
}
|
||
|
||
/*!
|
||
\brief CAN 数据更新
|
||
*/
|
||
void CAN_transmiter::update(void) {
|
||
target_angle_union.f = FOC.target_angle;
|
||
target_velocity_union.f = FOC.target_velocity;
|
||
target_current_union.f = FOC.target_current;
|
||
|
||
rt_angle_union.f = FOC.rt_angle;
|
||
rt_velocity_union.f = FOC.rt_velocity;
|
||
rt_current_union.f = FOC.rt_current;
|
||
|
||
limit_voltage = FOC.limit_voltage * 1000;
|
||
mcu_temp = sampler.get_mcu_temp() * 100;
|
||
motor_temp = sampler.get_drv_temp() * 100;
|
||
Power_Voltage = sampler.get_vbus_voltage() * 1000;
|
||
MotorStatus = gpio_input_bit_get(GPIOB, GPIO_PIN_2) == RESET;
|
||
EEPROMStatus = flashEEP.EEPROMStatus;
|
||
Target_Control_Mode = FOC.control_mode;
|
||
controller_Mode = FOC.control_mode;
|
||
cpu_usage = get_cpu_usage();
|
||
|
||
// 使用简单平均滤波的电流值
|
||
u_current_union.f = sampler.get_current('U') * 1000;
|
||
w_current_union.f = sampler.get_current('W') * 1000;
|
||
v_current_union.f = sampler.get_current('V') * 1000;
|
||
}
|
||
|
||
/**
|
||
* @brief CANFD 发送函数
|
||
* @retval 0: 发送失败（或处于静默模式）
|
||
* 1: 发送成功
|
||
*/
|
||
u8 CAN_transmiter::CANFD_all_Send() {
|
||
// 检查是否在静默模式中，如果是则不发送
|
||
if (is_in_silent_mode()) {
|
||
check_silent_mode(); // 检查是否已退出静默
|
||
return 0;
|
||
}
|
||
|
||
u32 success_flag = 0;
|
||
|
||
// 打包 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);
|
||
pack_current_s16_to_bytes(transmit_data, 4, u_current_union.f);
|
||
pack_current_s16_to_bytes(transmit_data, 6, v_current_union.f);
|
||
pack_current_s16_to_bytes(transmit_data, 8, w_current_union.f);
|
||
transmit_data[10] =
|
||
(transmit_data[10] & ~0x07) |
|
||
((MotorStatus & 0x01) | ((MagStatus & 0x01) << 1) |
|
||
((ThermalStatus & 0x01) << 2) | ((EEPROMStatus & 0x07) << 3));
|
||
pack_u16_to_bytes(transmit_data, 11, Power_Voltage);
|
||
pack_float_to_bytes(transmit_data, 13, rt_angle_union);
|
||
pack_float_to_bytes(transmit_data, 17, rt_velocity_union);
|
||
pack_float_to_bytes(transmit_data, 21, rt_current_union);
|
||
pack_float_to_bytes(transmit_data, 25, testfloat_union);
|
||
transmit_data[29] = test_u16 & 0xFF;
|
||
transmit_data[30] = (test_u16 >> 8) & 0xFF;
|
||
transmit_data[31] = cpu_usage;
|
||
// 使用节点 ID 构建发送 ID (高 16 位为节点 ID)
|
||
success_flag +=
|
||
CANFD_AsyncSend(build_can_id(CANFD_STATUS_ID), 32, transmit_data);
|
||
|
||
// 打包 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);
|
||
// 使用节点 ID 构建发送 ID (高 16 位为节点 ID)
|
||
success_flag +=
|
||
CANFD_AsyncSend(build_can_id(CANFD_CONFIG_ID), 24, transmit_data);
|
||
|
||
return success_flag;
|
||
}
|
||
|
||
/**
|
||
* @brief CAN 通信检查函数
|
||
* @retval 0: 无新消息
|
||
* 1: 有新消息
|
||
*/
|
||
void CAN_transmiter::communication_check() {
|
||
// 先处理周期发送，再抽空接收队列，保持原来的调度顺序。
|
||
handle_periodic_send_tick();
|
||
|
||
can_rx_queue_item_t rx_item;
|
||
while (CAN_RX_QueuePop(&rx_item)) {
|
||
handle_received_queue_item(rx_item.id, rx_item.length, rx_item.data);
|
||
}
|
||
}
|
||
|
||
void CAN_transmiter::handle_periodic_send_tick() {
|
||
// 无发送请求时直接返回，避免每周期进入发送逻辑。
|
||
if (SET != can0_send_flag) return;
|
||
|
||
if (!is_in_silent_mode()) {
|
||
update();
|
||
CANFD_all_Send();
|
||
} else {
|
||
check_silent_mode();
|
||
}
|
||
can0_send_flag = RESET;
|
||
}
|
||
|
||
void CAN_transmiter::handle_received_queue_item(u32 id, u8 length, const u8 *data) {
|
||
// 将轻量队列项还原为原 CAN 消息结构，后续沿用原解析逻辑。
|
||
can_receive_message_struct current_message;
|
||
can_struct_para_init(CAN_RX_MESSAGE_STRUCT, ¤t_message);
|
||
current_message.rx_efid = id;
|
||
current_message.rx_dlen = length;
|
||
memcpy(current_message.rx_data, data, length);
|
||
|
||
u32 base_id = current_message.rx_efid & 0xFFFF;
|
||
u16 sender_node_id = (u16)((current_message.rx_efid >> 16) & 0xFFFF);
|
||
|
||
// BOOT 和 SET_NODE_ID 需要在节点过滤前处理。
|
||
if (handle_special_message(current_message, base_id, sender_node_id)) return;
|
||
if (!is_message_for_me(current_message.rx_efid)) return;
|
||
|
||
handle_node_message(current_message, base_id);
|
||
}
|
||
|
||
bool CAN_transmiter::handle_special_message(can_receive_message_struct ¤t_message,
|
||
u32 base_id, u16 sender_node_id) {
|
||
if (base_id == CANFD_BOOT_ID) {
|
||
handle_boot_message(sender_node_id);
|
||
return true;
|
||
}
|
||
if (base_id == CANFD_SET_NODE_ID) {
|
||
handle_set_node_id_message(current_message);
|
||
return true;
|
||
}
|
||
return false;
|
||
}
|
||
|
||
void CAN_transmiter::handle_boot_message(u16 sender_node_id) {
|
||
// 其他节点或广播 BOOT 命令只让本节点静默，避免抢占总线。
|
||
if (sender_node_id == BROADCAST_NODE_ID || sender_node_id != node_id) {
|
||
CAN_DEBUG_PRINT(
|
||
"Received base_id=0x0000 from node %d, entering silent mode for 10s\r\n",
|
||
sender_node_id);
|
||
enter_silent_mode();
|
||
return;
|
||
}
|
||
|
||
CAN_DEBUG_PRINT("Received base_id=0x0000 from self, jumping to BOOT\r\n");
|
||
motor_sleep();
|
||
flashEEP.save_system_config(false);
|
||
fmc_unlock();
|
||
OTA_SET(n_APP_LATEST_EN_ADDR, OTA_NOT_AVAL);
|
||
fmc_lock();
|
||
jumpToBOOT();
|
||
}
|
||
|
||
void CAN_transmiter::handle_set_node_id_message(can_receive_message_struct ¤t_message) {
|
||
// ── 防护：只处理发给本节点（或广播）的 SET_NODE_ID ──
|
||
u16 target_node = (u16)((current_message.rx_efid >> 16) & 0xFFFF);
|
||
if (target_node != node_id && target_node != BROADCAST_NODE_ID) {
|
||
return;
|
||
}
|
||
|
||
// 支持 1 字节和 2 字节节点 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]);
|
||
|
||
u16 new_node_id = (current_message.rx_dlen >= 2) ?
|
||
unpack_bytes_to_u16(current_message.rx_data, 0) : current_message.rx_data[0];
|
||
u16 old_node_id = node_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);
|
||
|
||
transmit_data[1] = (u8)(new_node_id & 0xFF);
|
||
transmit_data[2] = (u8)((new_node_id >> 8) & 0xFF);
|
||
// 响应帧使用旧节点 ID 构造，确保上位机能收到确认。
|
||
node_id = old_node_id;
|
||
|
||
if (new_node_id < 1 || new_node_id > SETTABLE_NODE_ID_MAX) {
|
||
CAN_DEBUG_PRINT("SET_NODE_ID: invalid new_id=%d (range 1~%d)\r\n",
|
||
new_node_id, SETTABLE_NODE_ID_MAX);
|
||
transmit_data[0] = 0x00;
|
||
CANFD_AsyncSend(build_can_id(CANFD_SET_NODE_ID), 3, transmit_data);
|
||
return;
|
||
}
|
||
|
||
// 先写 Flash，成功后再回确认，避免 Flash 失败时上位机已收到 OK。
|
||
u8 save_status = save_node_id_to_flash(new_node_id);
|
||
|
||
CAN_DEBUG_PRINT("SET_NODE_ID: save_status=%d\r\n", save_status);
|
||
|
||
if (save_status == FLASH_EEPROM_OK) {
|
||
transmit_data[0] = 0x01;
|
||
CANFD_AsyncSend(build_can_id(CANFD_SET_NODE_ID), 3, transmit_data);
|
||
CAN_DEBUG_PRINT("SET_NODE_ID: SUCCESS, node_id updated from %d to %d\r\n",
|
||
old_node_id, new_node_id);
|
||
return;
|
||
}
|
||
|
||
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);
|
||
}
|
||
|
||
void CAN_transmiter::handle_node_message(can_receive_message_struct ¤t_message,
|
||
u32 base_id) {
|
||
// 这里只处理已经确认发给本节点的普通协议消息。
|
||
if (base_id == CANFD_CMD_ID) {
|
||
handle_control_command(current_message);
|
||
} else if (base_id == CANFD_RESET_ID) {
|
||
handle_reset_command(current_message);
|
||
} else if (base_id == CANFD_VERSION_ID) {
|
||
Version.sendVersionInfoViaCan();
|
||
} else if (base_id == CANFD_OTA_ACK_ID) {
|
||
handle_ota_ack_message();
|
||
} 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 {
|
||
CAN_DEBUG_PRINT("Unknown CAN ID: %X (node_id=%d)\n",
|
||
current_message.rx_efid, node_id);
|
||
}
|
||
}
|
||
|
||
void CAN_transmiter::handle_control_command(can_receive_message_struct ¤t_message) {
|
||
// 控制帧布局：模式、电压限制、目标电流/速度/角度、三组 P 参数。
|
||
FOC.control_mode = current_message.rx_data[0];
|
||
FOC.limit_voltage = float(unpack_bytes_to_u16(current_message.rx_data, 1)) / 1000;
|
||
target_current_union.f = unpack_bytes_to_float(current_message.rx_data, 3).f;
|
||
FOC.current_loop.set_limit(target_current_union.f);
|
||
FOC.vel_inMITloop.set_limit(target_current_union.f);
|
||
target_velocity_union.f = unpack_bytes_to_float(current_message.rx_data, 7).f;
|
||
FOC.ang_inVelAngloop.set_limit(target_velocity_union.f);
|
||
FOC.ang_inMITloop.set_limit(target_velocity_union.f);
|
||
target_angle_union.f = unpack_bytes_to_float(current_message.rx_data, 11).f;
|
||
FOC.target_angle = target_angle_union.f;
|
||
FOC.target_velocity = target_velocity_union.f;
|
||
FOC.target_current = target_current_union.f;
|
||
|
||
// limit_voltage=0 时 P 换算除零会得到 inf 增益，钳到下限
|
||
float voltage = FOC.limit_voltage;
|
||
if (voltage < MIN_LIMIT_VOLTAGE) {
|
||
voltage = MIN_LIMIT_VOLTAGE;
|
||
}
|
||
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;
|
||
|
||
P_config = unpack_bytes_to_u16(current_message.rx_data, 17);
|
||
FOC.vel_loop_p = DEFAULT_vel_loop_p * P_config / voltage;
|
||
FOC.vel_angloop_p = DEFAULT_vel_angloop_p * P_config;
|
||
FOC.vel_MIT_p = DEFAULT_vel_MIT_p * P_config;
|
||
|
||
P_config = unpack_bytes_to_u16(current_message.rx_data, 19);
|
||
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;
|
||
}
|
||
|
||
void CAN_transmiter::handle_reset_command(can_receive_message_struct ¤t_message) {
|
||
// RESET 帧按 bit 位触发独立动作，保持与原协议兼容。
|
||
u8 first_byte = current_message.rx_data[0];
|
||
u8 eigth_byte = current_message.rx_data[7];
|
||
|
||
if (first_byte & 0x01) {
|
||
motor_reset();
|
||
testfloat_union.f = KTH7823.getcurrent_sensorangle();
|
||
}
|
||
if (first_byte >> 1 & 0x01) {
|
||
// flashEEP.format();
|
||
}
|
||
if (first_byte >> 2 & 0x01) {
|
||
flashEEP.save_system_config(false);
|
||
}
|
||
if (first_byte >> 3 & 0x01) {
|
||
KTH7823.angle_set(0);
|
||
Angle_kalman.init(0);
|
||
Vel_kalman.init(0);
|
||
}
|
||
if (first_byte >> 5 & 0x01) {
|
||
// 零位校准：施加对齐电压、读传感器角、计算并保存校准角
|
||
calibrate_zero_position();
|
||
}
|
||
#if FOC_DEBUG
|
||
if (first_byte >> 4 & 0x01) {
|
||
FOC.cogging_calib_request = true;
|
||
}
|
||
#endif
|
||
if (eigth_byte & 0x80 >> 7) {
|
||
app_prepare_deep_sleep(true);
|
||
}
|
||
}
|
||
|
||
void CAN_transmiter::handle_ota_ack_message() {
|
||
// 只有当前 APP 标记为可用且校验通过时才回 ACK。
|
||
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;
|
||
u32 app_check = *flash_ptr;
|
||
transmit_data[0] = 0x0A;
|
||
transmit_data[1] = 0xAC;
|
||
if (app_enable == OTA_AVAL && app_check == OTA_AVAL) {
|
||
CANFD_AsyncSend(build_can_id(CANFD_OTA_ACK_ID), 2, transmit_data);
|
||
}
|
||
}
|
||
|
||
/**
|
||
* @brief CAN 睡眠
|
||
* @param None
|
||
* @retval None
|
||
*/
|
||
u8 CAN_transmiter::CAN_Sleep() {
|
||
gpio_bit_set(GPIOB, GPIO_PIN_13);
|
||
is_sleep = true;
|
||
return 0;
|
||
}
|
||
|
||
/**
|
||
* @brief CAN 唤醒
|
||
* @param None
|
||
* @retval None
|
||
*/
|
||
u8 CAN_transmiter::CAN_Wakeup() {
|
||
CAN_DEBUG_PRINT("can wake\r\n");
|
||
is_sleep = false;
|
||
canfd_config();
|
||
|
||
return 0;
|
||
}
|
||
|
||
/**
|
||
* @brief CAN0 中断服务程序（优化版：直接读取数据，减少DMA竞态条件）
|
||
* @retval 无
|
||
*/
|
||
void CAN0_RX0_IRQHandler(void) {
|
||
// FIFO0 非空时，关中断并持续抽空 FIFO；DMA 完成后由 DMA ISR 继续续传。
|
||
if (CAN_RX_Fifo0PendingCount() != 0U) {
|
||
can_interrupt_disable(CAN0, CAN_INTEN_RFNEIE0);
|
||
CAN_RX_PumpFifo0();
|
||
}
|
||
|
||
// 检查其他可能的中断源
|
||
if (can_interrupt_flag_get(CAN0, CAN_INT_FLAG_RFF0) != RESET) {
|
||
// FIFO0 满中断
|
||
can_interrupt_flag_clear(CAN0, CAN_INT_FLAG_RFF0);
|
||
CAN_DEBUG_PRINT("CAN FIFO0 full\n");
|
||
}
|
||
|
||
if (can_interrupt_flag_get(CAN0, CAN_INT_FLAG_RFO0) != RESET) {
|
||
// FIFO0 溢出中断
|
||
can_interrupt_flag_clear(CAN0, CAN_INT_FLAG_RFO0);
|
||
CAN_DEBUG_PRINT("CAN FIFO0 overflow\n");
|
||
}
|
||
}
|
||
|
||
/**
|
||
* @brief DMA 通道 4 中断服务程序 (CAN 接收 DMA)
|
||
* @retval 无
|
||
*/
|
||
void DMA0_Channel4_IRQHandler(void) {
|
||
if (dma_interrupt_flag_get(DMA0, DMA_CH4, DMA_INT_FLAG_ERR)) {
|
||
dma_interrupt_flag_clear(DMA0, DMA_CH4, DMA_INT_FLAG_G);
|
||
dma_channel_disable(DMA0, DMA_CH4);
|
||
if (can_rx_dma_busy) {
|
||
CAN_RX_ReleaseFifo0();
|
||
can_rx_dma_busy = false;
|
||
}
|
||
CAN_DEBUG_PRINT("CAN RX DMA error, frame dropped\r\n");
|
||
CAN_RX_PumpFifo0();
|
||
return;
|
||
}
|
||
|
||
if (dma_interrupt_flag_get(DMA0, DMA_CH4, DMA_INT_FLAG_FTF)) {
|
||
// 清除 DMA 传输完成中断标志
|
||
dma_interrupt_flag_clear(DMA0, DMA_CH4, DMA_INT_FLAG_G);
|
||
dma_channel_disable(DMA0, DMA_CH4);
|
||
if (!can_rx_dma_busy) {
|
||
CAN_RX_PumpFifo0();
|
||
return;
|
||
}
|
||
memcpy(can_rx_dma_message.rx_data, can_rx_dma_words,
|
||
can_rx_dma_message.rx_dlen);
|
||
CAN_RX_QueuePushFromIsr(can_rx_dma_message);
|
||
CAN_RX_ReleaseFifo0();
|
||
can_rx_dma_busy = false;
|
||
CAN_RX_PumpFifo0();
|
||
}
|
||
}
|
||
|
||
/**
|
||
* @brief DMA 通道 5 中断服务程序 (CAN/CANFD 发送 DMA)
|
||
* @retval 无
|
||
*/
|
||
void DMA0_Channel5_IRQHandler(void) {
|
||
// 保留空实现：发送链路改用 CAN mailbox + TME 中断队列。
|
||
}
|
||
|
||
/**
|
||
* @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 规范
|
||
if (length > 8) {
|
||
CAN_DEBUG_PRINT("CAN data length exceeds maximum of 8 bytes\r\n");
|
||
return 0;
|
||
}
|
||
|
||
// 检查数据指针是否有效
|
||
if (data == nullptr) {
|
||
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_dlen = length; // 设置数据长度
|
||
|
||
// 复制数据到发送缓冲区
|
||
for (int i = 0; i < length && i < 8; i++) {
|
||
transmit_message.tx_data[i] = *(data + i);
|
||
}
|
||
|
||
// 自动选择空闲 mailbox 发送消息
|
||
transmit_mailbox = can_message_transmit(CAN0, &transmit_message);
|
||
|
||
// 检查是否成功获取 mailbox
|
||
if (transmit_mailbox == CAN_NOMAILBOX) {
|
||
// 没有可用的 mailbox，启用发送中断以便在邮箱可用时重试
|
||
CAN_DEBUG_PRINT(
|
||
"No available mailbox for CAN transmission, enabling TX "
|
||
"interrupt\r\n");
|
||
// 可以设置一个标志位，当邮箱可用时发送
|
||
// 在实际应用中，可以维护一个待发送队列
|
||
return 0;
|
||
}
|
||
|
||
// 启用 CAN 发送中断以检测传输完成
|
||
can_interrupt_enable(CAN0, CAN_INTEN_TMEIE);
|
||
return 1;
|
||
}
|
||
|
||
/**
|
||
* @brief 异步 CANFD 数据发送 (使用队列机制防止丢帧)
|
||
* @param id CAN 消息 ID
|
||
* @param length CAN 消息长度
|
||
* @param data CAN 消息数据
|
||
*/
|
||
u8 CAN_transmiter::CANFD_AsyncSend(u32 id, u8 length, u8 *data) {
|
||
u8 valid_length = 0U;
|
||
// 检查 CANFD 模式是否启用
|
||
if ((CAN_FDCTL(CAN0) & CAN_FDCTL_FDEN) == 0U) {
|
||
CAN_DEBUG_PRINT("CANFD mode not enabled\r\n");
|
||
return 0;
|
||
}
|
||
|
||
// 检查数据指针是否有效
|
||
if (data == nullptr && length != 0U) {
|
||
CAN_DEBUG_PRINT("Invalid data pointer\r\n");
|
||
return 0;
|
||
}
|
||
|
||
// 检查数据长度是否符合 CANFD 规范，并映射到合法 DLC 长度
|
||
if (length == 0U) {
|
||
valid_length = 0U;
|
||
} else {
|
||
valid_length = CANFD_NormalizeLength(length);
|
||
}
|
||
if ((length != 0U) && (valid_length == 0U)) {
|
||
CAN_DEBUG_PRINT("CANFD data length error\r\n");
|
||
return 0;
|
||
}
|
||
|
||
/* 配置发送消息参数 */
|
||
can_struct_para_init(CAN_TX_MESSAGE_STRUCT, &transmit_message);
|
||
transmit_message.tx_ft = CAN_FT_DATA; // 数据帧
|
||
transmit_message.tx_ff = CAN_FF_EXTENDED; // 扩展帧格式
|
||
transmit_message.tx_efid = id; // 扩展帧 ID
|
||
transmit_message.tx_dlen = valid_length; // 设置数据长度
|
||
|
||
// 设置 CANFD 特定参数
|
||
transmit_message.fd_flag = CAN_FDF_FDFRAME; // CANFD 帧格式
|
||
transmit_message.fd_brs = ENABLE; // 使能比特率切换
|
||
transmit_message.fd_esi = DISABLE; // 错误状态指示
|
||
memset(transmit_message.tx_data, 0, sizeof(transmit_message.tx_data));
|
||
for (int i = 0; i < valid_length; i++) {
|
||
if (i < length) {
|
||
transmit_message.tx_data[i] = *(data + i);
|
||
}
|
||
}
|
||
|
||
// 自动选择空闲 mailbox 发送消息
|
||
transmit_mailbox = can_message_transmit(CAN0, &transmit_message);
|
||
|
||
// 检查是否成功获取 mailbox
|
||
if (transmit_mailbox == CAN_NOMAILBOX) {
|
||
// 没有可用的 mailbox，将消息加入发送队列
|
||
CAN_DEBUG_PRINT("No available mailbox, adding to TX queue\r\n");
|
||
return CAN_TX_QueuePush(transmit_message);
|
||
}
|
||
|
||
// 启用 CAN 发送中断以检测传输完成（用于队列发送）
|
||
can_interrupt_enable(CAN0, CAN_INTEN_TMEIE);
|
||
return 1;
|
||
}
|
||
|
||
/**
|
||
* @brief CAN 传输中断处理函数（支持队列发送）
|
||
*/
|
||
void CAN0_TX_IRQHandler(void) {
|
||
if (CAN_TX_HasEmptyMailbox()) {
|
||
CAN_TX_ServiceQueue();
|
||
}
|
||
}
|