174 lines
7.0 KiB
Python
174 lines
7.0 KiB
Python
#!/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")
|