You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
151 lines
7.0 KiB
151 lines
7.0 KiB
# serial_handler.py
|
|
import serial
|
|
import time
|
|
from serial_init import SerialSharedData
|
|
from transform import transform_data
|
|
|
|
# 声明需要用到的全局变量（与主程序保持一致）
|
|
speed_adjustment = 100 # 初始值设为100，在50-100范围内
|
|
Trajectory_angle = 0
|
|
compensate_queue = None # 后续由主程序赋值
|
|
|
|
# 初始化全局共享数据实例
|
|
serial_shared = SerialSharedData()
|
|
# 串口接收线程函数，持续监听串口数据，解析0xFF坐标帧并更新补偿变量
|
|
def serial_receive_data(ser):
|
|
"""线程内持续接收串口数据，同步解析0xFF坐标帧并更新全局补偿变量"""
|
|
frame_header = 0xFF
|
|
frame_length = 7 # 0xFF + x(2) + y(2) + z(2)
|
|
|
|
while True:
|
|
try:
|
|
if not ser or not isinstance(ser, serial.Serial) or not ser.is_open:
|
|
time.sleep(0.1)
|
|
continue
|
|
|
|
if ser.in_waiting > 0:
|
|
data = ser.read(ser.in_waiting)
|
|
with serial_shared.lock:
|
|
serial_shared.buffer += data
|
|
serial_shared.cmd_buffer += data
|
|
|
|
# 在接收线程中解析0xFF坐标帧
|
|
with serial_shared.lock:
|
|
current_buf = serial_shared.buffer[:]
|
|
|
|
if len(current_buf) >= frame_length:
|
|
header_index = current_buf.find(bytes([frame_header]))
|
|
if header_index != -1 and header_index + frame_length <= len(current_buf):
|
|
# 提取完整帧并更新缓冲区
|
|
frame = current_buf[header_index:header_index+frame_length]
|
|
with serial_shared.lock:
|
|
serial_shared.buffer = serial_shared.buffer[header_index+frame_length:]
|
|
|
|
# 坐标解码
|
|
x_coord = (frame[1] << 8) | frame[2]
|
|
x_coord = x_coord - 0x10000 if x_coord & 0x8000 else x_coord
|
|
if x_coord>90:
|
|
x_coord = 90
|
|
elif x_coord<-90:
|
|
x_coord = -90
|
|
y_coord = (frame[3] << 8) | frame[4]
|
|
y_coord = y_coord - 0x10000 if y_coord & 0x8000 else y_coord
|
|
if y_coord>50:
|
|
y_coord = 50
|
|
elif y_coord<-50:
|
|
y_coord = -50
|
|
z_coord = (frame[5] << 8) | frame[6]
|
|
z_coord = z_coord - 0x10000 if z_coord & 0x8000 else z_coord
|
|
if z_coord>90:
|
|
z_coord = 90
|
|
elif z_coord<-90:
|
|
z_coord = -90
|
|
|
|
inc_x, inc_y, inc_z = transform_data(x_coord, y_coord, z_coord)
|
|
# 存入队列（覆盖旧值，确保取到最新）
|
|
if compensate_queue is not None and not compensate_queue.empty():
|
|
compensate_queue.get_nowait()
|
|
if compensate_queue is not None:
|
|
compensate_queue.put((inc_x, inc_y, inc_z))
|
|
|
|
print(f"接收线程解析更新 - x:{x_coord:.1f}, y:{y_coord:.1f}, z:{z_coord:.1f}")
|
|
|
|
except Exception as e:
|
|
print(f"串口接收线程错误: {str(e)}")
|
|
time.sleep(0.005)
|
|
# 从共享缓冲区读取指令帧，解析0xAA指令并返回指令内容（加锁确保线程安全）
|
|
def read_cmd_from_shared(timeout=1.0):
|
|
"""从共享缓冲区读取指令帧，解析0xAA指令并返回指令内容（加锁确保线程安全）"""
|
|
frame_header = 0xAA
|
|
cmd_length = 2 # 指令帧结构：0xAA（帧头） + 1字节指令值
|
|
start_time = time.time()
|
|
|
|
while (time.time() - start_time) < timeout:
|
|
# 加锁读取指令缓冲区
|
|
with serial_shared.lock:
|
|
current_cmd_buf = serial_shared.cmd_buffer[:]
|
|
|
|
if len(current_cmd_buf) >= cmd_length:
|
|
header_index = current_cmd_buf.find(bytes([frame_header]))
|
|
if header_index != -1:
|
|
# 提取完整指令帧
|
|
cmd_bytes = current_cmd_buf[header_index:header_index+cmd_length]
|
|
# 更新共享缓冲区（移除已处理指令）
|
|
with serial_shared.lock:
|
|
serial_shared.cmd_buffer = serial_shared.cmd_buffer[header_index+cmd_length:]
|
|
return cmd_bytes
|
|
time.sleep(0.01)
|
|
return None
|
|
# 处理特殊指令0xBB，更新全局参数并打印清晰信息
|
|
def handle_special_commands(PRESET_PARAMS):
|
|
|
|
global speed_adjustment, Trajectory_angle
|
|
|
|
bb_header = 0xBB
|
|
bb_frame_length = 2 # 0xBB + 1字节配置选择值(0-11)
|
|
# 初始化返回值（默认无选中配置）
|
|
selected_delay_map = None
|
|
selected_relay_config = None
|
|
|
|
# 加锁读取缓冲区，避免线程冲突
|
|
with serial_shared.lock:
|
|
current_cmd_buf = serial_shared.cmd_buffer[:]
|
|
|
|
# 处理0xBB指令（参数配置选择）
|
|
bb_index = current_cmd_buf.find(bytes([bb_header]))
|
|
if bb_index != -1 and bb_index + bb_frame_length <= len(current_cmd_buf):
|
|
# 提取完整的0xBB帧
|
|
bb_frame = current_cmd_buf[bb_index:bb_index+bb_frame_length]
|
|
# 解析配置选择值（1-12有效，对应PRESET_PARAMS的0-11索引）
|
|
config_select = bb_frame[1]
|
|
|
|
# 校验配置值范围（仅处理1-12，共12组）
|
|
if 1 <= config_select <= len(PRESET_PARAMS):
|
|
# 匹配预设参数项（索引 = 配置值 - 1）
|
|
selected_preset = PRESET_PARAMS[config_select - 1]
|
|
# 提取params并更新全局变量
|
|
speed_val, angle_val = selected_preset["params"]
|
|
speed_adjustment = speed_val
|
|
Trajectory_angle = angle_val
|
|
# 提取当前配置项对应的delay和relay配置（赋值给返回变量）
|
|
selected_delay_map = selected_preset["program_delay_map"]
|
|
selected_relay_config = selected_preset["relay_config"]
|
|
|
|
# 打印清晰的配置信息
|
|
print(f"=== 0xBB指令处理成功 ===")
|
|
print(f"配置选择值：{config_select}")
|
|
print(f"speed_adjustment: {speed_adjustment}%")
|
|
print(f"Trajectory_angle: {Trajectory_angle}")
|
|
print(f"已加载对应program_delay_map和relay_config配置")
|
|
|
|
# 移除已处理的帧（加锁更新缓冲区）
|
|
with serial_shared.lock:
|
|
serial_shared.cmd_buffer = serial_shared.cmd_buffer[bb_index+bb_frame_length:]
|
|
else:
|
|
# 无效配置值处理：清理帧但不更新参数
|
|
with serial_shared.lock:
|
|
serial_shared.cmd_buffer = serial_shared.cmd_buffer[bb_index+bb_frame_length:]
|
|
print(f"⚠️ 收到0xBB指令，配置选择值{config_select}无效（仅支持1-{len(PRESET_PARAMS)}）")
|
|
|
|
# 返回选中的delay配置和relay配置（无选中则返回None）
|
|
return selected_delay_map, selected_relay_config
|