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

feat(CAN): 添加静默模式支持并优化节点ID处理

Browse Source 
- 添加静默模式功能，在接收到base_id=0x0000消息时停止发送10秒
- 修正节点ID加载逻辑，区分有效节点ID(0-65534)和Flash擦除状态(0xFFFF)
- 添加静默模式相关的状态检查和时间管理功能
- 在CAN通信检查中集成静默模式处理流程

fix(FOC): 修复速度控制中的方向参数应用
- 在velocity_control、angle_control_with_velocity_limit和MIT_Mode函数中
 正确应用DIR参数调整速度误差的符号
- 添加详细注释说明DIR参数对速度误差计算的影响

refactor(systick): 添加毫秒级系统时间功能
- 实现get_systick_ms()函数提供毫秒级系统时间
- 添加SysTick中断处理程序更新系统时间计数器

chore(init): 调整初始化顺序优化系统启动流程
- 将EEPROM初始化移至CAN初始化之前，确保正确的依赖关系
This commit is contained in:
cbb
2026-03-19 23:49:08 +08:00
parent 9f56ab394e
commit bdf03517b3
9 changed files with 126 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Robot_V2/Algorithm/FOC.cpp
+6 -4
View File
@@ -367,8 +367,10 @@ float FOC_Controller::angle_control() {
* @return float: 当前速度
*/
float FOC_Controller::velocity_control() {
// PID 控制计算
float Uq = vel_loop((target_velocity - rt_velocity) * REDUCTION_RATIO);
// PID 控制计算，使用 DIR 参数调整速度误差的符号
// DIR = -1.0f 时：速度误差 = -(target_velocity - rt_velocity) = rt_velocity - target_velocity
// DIR = 1.0f 时：速度误差 = target_velocity - rt_velocity
float Uq = vel_loop((target_velocity - rt_velocity) * DIR * REDUCTION_RATIO);
CAN0_transmiter.testfloat_union.f = Uq;
// 获取电机轴角度
@@ -424,7 +426,7 @@ float FOC_Controller::angle_control_with_velocity_limit() {
} else {
vel_inVelAngloop.set_P(vel_angloop_p);
}
float Uq = vel_inVelAngloop(target_velocity - rt_velocity);
float Uq = vel_inVelAngloop(target_velocity - rt_velocity)* DIR;
CAN0_transmiter.testfloat_union.f = Uq;
// // 实现力度控制：如果电流超过目标电流，则限制输出
@@ -465,7 +467,7 @@ float FOC_Controller::MIT_Mode() {
// 第二层：速度环（中环）
// 根据目标速度和实际速度的差值，通过速度环PID控制器计算目标电流
float velocity_error = (rt_velocity - target_velocity);
float velocity_error = (target_velocity - rt_velocity)* DIR;
vel_inMITloop.set_P(vel_MIT_p);
float target_current = vel_inMITloop(velocity_error);
Robot_V2/device/ADC.cpp
+1 -1
View File
@@ -80,7 +80,7 @@ void adc_config(void) {
adc_channel_length_config(ADC0, ADC_REGULAR_CHANNEL, 6);
/* 配置ADC规则通道 */
adc_regular_channel_config(ADC0, 0, ADC_CHANNEL_16,
ADC_SAMPLETIME_55POINT5);
ADC_SAMPLETIME_41POINT5);
adc_regular_channel_config(ADC0, 1, ADC_CHANNEL_9, ADC_SAMPLETIME_28POINT5);
adc_regular_channel_config(ADC0, 2, ADC_CHANNEL_3, ADC_SAMPLETIME_28POINT5);
adc_regular_channel_config(ADC0, 3, ADC_CHANNEL_4, ADC_SAMPLETIME_28POINT5);
Robot_V2/device/Can.cpp
+79 -22
View File
@@ -33,6 +33,7 @@ extern EncoderAngleKalman Angle_kalman; // 角度滤波
extern EncoderVelocityKalman Vel_kalman; // 速度滤波
extern FlashEEPROM flashEEP;
extern VersionManager Version; // 声明外部版本管理器实例
extern u32 motor_runtime_50us; // 电机驱动计算轮次（用于静默模式计时）
// 添加 DMA 相关变量
dma_parameter_struct dma_rx_parameter;
@@ -105,16 +106,17 @@ void CAN_transmiter::set_node_id(u16 new_id) {
\retval FLASH_EEPROM_OK: 加载成功
其他：加载失败，使用默认值
\note Flash 擦除状态为 0xFFFF，需要特殊处理避免误判
注意：0 是有效的节点 ID，不应被视为无效值
*/
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) {
// 检查 Flash 是否为擦除状态 (0xFFFF) 或读取失败
// 注意：Flash 未写入时读到的值是 0xFFFF
// 0 是有效的节点 ID，不应被视为无效值
if (status != FLASH_EEPROM_OK || node_id_temp == 0xFFFF) {
node_id = DEFAULT_NODE_ID;
CAN_DEBUG_PRINT(
"Node ID not found in Flash (status=%d, val=0x%04X), using "
@@ -123,7 +125,7 @@ u8 CAN_transmiter::load_node_id_from_flash() {
return FLASH_EEPROM_DATA_NOT_FOUND;
}
// 检查节点 ID 是否有效
// 检查节点 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);
@@ -203,6 +205,37 @@ bool CAN_transmiter::dequeue_rx_message(can_receive_message_struct &message) {
return false;
}
// 静默模式相关函数实现
// 使用 systick 获取系统时间（ms）
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
@@ -645,10 +678,16 @@ void CAN_transmiter::update(void) {
/**
* @brief CANFD 发送函数
* @retval 0: 发送失败
* @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 字节
@@ -694,11 +733,13 @@ u8 CAN_transmiter::CANFD_all_Send() {
void CAN_transmiter::communication_check() {
// CAN0 发送数据标志
if (SET == can0_send_flag) {
// CAN_DEBUG_PRINT("\r\n can0 %X receive data:",
// receive_message.rx_efid);
update();
// CAN0_transmiter.CAN_all_Send();
CANFD_all_Send();
// 检查是否在静默模式中
if (!is_in_silent_mode()) {
update();
CANFD_all_Send();
} else {
check_silent_mode(); // 检查静默是否结束
}
can0_send_flag = RESET;
}
@@ -709,14 +750,37 @@ void CAN_transmiter::communication_check() {
// current_message.rx_efid); for (i = 0; i < current_message.rx_dlen;
// i++) { CAN_DEBUG_PRINT(" %02x", current_message.rx_data[i]);
// }
// 检查消息是否是发给本节点的 (检查高 16 位)
// 提取基础 ID (低 16 位，去除高 16 位节点 ID 部分)
u32 base_id = current_message.rx_efid & 0xFFFF;
// 提取发送方的节点 ID
u16 sender_node_id = (u16)((current_message.rx_efid >> 16) & 0xFFFF);
// 特殊处理 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 命令处理完跳过后续逻辑
}
// 其他 base_id 的消息，检查是否是发给本节点的
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];
@@ -787,13 +851,6 @@ void CAN_transmiter::communication_check() {
} else if (base_id == CANFD_VERSION_ID) {
// 处理版本号请求，发送当前系统版本信息
Version.sendVersionInfoViaCan();
} 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 为不激活
fmc_lock();
jumpToBOOT();
} else if (base_id == CANFD_OTA_ACK_ID) {
volatile u32 *flash_ptr = (volatile u32 *)n_APP_LATEST_EN_ADDR;
u32 app_enable = *flash_ptr;
Robot_V2/device/Can.h
+6
View File
@@ -73,8 +73,11 @@ class CAN_transmiter {
private:
static const uint8_t CAN_RX_BUFFER_SIZE = 8; // CAN 接收缓冲区大小
static const u32 CAN_SILENT_DURATION_MS = 10000; // CAN 静默时长 10 秒
bool is_sleep = false; // 是否休眠
bool is_silent = false; // 是否静默（收到 base_id=0x0000 后进入静默）
u32 silent_start_time = 0; // 静默开始时间（ms）
u16 node_id; // 本节点 ID(0-254)，使用 CAN ID 高 8 位存储 (在 init() 中初始化为 DEFAULT_NODE_ID)
u8 Target_Control_Mode = 0; // 目标控制模式
@@ -115,6 +118,7 @@ class CAN_transmiter {
bool is_rx_buffer_full();
bool is_rx_buffer_empty();
bool dequeue_rx_message(can_receive_message_struct& message);
void check_silent_mode(); // 检查静默模式是否结束
public:
// 节点 ID 相关辅助函数 (使用高 16 位存储节点 ID)
@@ -139,6 +143,8 @@ class CAN_transmiter {
u8 CANFD_AsyncSend(u32 ID, u8 length, u8 *data); // CANFD 异步发送 (使用 DMA)
u8 CAN_all_Send(); // CAN 发送
u8 CANFD_all_Send(); // CANFD 发送
void enter_silent_mode(); // 进入静默模式（停发 10 秒）
bool is_in_silent_mode(); // 检查是否在静默模式中
// 节点 ID 管理接口
u16 get_node_id(); // 获取当前节点 ID
Robot_V2/main.cpp
+4 -5
View File
@@ -78,13 +78,12 @@ u8 hardware_init(void) {
init_status += KTH7823.init(); // 角度传感器初始化
init_status += sampler.init(); // 采样器初始化
// 先初始化 CAN，确保节点 ID 被正确设置
// CAN 初始化会先设置默认节点 ID，然后从 Flash 加载
init_status += CAN0_transmiter.init(); // CAN 初始化
// EEPROM 初始化 (必须在 CAN 初始化之后，因为会读取节点 ID)
// EEPROM 初始化 (必须在 CAN 初始化之前，因为 CAN 会从 Flash 读取节点 ID)
flashEEP.init(); // EEPROM 初始化
// CAN 初始化 (会先设置默认节点 ID，然后从 Flash 加载已保存的节点 ID)
init_status += CAN0_transmiter.init(); // CAN 初始化
init_status += Angle_kalman.init(KTH7823.getAngle()); // 角度滤波初始化
init_status += Vel_kalman.init(0.0f); // 速度滤波初始化
init_status += FOC.init(); // FOC 初始化
Robot_V2/sys/config.h
+3 -3
View File
@@ -17,8 +17,8 @@
// 版本号定义 - 可根据实际项目进行调整
constexpr u16 SOFTWARE_VERSION_MAJOR = 26;
constexpr u16 SOFTWARE_VERSION_MINOR = 3;
constexpr u16 SOFTWARE_VERSION_PATCH = 15;
constexpr u16 SOFTWARE_VERSION_BUILD = 4;
constexpr u16 SOFTWARE_VERSION_PATCH = 16;
constexpr u16 SOFTWARE_VERSION_BUILD = 2;
constexpr u16 HARDWARE_VERSION_MAJOR = 25;
constexpr u16 HARDWARE_VERSION_MINOR = 12;
@@ -64,7 +64,7 @@ constexpr u32 OTA_AVAL = 0x00000000; // 全零表示可用
// 参数配置
constexpr u8 PP = 11;
constexpr float DIR = -1.0f;
constexpr float DIR = 1.0f; // 默认方向：-1.0f 反转，1.0f 正转
constexpr u16 REDUCTION_RATIO = 25;
constexpr float CURRENT_OFFSET = -3.31f / 2.0f; // 当前电流传感器的零点偏移
constexpr float HOLD_PWM_DUTY = 0.1f;
Robot_V2/sys/flash.cpp
+3 -1
View File
@@ -165,7 +165,9 @@ FlashEepromStatus FlashEEPROM::read(u8 virtualAddr, u16 *data) const {
u8 addr = (u8)(data_word & DATA_ADDR_MASK);
// 找到匹配的虚拟地址
if (addr == virtualAddr) {
*data = (u16)(data_word & DATA_U16_MASK);
// 右移 16 位以提取高 16 位的 u16 数据
*data = (u16)((data_word & DATA_U16_MASK) >> 16);
EEP_DEBUG_PRINT("read u16: addr=0x%x, val=%d\r\n", address, *data);
return FLASH_EEPROM_OK;
}
}
Robot_V2/sys/systick.cpp
+22
View File
@@ -52,9 +52,20 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#endif
volatile static uint32_t delay;
volatile static uint32_t systick_ms = 0; // 系统运行时间（毫秒）
extern CAN_transmiter CAN0_transmiter;
extern FlashEEPROM flashEEP;
/*!
\brief get system tick time in milliseconds
\param[in] none
\param[out] none
\retval system time in ms
*/
uint32_t get_systick_ms(void) {
return systick_ms;
}
/*!
\brief configure systick
\param[in] none
@@ -71,6 +82,17 @@ void systick_config(void) {
/* configure the systick handler priority */
NVIC_SetPriority(SysTick_IRQn, 0x00U);
}
/*!
\brief SysTick handler - increment millisecond counter
\param[in] none
\param[out] none
\retval none
*/
extern "C" void SysTick_Handler(void) {
systick_ms++; // 递增系统时间
delay_decrement(); // 递减延时计数器
}
/*!
\brief delay a time in seconds
\param[in] count: count in seconds
Robot_V2/sys/systick.h
+2
View File
@@ -42,6 +42,8 @@ extern "C" {
/* configure systick */
void systick_config(void);
/* get system tick time in milliseconds */
uint32_t get_systick_ms(void);
/* delay a time in milliseconds */
void delay_1ms(uint32_t count);
/* delay a time in seconds */