电池部分

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

src/mc/src/can_struct.cpp

@@ -5,3 +5,5 @@ 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;
+can_BMS_query_0x100 bms_query_0x100;
+can_BMS_query_0x101 bms_query_0x101;

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.gear " << msg.gear << std::endl;
-  std::cout << "msg.brake " << msg.brake << std::endl;
+  // std::cout << "msg.brake " << msg.brake << std::endl;
-  std::cout << "msg.rpm " << msg.rpm << 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");
 }

src/mqtt_report/src/mqtt_report.cpp

@@ -20,36 +20,49 @@ void Msg_Handler(const sweeperMsg::CanFrame::SharedPtr msg)
 {
     switch (msg->id)
     {
-    // BMS_Status
+    // BMS_Status 100
-    case 0x1821E5F1:
+    case 0x100:
     {
-        info_report.power = msg->data[4] * 0.5f;
+        uint16_t voltage_raw = (msg->data[0] << 8) | msg->data[1];  // 总电压
-        info_report.chargeStatus = (msg->data[3] == 0x61);
+        int16_t current_raw = (msg->data[2] << 8) | msg->data[3];   // 电流
-        std::cout << "[0x1821E5F1] power : " << info_report.power << std::endl;
+        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
+        // BMS_Status 101
-    case 0x1825E5F1:
+    case 0x101:
     {
-        for (int i = 0; i < 28; ++i)
+        // 提取原始数据
-        {
+        uint16_t full_capacity_raw = (msg->data[0] << 8) | msg->data[1]; // 充满容量
-            int byte_index = 1 + (i / 4);       // 从 data[1] 开始，所以 +1
+        uint16_t cycle_count_raw = (msg->data[2] << 8) | msg->data[3];   // 循环次数
-            int bit_offset = (3 - (i % 4)) * 2; // 每 2bit 表示一个错误
+        uint16_t rsoc_raw = (msg->data[4] << 8) | msg->data[5];          // 剩余容量百分比
-            uint8_t level = (msg->data[byte_index] >> bit_offset) & 0x03;
 
-            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)
+        // 保存解析结果
-            {
+        info_report.power = rsoc;
-                vehicle_error_code.addErrorCode(error_code);
+
-                updateFaultLevel(error_code, level);
+        // 输出打印
-            }
+        std::cout << std::fixed << std::setprecision(2);
-            else
+        std::cout << "[0x101] 充满容量: " << full_capacity << " Ah, "
-            {
+                  << "循环次数: " << cycle_count << " 次, "
-                vehicle_error_code.removeErrorCode(error_code);
+                  << "剩余容量百分比(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<uint16_t>(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] controllerTemp : " << info_report.controllerTemp << std::endl;
-        std::cout << "[0x0CF11F00] motorTemp : " << info_report.motorTemp << std::endl;
+        // std::cout << "[0x0CF11F00] motorTemp : " << info_report.motorTemp << std::endl;
-        std::cout << "[0x0CF11F00] gear : " << info_report.gear << 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<uint16_t>((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)