Initial commit

2026-07-27 13:51:19 +08:00
commit 7bec56ca51
5408 changed files with 1126933 additions and 0 deletions
@@ -0,0 +1,173 @@
+#!/usr/bin/env python3
+import rospy
+import serial
+import binascii
+import time
+from std_msgs.msg import String
+from sensor_msgs.msg import BatteryState
+
+class BMSDriverNode:
+    def __init__(self):
+        rospy.init_node('bms_driver_mingnuo', anonymous=False)
+        
+        self.port = rospy.get_param('~port', '/dev/ttyUSB0')
+        self.baudrate = rospy.get_param('~baudrate', 9600)
+        
+        self.timeout = 0.5
+        self.battery_cmd = b'\x01\x03\x00\x00\x00\x1E\xC5\xC2'  # 电量查询指令
+        self.alarm_cmd = b'\x01\x01\x00\x00\x00\x34\x3D\xDD'     # 报警查询指令
+        self.polling_rate = 1.0
+        
+        try:
+            self.ser = serial.Serial(
+                port=self.port,
+                baudrate=self.baudrate,
+                timeout=self.timeout,
+                parity='N',
+                stopbits=1,
+                bytesize=8
+            )
+            rospy.loginfo(f"Successfully connected to BMS: {self.port} @ {self.baudrate}bps")
+        except serial.SerialException as e:
+            rospy.logerr(f"Failed to connect to BMS: {e}")
+            raise
+        
+        # 初始化全局消息对象
+        self.global_msg = BatteryState()
+        self.battery_pub = rospy.Publisher('/sensor/battery_state', BatteryState, queue_size=10)
+        self.rate = rospy.Rate(self.polling_rate)
+
+    def update_battery_data(self, hex_data):
+        """更新全局消息中的电池数据"""
+        try:
+            # if len(hex_data) < 65:
+            #     rospy.logwarn(f"Battery response frame too short: {hex_data.hex()}")
+            #     return False
+            
+            # if hex_data[0] != 0x01 or hex_data[1] != 0x03 or hex_data[2] != 0x3C:
+            #     rospy.logwarn(f"Battery response frame format error: {hex_data.hex()}")
+            #     return False
+            
+            # 更新电池数据
+            self.global_msg.header.stamp = rospy.Time.now()
+            self.global_msg.voltage = ((hex_data[3] << 8) | hex_data[4]) / 100.0
+            self.global_msg.percentage = ((hex_data[7] << 8) | hex_data[8])
+            
+            unsigned_value = (hex_data[13] << 8) | hex_data[14]
+	    # 转换为16位有符号整数
+            if unsigned_value & 0x8000: 
+               signed_value = unsigned_value - 0x10000  
+            else:
+               signed_value = unsigned_value  # 正数直接使用
+	    
+            self.global_msg.current = signed_value / 100.0
+           
+            self.global_msg.power_supply_status = 1 if self.global_msg.current > 0.5 else 0
+            self.global_msg.present = True
+            self.global_msg.temperature = (hex_data[61] << 8) | hex_data[62]
+            
+            return True
+        except Exception as e:
+            rospy.loginfo(f"Error updating battery data: {e}")
+            return False
+
+    def update_alarm_data(self, hex_data):
+        """更新全局消息中的报警数据"""
+        try:
+            # if len(hex_data) < 12:
+            #     rospy.logwarn(f"Alarm response frame too short: {hex_data.hex()}")
+            #     return False
+            
+            # if hex_data[0] != 0x01 or hex_data[1] != 0x01 or hex_data[2] != 0x07:
+            #     rospy.logwarn(f"Alarm response frame format error: {hex_data.hex()}")
+            #     return False
+            
+            # 更新报警数据
+            self.global_msg.header.stamp = rospy.Time.now()
+            
+            alarm_code1 = hex_data[3]
+            self.global_msg.power_supply_health = alarm_code1
+            
+            if ((hex_data[7] == 0) and (hex_data[8] == 0) and (hex_data[9] == 0)):
+                alarm_code_over_discharge_voltage = 0
+            else:
+                alarm_code_over_discharge_voltage = 1
+            
+            if ((hex_data[6] == 0) and (hex_data[5] == 0) and (hex_data[4] < 16)):
+                alarm_code_over_charge_voltage = 0
+            else:
+                alarm_code_over_charge_voltage = 1
+            
+            self.global_msg.power_supply_technology = (hex_data[4] & 0xF) | (alarm_code_over_charge_voltage << 4) | (alarm_code_over_discharge_voltage << 5)
+            return True
+        except Exception as e:
+            rospy.loginfo(f"Error updating alarm data: {e}")
+            return False
+
+    def receive_and_update(self):
+        """接收数据并更新全局消息"""
+        data = self.ser.read_all()
+        if not data:
+            return False, "No data"
+        
+        # 判断是否为电量响应
+        if data[0] == 0x01 and data[1] == 0x03 and data[2] == 0x3C:
+            success = self.update_battery_data(data)
+            return success, "Battery data updated"
+        
+        # 判断是否为报警响应
+        elif data[0] == 0x01 and data[1] == 0x01 and data[2] == 0x07:
+            success = self.update_alarm_data(data)
+            return success, "Alarm data updated"
+        
+        # 不符合任何类型
+        else:
+            rospy.loginfo(f"Unknown response frame: {data.hex()}")
+            return False, "Unknown frame"
+
+    def run(self):
+        while not rospy.is_shutdown():
+            try:
+                # 1. 发送电量指令并更新
+                self.ser.flushInput()
+                self.ser.write(self.battery_cmd)
+                rospy.logdebug(f"Sent battery command: {binascii.hexlify(self.battery_cmd).decode().upper()}")
+                time.sleep(0.19)
+                
+                battery_success, battery_msg = self.receive_and_update()
+                if battery_success:
+                    self.battery_pub.publish(self.global_msg)
+                    # rospy.loginfo(f"Battery state updated: Voltage={self.global_msg.voltage:.2f}V, SOC={self.global_msg.percentage:.1f}%, Current={self.global_msg.current:.2f}A")
+                else:
+                    rospy.loginfo(f"Battery update failed: {battery_msg}")
+                
+                time.sleep(0.2)
+
+                # 2. 发送报警指令并更新
+                self.ser.flushInput()
+                self.ser.write(self.alarm_cmd)
+                rospy.logdebug(f"Sent alarm command: {binascii.hexlify(self.alarm_cmd).decode().upper()}")
+                time.sleep(0.08)
+                
+                alarm_success, alarm_msg = self.receive_and_update()
+                if alarm_success:
+                    self.battery_pub.publish(self.global_msg)
+                    # rospy.loginfo(f"Alarm state updated: Health={self.global_msg.power_supply_health}")
+                else:
+                    rospy.loginfo(f"Alarm update failed: {alarm_msg}")
+
+            except Exception as e:
+                rospy.logerr(f"Communication error: {e}")
+            
+            self.rate.sleep()
+
+if __name__ == '__main__':
+    try:
+        node = BMSDriverNode()
+        node.run()
+    except rospy.ROSInterruptException:
+        pass
+    finally:
+        if 'node' in locals() and hasattr(node, 'ser') and node.ser.is_open:
+            node.ser.close()
+            rospy.loginfo("BMS serial port closed")