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

refactor(FOC): 重构齿槽转矩校准状态机为独立函数

Browse Source 
- 将原本在 calibrate_cogging_tick 中的大函数拆分为多个职责单一的函数
- 新增 start_cogging_calibration、cogging_settling_tick、cogging_sampling_tick、
  cogging_stopping_tick、cogging_done_tick 等状态处理函数
- 添加 process_cogging_run_data、start_next_cogging_run、build_cogging_tables、
  print_cogging_table、print_cogging_c_array 等辅助函数
- 移动全局 FOC 控制器单例注释到更合适位置
- 添加齿槽转矩校准参数常量定义和详细注释
- 添加防御性编程处理默认状态分支，避免状态机卡死
- 提高代码可读性和维护性

feat(tasks): 添加任务头文件到项目配置

- 在 Robot_V2.cproject.yml 中添加 tasks.h 和 tasks.cpp 文件引用

docs(ADC): 添加ADC转换常量说明注释

- 在 ADC.cpp 中添加关于ADC换算常量的详细注释
- 说明全局采样器单例的初始化和使用方式
This commit is contained in:
cbb
2026-05-24 01:21:41 +08:00
parent 4c4e47e9e3
commit e646544f7f
23 changed files with 1705 additions and 647 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Robot_V2/device/Can.cpp
+206 -209
View File
@@ -33,6 +33,7 @@
#define CAN_DEBUG_PRINT(fmt, ...) ((void)0)
#endif
// CAN0 固定外设号，编译期常量，非可变全局状态。
/** @brief CANX 常量定义 */
constexpr u16 CANX = CAN0;
extern FOC_Controller FOC;
@@ -44,7 +45,9 @@ extern FlashEEPROM flashEEP;
extern VersionManager Version; // 声明外部版本管理器实例
extern u32 motor_runtime_50us; // 电机驱动计算轮次（用于静默模式计时）
// 添加 DMA 相关变量
// 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字节
@@ -81,6 +84,8 @@ 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 初始化
// ============================================================================
@@ -701,8 +706,10 @@ u8 CAN_transmiter::init() {
}
/**
* @brief CAN_EnterCritical 函数
* @return uint32_t 返回值
* @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();
@@ -711,9 +718,10 @@ static uint32_t CAN_EnterCritical(void) {
}
/**
* @brief CAN_ExitCritical 函数
* @param[in] primask 参数
* @brief 退出 CAN 临界区并恢复进入前的中断屏蔽状态
* @param[in] primask CAN_EnterCritical() 返回的原 PRIMASK 状态
* @return void
* @note 不直接调用 __enable_irq()，避免误开启外层已经关闭的中断。
*/
static void CAN_ExitCritical(uint32_t primask) { __set_PRIMASK(primask); }
@@ -1074,228 +1082,217 @@ u8 CAN_transmiter::CANFD_all_Send() {
* 1: 有新消息
*/
void CAN_transmiter::communication_check() {
// CAN0 发送数据标志
if (SET == can0_send_flag) {
// 检查是否在静默模式中
if (!is_in_silent_mode()) {
update();
CANFD_all_Send();
} else {
check_silent_mode(); // 检查静默是否结束
}
can0_send_flag = RESET;
}
// 先处理周期发送，再抽空接收队列，保持原来的调度顺序。
handle_periodic_send_tick();
// 处理所有接收到的 CANFD 消息（从底层中断队列中取出）
can_rx_queue_item_t rx_item;
while (CAN_RX_QueuePop(&rx_item)) {
can_receive_message_struct current_message;
can_struct_para_init(CAN_RX_MESSAGE_STRUCT, &current_message);
current_message.rx_efid = rx_item.id;
current_message.rx_dlen = rx_item.length;
memcpy(current_message.rx_data, rx_item.data, rx_item.length);
handle_received_queue_item(rx_item.id, rx_item.length, rx_item.data);
}
}
// 提取基础 ID (低 16 位，去除高 16 位节点 ID 部分)
u32 base_id = current_message.rx_efid & 0xFFFF;
void CAN_transmiter::handle_periodic_send_tick() {
// 无发送请求时直接返回，避免每周期进入发送逻辑。
if (SET != can0_send_flag) return;
// 提取发送方的节点 ID
u16 sender_node_id = (u16)((current_message.rx_efid >> 16) & 0xFFFF);
if (!is_in_silent_mode()) {
update();
CANFD_all_Send();
} else {
check_silent_mode();
}
can0_send_flag = RESET;
}
// 特殊处理 base_id=0x0000（BOOT 命令）：即使是其他节点的广播消息也要处理
if (base_id == CANFD_BOOT_ID) {
// 判断是广播消息还是其他节点的命令
if (sender_node_id == BROADCAST_NODE_ID || sender_node_id != node_id) {
// 广播消息或其他节点的命令 → 进入静默模式（停发 10 秒）
CAN_DEBUG_PRINT(
"Received base_id=0x0000 from node %d, entering silent mode for 10s\r\n",
sender_node_id);
enter_silent_mode();
} else {
// 自己节点的命令 → 执行 BOOT 功能
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); // 设置 APP 为不激活
fmc_lock();
jumpToBOOT();
}
continue; // BOOT 命令处理完跳过后续逻辑
}
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, &current_message);
current_message.rx_efid = id;
current_message.rx_dlen = length;
memcpy(current_message.rx_data, data, length);
// 设置节点 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",
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 &current_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 &current_message) {
// 支持 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);
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 高字节
transmit_data[1] = (u8)(new_node_id & 0xFF);
transmit_data[2] = (u8)((new_node_id >> 8) & 0xFF);
// 响应帧使用旧节点 ID 构造，确保上位机能收到确认。
node_id = old_node_id;
// 使用旧节点 ID 发送响应
node_id = old_node_id;
u8 send_result = CANFD_AsyncSend(build_can_id(CANFD_SET_NODE_ID), 3,
transmit_data);
if (new_node_id > MAX_NODE_ID || new_node_id == BROADCAST_NODE_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;
CANFD_AsyncSend(build_can_id(CANFD_SET_NODE_ID), 3, transmit_data);
return;
}
// 然后保存新节点 ID 到 Flash
u8 save_status = save_node_id_to_flash(new_node_id);
transmit_data[0] = 0x01;
u8 send_result = CANFD_AsyncSend(build_can_id(CANFD_SET_NODE_ID), 3,
transmit_data);
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);
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 命令处理完跳过后续逻辑
}
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);
return;
}
// 其他 base_id 的消息，检查是否是发给本节点的
if (!is_message_for_me(current_message.rx_efid)) {
continue; // 不是发给本节点的消息，跳过
}
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);
}
if (base_id == CANFD_CMD_ID) {
// 控制模式
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;
void CAN_transmiter::handle_node_message(can_receive_message_struct &current_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);
}
}
// 解析 PID 参数
float voltage = FOC.limit_voltage;
// 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;
void CAN_transmiter::handle_control_command(can_receive_message_struct &current_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;
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;
float voltage = FOC.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;
} else if (base_id == CANFD_RESET_ID) {
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();
}
// EEPROM 重置
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); // 角度滤波
}
// 齿槽转矩校准 (first_byte bit4) — DEBUG，平时不启用
#if FOC_DEBUG
if (first_byte >> 4 & 0x01) {
FOC.cogging_calib_request = true;
}
#endif
// 休眠
if (eigth_byte & 0x80 >> 7) {
app_prepare_deep_sleep(true);
// CAN_Sleep();
}
} else if (base_id == CANFD_VERSION_ID) {
// 处理版本号请求，发送当前系统版本信息
Version.sendVersionInfoViaCan();
} 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;
u32 app_check = *flash_ptr;
transmit_data[0] = 0x0A;
transmit_data[1] = 0xAC;
delay_1ms(1);
if (app_enable == OTA_AVAL && app_check == OTA_AVAL) {
CANFD_AsyncSend(build_can_id(CANFD_OTA_ACK_ID), 2,
transmit_data);
}
}
// 查询节点 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 {
CAN_DEBUG_PRINT("Unknown CAN ID: %X (node_id=%d)\n",
current_message.rx_efid, node_id);
}
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 &current_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 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;
delay_1ms(1);
if (app_enable == OTA_AVAL && app_check == OTA_AVAL) {
CANFD_AsyncSend(build_can_id(CANFD_OTA_ACK_ID), 2, transmit_data);
}
}