From dbd3a49331ccce7f9ee9326ed08858e81cfac5dc Mon Sep 17 00:00:00 2001 From: lyq Date: Tue, 4 Nov 2025 10:05:38 +0800 Subject: [PATCH] =?UTF-8?q?=E7=94=B5=E6=B1=A0=E9=83=A8=E5=88=86?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/mc/include/mc/can_struct.h | 62 ++++++++++++++++++++++++++ src/mc/src/can_struct.cpp | 4 +- src/mc/src/timer_tasks.cpp | 28 ++++++++++-- src/mqtt_report/src/mqtt_report.cpp | 69 +++++++++++++++++------------ 4 files changed, 131 insertions(+), 32 deletions(-) diff --git a/src/mc/include/mc/can_struct.h b/src/mc/include/mc/can_struct.h index 4df53f8..b7f7b5f 100644 --- a/src/mc/include/mc/can_struct.h +++ b/src/mc/include/mc/can_struct.h @@ -9,6 +9,66 @@ #pragma pack(push, 1) // 禁止结构体补齐，确保8字节紧密排列 +struct can_BMS_query_0x100 +{ + // 报文ID与控制标志定义 + static constexpr uint32_t CMD_ID = 0x100; // 标准帧ID + static constexpr bool EXT_FLAG = false; // 11位标准帧 + static constexpr bool RTR_FLAG = false; // 普通数据帧（非远程帧） + static constexpr uint8_t DLC = 8; // 数据长度为8字节 + + // 数据区（主机查询时通常发送全0） + uint8_t data[8]; + + // 构造函数：初始化数据为0 + can_BMS_query_0x100() + { + memset(data, 0, sizeof(data)); + } + + // 生成CAN帧 + CANFrame toFrame() const + { + CANFrame frame; + frame.id = CMD_ID; + frame.ext = EXT_FLAG; + frame.rtr = RTR_FLAG; + frame.dlc = DLC; + memcpy(frame.data, data, sizeof(data)); + return frame; + } +}; + +struct can_BMS_query_0x101 +{ + // 报文ID与控制标志定义 + static constexpr uint32_t CMD_ID = 0x101; // 标准帧ID + static constexpr bool EXT_FLAG = false; // 11位标准帧 + static constexpr bool RTR_FLAG = false; // 普通数据帧（非远程帧） + static constexpr uint8_t DLC = 8; // 数据长度为8字节 + + // 数据区（主机查询时通常发送全0） + uint8_t data[8]; + + // 构造函数：初始化数据为0 + can_BMS_query_0x101() + { + memset(data, 0, sizeof(data)); + } + + // 生成CAN帧 + CANFrame toFrame() const + { + CANFrame frame; + frame.id = CMD_ID; + frame.ext = EXT_FLAG; + frame.rtr = RTR_FLAG; + frame.dlc = DLC; + memcpy(frame.data, data, sizeof(data)); + return frame; + } +}; + union can_MCU_cmd_union { struct @@ -358,5 +418,7 @@ extern can_EPS_cmd eps_cmd; extern can_VCU_enable_cmd vcu_enable_cmd; extern can_VCU_motor1_cmd vcu_motor1_cmd; extern can_VCU_motor2_cmd vcu_motor2_cmd; +extern can_BMS_query_0x100 bms_query_0x100; +extern can_BMS_query_0x101 bms_query_0x101; #endif diff --git a/src/mc/src/can_struct.cpp b/src/mc/src/can_struct.cpp index 4a29203..79629e8 100644 --- a/src/mc/src/can_struct.cpp +++ b/src/mc/src/can_struct.cpp @@ -4,4 +4,6 @@ can_MCU_cmd mcu_cmd; can_EPS_cmd eps_cmd; can_VCU_enable_cmd vcu_enable_cmd; can_VCU_motor1_cmd vcu_motor1_cmd; -can_VCU_motor2_cmd vcu_motor2_cmd; \ No newline at end of file +can_VCU_motor2_cmd vcu_motor2_cmd; +can_BMS_query_0x100 bms_query_0x100; +can_BMS_query_0x101 bms_query_0x101; \ No newline at end of file diff --git a/src/mc/src/timer_tasks.cpp b/src/mc/src/timer_tasks.cpp index 19585bc..a5468f6 100644 --- a/src/mc/src/timer_tasks.cpp +++ b/src/mc/src/timer_tasks.cpp @@ -12,9 +12,9 @@ void mcuTimerTask(CANDriver &canctl) mcu_cmd.setBrake(msg.brake); canctl.sendFrame(mcu_cmd.toFrame()); // std::cout << "mcuTimerTask" << std::endl; - std::cout << "msg.gear " << msg.gear << std::endl; - std::cout << "msg.brake " << msg.brake << std::endl; - std::cout << "msg.rpm " << msg.rpm << std::endl; + // std::cout << "msg.gear " << msg.gear << std::endl; + // std::cout << "msg.brake " << msg.brake << std::endl; + // std::cout << "msg.rpm " << msg.rpm << std::endl; } // 定时器回调：EPS 控制 @@ -89,6 +89,23 @@ void vcuTimerTask(CANDriver &canctl) } } +// 定时器回调：BMS 查询任务 +void bmsTimerTask(CANDriver &canctl) +{ + static bool bms_initialized = false; + + // 首次运行时初始化日志 + if (!bms_initialized) + { + RCLCPP_INFO(rclcpp::get_logger("timer_tasks"), "[BMS] Query task started"); + bms_initialized = true; + } + + // 周期性发送 + canctl.sendFrame(bms_query_0x100.toFrame()); + canctl.sendFrame(bms_query_0x101.toFrame()); +} + // 注册所有定时器任务 void setupTimers(rclcpp::Node::SharedPtr node, CANDriver &canctl) { @@ -107,5 +124,10 @@ void setupTimers(rclcpp::Node::SharedPtr node, CANDriver &canctl) [&canctl]() { vcuTimerTask(canctl); }); + static auto timer_bms = node->create_wall_timer( + std::chrono::milliseconds(200), + [&canctl]() + { bmsTimerTask(canctl); }); + RCLCPP_INFO(node->get_logger(), "[TIMER] All timers setup completed"); } \ No newline at end of file diff --git a/src/mqtt_report/src/mqtt_report.cpp b/src/mqtt_report/src/mqtt_report.cpp index 50cf918..c2ae575 100644 --- a/src/mqtt_report/src/mqtt_report.cpp +++ b/src/mqtt_report/src/mqtt_report.cpp @@ -20,36 +20,49 @@ void Msg_Handler(const sweeperMsg::CanFrame::SharedPtr msg) { switch (msg->id) { - // BMS_Status - case 0x1821E5F1: + // BMS_Status 100 + case 0x100: { - info_report.power = msg->data[4] * 0.5f; - info_report.chargeStatus = (msg->data[3] == 0x61); - std::cout << "[0x1821E5F1] power : " << info_report.power << std::endl; + uint16_t voltage_raw = (msg->data[0] << 8) | msg->data[1]; // 总电压 + int16_t current_raw = (msg->data[2] << 8) | msg->data[3]; // 电流 + uint16_t capacity_raw = (msg->data[4] << 8) | msg->data[5]; // 剩余容量 + + float voltage = voltage_raw * 0.01f; // 10mV -> V 总电压 + float current = current_raw * 0.01f; // 10mA -> A 电流 + float capacity = capacity_raw * 0.01f; // 10mAh -> Ah 剩余容量 + + // 根据电流正负判断充放电状态 + info_report.chargeStatus = (current >= 0) ? true : false; + + std::cout << std::fixed << std::setprecision(2); + std::cout << "[0x100] 总电压: " << voltage << " V, " + << "电流: " << current << " A, " + << "剩余容量: " << capacity << " Ah, " + << "是否在充电: " << (info_report.chargeStatus == 1 ? "是" : "否") << std::endl; break; } - // BMS_Fault - case 0x1825E5F1: + // BMS_Status 101 + case 0x101: { - for (int i = 0; i < 28; ++i) - { - int byte_index = 1 + (i / 4); // 从 data[1] 开始，所以 +1 - int bit_offset = (3 - (i % 4)) * 2; // 每 2bit 表示一个错误 - uint8_t level = (msg->data[byte_index] >> bit_offset) & 0x03; + // 提取原始数据 + uint16_t full_capacity_raw = (msg->data[0] << 8) | msg->data[1]; // 充满容量 + uint16_t cycle_count_raw = (msg->data[2] << 8) | msg->data[3]; // 循环次数 + uint16_t rsoc_raw = (msg->data[4] << 8) | msg->data[5]; // 剩余容量百分比 - int error_code = 1001 + i; + // 数据换算 + float full_capacity = full_capacity_raw * 0.01f; // 10mAh → Ah + uint16_t cycle_count = cycle_count_raw; // 单位：次 + float rsoc = rsoc_raw * 1.0f; // 以百分比% - if (level != 0) - { - vehicle_error_code.addErrorCode(error_code); - updateFaultLevel(error_code, level); - } - else - { - vehicle_error_code.removeErrorCode(error_code); - } - } + // 保存解析结果 + info_report.power = rsoc; + + // 输出打印 + std::cout << std::fixed << std::setprecision(2); + std::cout << "[0x101] 充满容量: " << full_capacity << " Ah, " + << "循环次数: " << cycle_count << " 次, " + << "剩余容量百分比(RSOC): " << rsoc << " %" << std::endl; break; } @@ -63,7 +76,7 @@ void Msg_Handler(const sweeperMsg::CanFrame::SharedPtr msg) float gear_ratio = 32.0; // 电机速比 float wheel_radius = 0.3; // 轮胎直径(m) info_report.speed = (actual_speed_rpm * 60 * 3.1416 * wheel_radius) / (1000 * gear_ratio); - std::cout << "[0x0CF11E00] speed : " << info_report.speed << std::endl; + // std::cout << "[0x0CF11E00] speed : " << info_report.speed << std::endl; // 故障码（data[6]：低字节，data[7]：高字节） uint16_t mcu_fault_code = (static_cast(msg->data[7]) << 8) | msg->data[6]; @@ -109,9 +122,9 @@ void Msg_Handler(const sweeperMsg::CanFrame::SharedPtr msg) int feedback_gear = (ctrl_status >> 2) & 0x03; info_report.gear = feedback_gear; - std::cout << "[0x0CF11F00] controllerTemp : " << info_report.controllerTemp << std::endl; - std::cout << "[0x0CF11F00] motorTemp : " << info_report.motorTemp << std::endl; - std::cout << "[0x0CF11F00] gear : " << info_report.gear << std::endl; + // std::cout << "[0x0CF11F00] controllerTemp : " << info_report.controllerTemp << std::endl; + // std::cout << "[0x0CF11F00] motorTemp : " << info_report.motorTemp << std::endl; + // std::cout << "[0x0CF11F00] gear : " << info_report.gear << std::endl; break; } @@ -122,7 +135,7 @@ void Msg_Handler(const sweeperMsg::CanFrame::SharedPtr msg) // 第3、4字节拼接为原始角度数据 uint16_t raw_value = static_cast((msg->data[3] << 8) | msg->data[4]); info_report.steeringAngle = (raw_value - 1024) / 7.0f; - std::cout << "[0x401] steeringAngle : " << info_report.steeringAngle << std::endl; + // std::cout << "[0x401] steeringAngle : " << info_report.steeringAngle << std::endl; // 清空所有 EPS 错误码 for (int code = 1201; code <= 1218; ++code)