Files
CPY_DevController/udp_server_enhanced.py
2026-06-21 17:13:18 +08:00

554 lines
22 KiB
Python
Executable File

import socket
import base64
import json
import time
import psutil
import struct
import csv
import threading
from collections import defaultdict
from typing import Dict, List, Tuple, Optional
import logging
from config import *
from x_ray_data_processor import processor
from PySide6.QtCore import QObject, Signal, Slot, QThread, QTimer
from scipy.ndimage import gaussian_filter1d
import numpy as np
# 配置日志
logging.basicConfig(level=logging.INFO, format='%(asctime)s - %(levelname)s - %(message)s')
logger = logging.getLogger(__name__)
# 定义adcDataUnitDef结构体
class adcDataUnitDef:
def __init__(self):
self.ucCmd = 0
self.ucAddr = 0
self.usData = 0
# 新增:UDP包头结构定义 (与C端保持一致)
class UDPPacketHeader:
"""UDP包头结构"""
MAGIC = 0xAA55
HEADER_SIZE = 16 # 包头大小
def __init__(self, data: bytes):
"""
从字节数据解析包头
"""
if len(data) < self.HEADER_SIZE:
raise ValueError("数据长度不足,无法解析包头")
# 解析包头字段 (小端序)
(self.magic, self.total_size, self.big_packet_id,
self.packet_id, self.total_packets, self.payload_len) = struct.unpack('<HHIHHH', data[:14])
# 验证魔数
if self.magic != self.MAGIC:
raise ValueError(f"无效的魔数: {self.magic:04X}, 期望: {self.MAGIC:04X}")
# 新增:包重组器,处理单个大包的重组
class PacketReassembler:
"""包重组器,处理单个大包的重组"""
def __init__(self, big_packet_id: int, total_size: int, total_packets: int,
sender_addr: Tuple[str, int], timeout: float = 1.0):
self.big_packet_id = big_packet_id
self.total_size = total_size
self.total_packets = total_packets
self.sender_addr = sender_addr
self.timeout = timeout
self.start_time = time.time()
# 存储接收到的包
self.packets: Dict[int, bytes] = {}
self.received_count = 0
self.completed = False
self.data = None
def add_packet(self, packet_id: int, payload: bytes) -> bool:
"""
添加一个包到重组器
返回True如果包是新接收的,False如果包已存在
"""
if packet_id in self.packets:
return False # 包已存在
self.packets[packet_id] = payload
self.received_count += 1
# 检查是否所有包都已接收
if self.received_count == self.total_packets:
self._reassemble()
return True
return True
def _reassemble(self):
"""重组完整数据"""
try:
# 按包序号排序
sorted_packets = sorted(self.packets.items())
# 重组数据
reassembled_data = b''
for packet_id, payload in sorted_packets:
reassembled_data += payload
# 截取到指定长度
self.data = reassembled_data[:self.total_size]
self.completed = True
logger.info(f"大包 {self.big_packet_id} 重组完成,大小: {len(self.data)} 字节")
except Exception as e:
logger.error(f"重组大包 {self.big_packet_id} 时出错: {e}")
def is_timeout(self) -> bool:
"""检查是否超时"""
return time.time() - self.start_time > self.timeout
def is_completed(self) -> bool:
"""检查是否完成重组"""
return self.completed
def get_data(self) -> Optional[bytes]:
"""获取重组后的数据"""
return self.data if self.completed else None
class UdpServerEnhanced(QThread):
start_cal = Signal(int, bytearray, str, int) # 信号:接收到消息,包含数据、发送者地址和端口
send_collect_data = Signal(dict) #
update_acquisition_info = Signal(int, dict)
def __init__(self, ip="0.0.0.0", port=50009, parent=None):
super(UdpServerEnhanced, self).__init__(parent)
self.kill_udp_port(port)
self.ip = ip
self.port = port
self.server_socket = None
self.running = True
self.motor1_direction = ""
self.motor2_direction = ""
# 新增:分包重组相关属性
self.reassemblers: Dict[Tuple[Tuple[str, int], int], PacketReassembler] = {}
self.reassembler_lock = threading.Lock()
self.cleanup_thread = None
self.timeout = 1.0 # 1秒超时
self.collect_object1 = {
"method" : "scanner1",
"channelA_data" :"base64",
"channelB_data" :"base64",
"intensityA" : 98,
"intensityB" : 98
}
self.collect_object2 = {
"method" : "scanner2",
"channelA_data" :"base64",
"channelB_data" :"base64",
"intensityA" : 98,
"intensityB" : 98
}
self.debug_timer = QTimer(self)
self.debug_timer.timeout.connect(self.update_debug_timer)
if YSMODE:
self.debug_timer.start(1000)
def update_debug_timer(self):
import random
channelA = [random.randint(1, 1000) for _ in range(3000)]
channelB = [random.randint(1, 1000) for _ in range(3000)]
self.collect_object1['channelA_data'] = base64.b64encode(json.dumps(channelA).encode()).decode()
self.collect_object1['channelB_data'] = base64.b64encode(json.dumps(channelB).encode()).decode()
self.collect_object2['channelA_data'] = base64.b64encode(json.dumps(channelA).encode()).decode()
self.collect_object2['channelB_data'] = base64.b64encode(json.dumps(channelB).encode()).decode()
self.send_collect_data.emit(self.collect_object1)
self.send_collect_data.emit(self.collect_object2)
def kill_udp_port(self, port):
for proc in psutil.process_iter():
try:
for conn in proc.connections(kind='udp'):
if conn.laddr.port == port:
print(f" PID={proc.pid}, procname={proc.name()}")
proc.kill()
except psutil.AccessDenied:
continue
def packet_crc(self, data):
usSum = 0
for byte in data:
usSum += byte
return usSum & 0xFFFF # 确保返回值为16位
def handle_sample_data(self, data):
# 确保数据长度是4的倍数
if len(data) % 4 != 0:
raise ValueError("Data length must be a multiple of 4")
# 使用列表推导式来简化代码
unpacked_data = [struct.unpack_from('<H', data[i:i+2])[0] for i in range(0, len(data), 2)]
# 将数据分为两组
data_list1 = unpacked_data[::2] # 取偶数索引的元素
data_list2 = unpacked_data[1::2] # 取奇数索引的元素
# 打印数据长度,用于调试
print(f"Data contains {len(unpacked_data) // 2} pairs of values")
# print(data_list1)
# print("=======================================")
# print(data_list2)
file_content = bytearray()
datalen = len(data_list2)
for i in range (datalen):
file_content.extend(struct.pack('<ff', data_list1[i], data_list2[i]))
return data_list1,data_list2,file_content,datalen
def parse_packet(self, base64_data):
binary_data = base64.b64decode(base64_data)
channel_A = []
channel_B = []
header_a = struct.unpack('<H', binary_data[0:2])[0]
header_b = struct.unpack('<H', binary_data[2:4])[0]
queue_remain = struct.unpack('<H', binary_data[4:6])[0]
data_len = struct.unpack('<H', binary_data[6:8])[0]
if header_a != 0xAA55 or header_b != 0x55CC:
print("unknown packet header",hex(header_a), hex(header_b))
return None
data_points = []
offset = 8
for _ in range(data_len):
if offset + 4 > len(binary_data):
print("offset:", offset, "data_len:", len(data_points))
break
channel_A.append(struct.unpack('<H', binary_data[offset:offset+2])[0])
channel_B.append(struct.unpack('<H', binary_data[offset+2:offset+4])[0])
offset += 4
# # 计算相邻元素的平均值,生成新的channel_A和channel_B
# new_channel_A = []
# new_channel_B = []
# data_len = len(channel_A)
# for i in range(0, data_len - 1, 2):
# avg_A = (int(channel_A[i]) + int(channel_A[i+1])) / 2
# avg_B = (int(channel_B[i]) + int(channel_B[i+1])) / 2
# new_channel_A.append(avg_A)
# new_channel_B.append(avg_B)
# # 更新数组和数据长度
# channel_A = new_channel_A
# channel_B = new_channel_B
# data_len = len(new_channel_A)
file_content = bytearray()
for i in range(data_len):
file_content.extend(struct.pack('<ff', channel_A[i], channel_B[i]))
# 解析数据单元
return channel_A, channel_B,file_content, data_len
# 新增:检查是否为分包数据
def is_fragmented_packet(self, data: bytes) -> bool:
"""检查数据包是否以UDP_PACKET_MAGIC开头"""
if len(data) < 2:
return False
magic = struct.unpack('<H', data[:2])[0]
return magic == UDPPacketHeader.MAGIC
# 新增:处理分包数据
def handle_fragmented_packet(self, data: bytes, addr: Tuple[str, int]):
"""处理分包数据"""
try:
# 解析包头
header = UDPPacketHeader(data)
# 提取载荷数据
payload = data[UDPPacketHeader.HEADER_SIZE:UDPPacketHeader.HEADER_SIZE + header.payload_len]
# 获取或创建重组器
reassembler = self._get_or_create_reassembler(header, addr)
if reassembler is None:
return # 超时或无效的包
# 添加包到重组器
if reassembler.add_packet(header.packet_id, payload):
logger.debug(f"接收包 {header.packet_id}/{header.total_packets} "
f"来自 {addr}, 大包ID: {header.big_packet_id}")
# 检查是否完成重组
if reassembler.is_completed():
self._handle_completed_packet(reassembler)
except Exception as e:
logger.error(f"处理来自 {addr} 的分包数据时出错: {e}")
# 新增:获取或创建包重组器
def _get_or_create_reassembler(self, header: UDPPacketHeader, addr: Tuple[str, int]) -> Optional[PacketReassembler]:
"""获取或创建包重组器"""
key = (addr, header.big_packet_id)
with self.reassembler_lock:
# 检查是否已存在
if key in self.reassemblers:
reassembler = self.reassemblers[key]
# 检查是否超时
if reassembler.is_timeout():
logger.warning(f"大包 {header.big_packet_id} 超时,丢弃")
del self.reassemblers[key]
return None
return reassembler
# 创建新的重组器
reassembler = PacketReassembler(
header.big_packet_id,
header.total_size,
header.total_packets,
addr,
self.timeout
)
self.reassemblers[key] = reassembler
logger.info(f"创建新的大包重组器: ID={header.big_packet_id}, "
f"大小={header.total_size}, 包数={header.total_packets}, 来源={addr}")
return reassembler
def daq_data_packet_ack(self, ip, pid):
try:
# 构建命令数据包
# 命令格式: AA CC 01 XX XX XX XX
# 其中XX XX XX XX为采集时长,小端格式
command = bytearray([0xAA, 0xCC, 0x03])
# 将时长转换为4字节小端格式
command.extend(struct.pack('<I', pid))
# 发送命令
self.server_socket.sendto(command, (ip, 6133))
print(f'已发送ack {pid}')
except Exception as e:
print(f"发送data ack命令失败: {e}")
# 新增:处理完成重组的数据包
def _handle_completed_packet(self, reassembler: PacketReassembler):
"""处理完成重组的数据包"""
data = reassembler.get_data()
if data:
logger.info(f"收到完整数据包: 大小={len(data)} 字节, "
f"来源={reassembler.sender_addr}, ID={reassembler.big_packet_id}")
# 调用现有的数据处理方法
try:
# 尝试解析为JSON格式
jsstr = data.decode().strip('\0')
# print(jsstr)
msg = json.loads(jsstr)
msg_type = msg.get('type')
print(msg_type)
if msg_type == "dev_discovery":
self.handle_dev_discovery(msg, reassembler.sender_addr[0])
elif msg_type == "data_points":
pid = msg.get('pid', None)
if pid:
self.daq_data_packet_ack(reassembler.sender_addr[0], pid)
print("send data ack", reassembler.sender_addr[0], pid)
self.handle_data_point(msg, reassembler.sender_addr[0])
else:
logger.warning(f"未知的消息类型: {msg_type}")
except json.JSONDecodeError:
logger.warning(f"重组后的数据不是有效的JSON格式")
except Exception as e:
logger.error(f"处理重组数据时出错: {e}")
# 从重组器列表中移除
with self.reassembler_lock:
key = (reassembler.sender_addr, reassembler.big_packet_id)
if key in self.reassemblers:
del self.reassemblers[key]
# 新增:清理超时的包
def _cleanup_timeout_packets(self):
"""清理超时的包(在后台线程中运行)"""
while self.running:
time.sleep(0.5) # 每0.5秒检查一次
with self.reassembler_lock:
timeout_keys = []
for key, reassembler in self.reassemblers.items():
if reassembler.is_timeout():
timeout_keys.append(key)
# 移除超时的包
for key in timeout_keys:
reassembler = self.reassemblers[key]
logger.warning(f"清理超时的大包: ID={reassembler.big_packet_id}, "
f"来源={reassembler.sender_addr}, "
f"已接收={reassembler.received_count}/{reassembler.total_packets}")
del self.reassemblers[key]
def run(self):
"""启动 UDP 服务器"""
self.server_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
self.server_socket.bind((self.ip, self.port))
# 增加接收缓冲区大小
self.server_socket.setsockopt(socket.SOL_SOCKET, socket.SO_RCVBUF, 65536) # 设置为 64KB
print(f"UDP server is running on {self.ip}:{self.port}")
# 启动清理线程
self.cleanup_thread = threading.Thread(target=self._cleanup_timeout_packets, daemon=True)
self.cleanup_thread.start()
while self.running:
try:
while True: # 循环接收数据
try:
data, addr = self.server_socket.recvfrom(65536) # 接收最多 64KB 数据
# 检查是否为分包数据
if self.is_fragmented_packet(data):
# 处理分包数据
self.handle_fragmented_packet(data, addr)
else:
# 使用现有的处理方法
try:
if data.startswith(b'hello'):
continue
msg = json.loads(data.decode().strip('\0'))
msg_type = msg.get('type')
if msg_type == "dev_discovery":
self.handle_dev_discovery(msg, addr[0])
if msg_type == "data_points":
self.handle_data_point(msg, addr[0])
except json.JSONDecodeError:
print(f"Error decoding JSON data: {data.decode()}")
except Exception as e:
print(f"Error processing data: {e}")
except OSError as e:
print(f"Error receiving data: {e}") # 记录错误信息
def correct_data(self, filename):
# 加载数据
processor.load_data(filename)
filename = filename[0:-4] +"_corrected.csv"
# 自动选择圆弧区间
if processor.auto_select_arcs():
# 拟合数据
if processor.fit_data():
# 校正数据
corrected = processor.correct_data()
# 保存校正后的数据,去掉头尾各200个点
channel_A,channel_B,corrected = processor.save_corrected_data(
corrected,
filename,
trim_edges=True,
trim_amount=0
)
return channel_A,channel_B,corrected
return None,None,None
def handle_dev_discovery(self, msg, ip_address):
if msg.get("name") == "daq":
status = msg.get("status")
if status:
# temp = status.get("temp", 0) / 10
# speed = status.get("speed", 0)
if ip_address == "192.168.100.236":
self.update_acquisition_info.emit(1, status)
elif ip_address == "192.168.100.237":
self.update_acquisition_info.emit(2, status)
def handle_data_point(self, msg, ip_address):
try:
print("handle data from", ip_address)
direction = ""
if msg.get("direction").upper() == "UP":
direction = "UP"
elif msg.get("direction").upper() == "DOWN":
direction = "DOWN"
if msg.get("name") == "daq":
channelA, channelB,file_content, row_count =self.parse_packet(msg.get("data","") )
index_col = [i for i in range(row_count)]
if len(channelA) > 100:
#gause filter
channelA_filtered = gaussian_filter1d(channelA, 0.5)
channelA = np.array(channelA_filtered).tolist()
channelB_filtered = gaussian_filter1d(channelB, 0.5)
channelB = np.array(channelB_filtered).tolist()
#save data to csv file
filename = f'/home/admin/PythonPro/TGX_PYSERVER/datas/data{time.strftime("%Y%m%d%H%M%S")}.csv'
with open(filename, 'w', newline='') as f:
writer = csv.writer(f)
writer.writerow(['INDEX', 'CH1', 'CH2'])
writer.writerows(zip(index_col, channelA, channelB))
channelA_average = np.mean(channelA)
channelB_average = np.mean(channelB)
if COREECTMODE and channelA_average > 5000 and channelB_average > 5000:
try:
_channelA,_channelB,corrected = self.correct_data(filename)
if corrected:
file_content = corrected
channelA = _channelA
channelB = _channelB
except Exception as e:
print(f"Error correcting data: {e}")
channelA_json = json.dumps(channelA)
channelB_json = json.dumps(channelB)
# print("================channelA_json",channelA_json)
# print("================channelB_json",channelB_json)
channelA_bytes = channelA_json.encode('utf-8')
channelB_bytes = channelB_json.encode('utf-8')
if ip_address == "192.168.100.236":
self.collect_object1["method"] = "scanner1"
if channelA_average < 5000 or channelB_average < 5000:
print("xray low idensity")
else:
self.start_cal.emit(1, file_content, direction, row_count)
print("====emit direction[1]:", direction)
self.collect_object1['channelA_data'] = base64.b64encode(channelA_bytes).decode('utf-8')
self.collect_object1['channelB_data'] = base64.b64encode(channelB_bytes).decode('utf-8')
self.send_collect_data.emit(self.collect_object1)
elif ip_address == "192.168.100.237":
self.collect_object2["method"] = "scanner2"
if channelA_average < 5000 or channelB_average < 5000:
print("xray low idensity")
else:
self.start_cal.emit(2, file_content, direction, row_count)
print("====emit direction[2]:", direction)
self.collect_object2['channelA_data'] = base64.b64encode(channelA_bytes).decode('utf-8')
self.collect_object2['channelB_data'] = base64.b64encode(channelB_bytes).decode('utf-8')
self.send_collect_data.emit(self.collect_object2)
except OSError as e:
print(f"Error handle_data_point: {e}") # 记录错误信息
def stop(self):
"""停止 UDP 服务器"""
self.running = False
if self.server_socket:
self.server_socket.close()
self.wait() # 等待线程结束
def send_message(self, message, ip, port):
"""发送消息到指定的 IP 和端口"""
if self.server_socket:
self.server_socket.sendto(message.encode(), (ip, port))
def __del__(self):
self.stop() # 确保在对象销毁时停止服务器