mirror of
https://github.com/dptech-corp/Uni-Lab-OS.git
synced 2025-12-17 21:11:12 +00:00
118 lines
3.8 KiB
Python
118 lines
3.8 KiB
Python
import json
|
||
import threading,time
|
||
|
||
# class TempSensorNode:
|
||
# def __init__(self,port,warning,address):
|
||
# self._value = 0.0
|
||
# self.warning = warning
|
||
# self.device_id = address
|
||
|
||
# self.hardware_interface = port
|
||
# # t = threading.Thread(target=self.read_temperature, daemon=True)
|
||
# # t.start()
|
||
|
||
# def send_command(self ,command):
|
||
# print('send_command---------------------')
|
||
# pass
|
||
|
||
# @property
|
||
# def value(self) -> float:
|
||
# self._value = self.send_command(self.device_id)
|
||
# return self._value
|
||
# # def read_temperature(self):
|
||
# # while True:
|
||
# # self.value = self.send_command(self.device_id)
|
||
# # print(self.value,'-----------')
|
||
# # time.sleep(1)
|
||
|
||
# def set_warning(self, warning_temp):
|
||
# self.warning = warning_temp
|
||
|
||
|
||
|
||
import serial
|
||
import struct
|
||
from rclpy.node import Node
|
||
import rclpy
|
||
import threading
|
||
|
||
class TempSensorNode():
|
||
def __init__(self,port,warning,address,baudrate=9600):
|
||
self._value = 0.0
|
||
self.warning = warning
|
||
self.device_id = address
|
||
self.success = False
|
||
# 配置串口
|
||
self.hardware_interface = serial.Serial(
|
||
port=port,
|
||
baudrate=baudrate,
|
||
bytesize=serial.EIGHTBITS,
|
||
parity=serial.PARITY_NONE,
|
||
stopbits=serial.STOPBITS_ONE,
|
||
timeout=1
|
||
)
|
||
self.lock = threading.Lock()
|
||
|
||
def calculate_crc(self,data):
|
||
crc = 0xFFFF
|
||
for pos in data:
|
||
crc ^= pos
|
||
for i in range(8):
|
||
if (crc & 0x0001) != 0:
|
||
crc >>= 1
|
||
crc ^= 0xA001
|
||
else:
|
||
crc >>= 1
|
||
return crc
|
||
|
||
def build_modbus_request(self, device_id, function_code, register_address, register_count):
|
||
request = struct.pack('>BBHH', device_id, function_code, register_address, register_count)
|
||
crc = self.calculate_crc(request)
|
||
request += struct.pack('<H', crc)
|
||
return request
|
||
|
||
def read_modbus_response(self,response):
|
||
if len(response) < 5:
|
||
return None
|
||
data = response[:-2]
|
||
crc_received = struct.unpack('<H', response[-2:])[0]
|
||
crc_calculated = self.calculate_crc(data)
|
||
if crc_received == crc_calculated:
|
||
return data[3:]
|
||
return None
|
||
|
||
def send_prototype_command(self ,command):
|
||
# with self.lock:
|
||
function_code = 0x04 # 读输入寄存器
|
||
register_address = 0x0003 # 温度高位寄存器地址
|
||
register_count = 0x0002 # 读取两个寄存器
|
||
|
||
request = self.build_modbus_request(command, function_code, register_address, register_count)
|
||
self.hardware_interface.write(request)
|
||
response = self.hardware_interface.read(9) # 读取9字节的响应
|
||
data = self.read_modbus_response(response)
|
||
if data is None:
|
||
return None
|
||
|
||
high_value = struct.unpack('>H', data[:2])[0]
|
||
low_value = struct.unpack('>H', data[2:])[0]
|
||
|
||
# 组合高位和低位并计算实际温度
|
||
raw_temperature = (high_value << 16) | low_value
|
||
if raw_temperature & 0x8000: # 如果低位寄存器最高位为1，表示负值
|
||
raw_temperature -= 0x10000 # 转换为正确的负数表示
|
||
|
||
actual_temperature = raw_temperature / 10.0
|
||
return actual_temperature
|
||
|
||
@property
|
||
def value(self) -> float:
|
||
self._value = self.send_prototype_command(self.device_id)
|
||
return self._value
|
||
|
||
def set_warning(self, command):
|
||
self.success = False
|
||
temp = json.loads(command)["warning_temp"]
|
||
self.warning = round(float(temp), 1)
|
||
self.success = True
|