2026.6.16小更新与光电协调客户端的初版
This commit is contained in:
@@ -315,7 +315,7 @@ opi-yolo/
|
||||
│ ├── check.py # 视频帧率测试
|
||||
│ ├── fc_test.py # 飞控通信测试
|
||||
│ ├── infer.py # 图片推理测试
|
||||
│ └── receiver.py # UDP 图传接收模拟
|
||||
│ └── receiver.py # UDP 图传接收模拟以及简易的JPEG转换为H.264线程实现
|
||||
│
|
||||
├── om/ # 昇腾 OM 模型文件存放目录
|
||||
│ └── yolo26n-balloon2.om
|
||||
|
||||
@@ -375,27 +375,6 @@ class UAVControlLoop:
|
||||
age_us, _ = self.proxy.get_state_freshness(state)
|
||||
age_s = (age_us or 0) / 1_000_000
|
||||
|
||||
if not mode or mode not in ['GUIDED', 'OFFBOARD']:
|
||||
target_mode = 'GUIDED' # ArduPilot
|
||||
# 如果飞控是 PX4，则使用 'OFFBOARD'
|
||||
if self.fc_cfg.get("fc_info", {}).get("type") == "px4":
|
||||
target_mode = 'OFFBOARD'
|
||||
self.logger.info("切换至 %s 模式...", target_mode)
|
||||
ok, ack = self.proxy.send_command(target_mode)
|
||||
if not ok:
|
||||
self.logger.error("模式切换失败: %s", ack)
|
||||
return False
|
||||
# 等待模式切换生效（最多 2 秒）
|
||||
for i in range(20):
|
||||
state = self.proxy.get_latest_state()
|
||||
if state and state["drone"]["mode"] == target_mode:
|
||||
self.logger.info("已进入 %s 模式", target_mode)
|
||||
break
|
||||
time.sleep(0.1)
|
||||
else:
|
||||
self.logger.error("模式切换超时")
|
||||
return False
|
||||
|
||||
self.logger.info("初始状态: mode=%s armed=%s alt_rel=%.1fm state_age=%.2fs",
|
||||
mode, armed, alt_rel, age_s)
|
||||
|
||||
|
||||
@@ -0,0 +1,282 @@
|
||||
import socket
|
||||
import struct
|
||||
import json
|
||||
import threading
|
||||
import time
|
||||
import zlib
|
||||
import hashlib
|
||||
import hmac
|
||||
|
||||
# 固定常量
|
||||
HEADER_MAGIC = 0xA5A5A5A5 # 消息头标记
|
||||
# < 表示小端字节序；I=4字节，B=1字节
|
||||
# 字段顺序: magic(4B), msg_id(4B), msg_type(1B), sub_pack(1B), res1(1B), res2(1B), data_len(4B)
|
||||
STRUCT_FORMAT = '<IIBBBB I'
|
||||
|
||||
|
||||
def pack_message(msg_id: int, json_data: dict) -> bytes:
|
||||
"""
|
||||
将 Python 字典打包成符合光电 IPC 协议的二进制 TCP 报文
|
||||
"""
|
||||
json_bytes = json.dumps(json_data).encode('utf-8')
|
||||
data_len = len(json_bytes)
|
||||
|
||||
# 组装 16 字节头部
|
||||
msg_type = 1 # 1：JSON格式文本数据
|
||||
sub_pack = 0 # 0表示不分包
|
||||
res1 = 0
|
||||
res2 = 0
|
||||
|
||||
header_bytes = struct.pack(
|
||||
STRUCT_FORMAT,
|
||||
HEADER_MAGIC,
|
||||
msg_id,
|
||||
msg_type,
|
||||
sub_pack,
|
||||
res1,
|
||||
res2,
|
||||
data_len
|
||||
)
|
||||
|
||||
# 计算从消息头开始到消息内容结束的 CRC32 校验码
|
||||
packet_payload = header_bytes + json_bytes
|
||||
crc32_val = zlib.crc32(packet_payload) & 0xffffffff
|
||||
crc_bytes = struct.pack('<I', crc32_val) # 4字节校验码
|
||||
|
||||
return packet_payload + crc_bytes
|
||||
|
||||
|
||||
def unpack_message(sock) -> tuple[int, dict] | None:
|
||||
"""
|
||||
从 socket 阻塞读取并解析一条完整的 TCP 报文
|
||||
"""
|
||||
try:
|
||||
# 1. 先读取 16 字节的固定头部
|
||||
header_bytes = b""
|
||||
while len(header_bytes) < 16:
|
||||
chunk = sock.recv(16 - len(header_bytes))
|
||||
if not chunk:
|
||||
return None # 连接断开
|
||||
header_bytes += chunk
|
||||
|
||||
# 2. 解析头部
|
||||
magic, msg_id, msg_type, sub_pack, res1, res2, data_len = struct.unpack(STRUCT_FORMAT, header_bytes)
|
||||
|
||||
if magic != HEADER_MAGIC:
|
||||
raise ValueError(f"不合法的消息头标记: {hex(magic)}")
|
||||
|
||||
# 3. 根据头部指明的数据长度，完整读取 JSON 文本
|
||||
json_bytes = b""
|
||||
while len(json_bytes) < data_len:
|
||||
chunk = sock.recv(data_len - len(json_bytes))
|
||||
if not chunk:
|
||||
return None
|
||||
json_bytes += chunk
|
||||
|
||||
# 4. 读取 4 字节的 CRC32 校验码
|
||||
crc_bytes = b""
|
||||
while len(crc_bytes) < 4:
|
||||
chunk = sock.recv(4 - len(crc_bytes))
|
||||
if not chunk:
|
||||
return None
|
||||
crc_bytes += chunk
|
||||
|
||||
# 5. 校验数值
|
||||
received_crc, = struct.unpack('<I', crc_bytes)
|
||||
local_crc = zlib.crc32(header_bytes + json_bytes) & 0xffffffff
|
||||
if received_crc != local_crc:
|
||||
print(f"[警告] CRC校验失败! 丢弃该包。收到: {received_crc}, 计算: {local_crc}")
|
||||
return None
|
||||
|
||||
# 6. 解析出字典格式的数据
|
||||
json_data = json.loads(json_bytes.decode('utf-8'))
|
||||
return msg_id, json_data
|
||||
except Exception as e:
|
||||
print(f"[网络] 读取数据时发生异常: {e}")
|
||||
return None
|
||||
|
||||
|
||||
def heartbeat_thread(sock, token_dict):
|
||||
"""每15秒发一次心跳包的子线程"""
|
||||
while True:
|
||||
time.sleep(15.0)
|
||||
if 'token' in token_dict:
|
||||
heartbeat_msg = {
|
||||
"cmd": "userOnlineHeart",
|
||||
"param": {"token": token_dict['token']}
|
||||
}
|
||||
# 【修复 Bug 1】：传入正确的 msg_id (1003) 和内容
|
||||
packet = pack_message(1003, heartbeat_msg)
|
||||
try:
|
||||
sock.sendall(packet)
|
||||
print("[心跳] 已发送保活包")
|
||||
except socket.error:
|
||||
print("[心跳] 发送失败，连接已断开")
|
||||
break
|
||||
|
||||
|
||||
def login_to_ipc(sock: socket.socket, username: str, password_raw: str) -> str | None:
|
||||
"""执行光电 IPC 的登录流程"""
|
||||
# 1. 发送获取 Salt 请求
|
||||
salt_request = {
|
||||
"cmd": "userSaltGet",
|
||||
"param": {
|
||||
"username": username
|
||||
}
|
||||
}
|
||||
sock.sendall(pack_message(msg_id=1001, json_data=salt_request))
|
||||
print(f"[登录] 已发送 Salt 获取请求...")
|
||||
|
||||
response = unpack_message(sock)
|
||||
if not response:
|
||||
print("[登录] 未收到 Salt 响应，连接断开")
|
||||
return None
|
||||
|
||||
_, salt_data = response
|
||||
param = salt_data.get("param", {})
|
||||
salt = param.get("salt", "").strip()
|
||||
login_enc = param.get("loginEnc") # 0: MD5, 1: HMAC-SHA256
|
||||
|
||||
print(f"[登录] 获取 Salt 成功: salt='{salt}', 加密类型={login_enc}")
|
||||
|
||||
# 2. 根据算法计算密码哈希值
|
||||
password_hash = ""
|
||||
if login_enc == 0:
|
||||
mix_str = password_raw + salt
|
||||
password_hash = hashlib.md5(mix_str.encode('utf-8')).hexdigest()
|
||||
print("[登录] 采用 MD5 加密")
|
||||
elif login_enc == 1:
|
||||
key = salt.encode('utf-8')
|
||||
msg = password_raw.encode('utf-8')
|
||||
password_hash = hmac.new(key, msg, hashlib.sha256).hexdigest()
|
||||
print("[登录] 采用 HMAC-SHA256 加密")
|
||||
else:
|
||||
print(f"[登录] 未知的加密类型: {login_enc}")
|
||||
return None
|
||||
|
||||
# 3. 发送用户登录请求
|
||||
login_request = {
|
||||
"cmd": "userLogin",
|
||||
"param": {
|
||||
"username": username,
|
||||
"password": password_hash
|
||||
}
|
||||
}
|
||||
sock.sendall(pack_message(msg_id=1002, json_data=login_request))
|
||||
print("[登录] 已发送用户登录验证请求...")
|
||||
|
||||
response = unpack_message(sock)
|
||||
if not response:
|
||||
print("[登录] 未收到登录响应")
|
||||
return None
|
||||
|
||||
_, login_result = response
|
||||
login_param = login_result.get("param", {})
|
||||
res_ack = login_param.get("ackvalue")
|
||||
|
||||
if res_ack == 100:
|
||||
token = login_param.get("token")
|
||||
print(f"[登录] 成功！获取到 Token: {token}")
|
||||
return token
|
||||
else:
|
||||
count = login_param.get("count", 0)
|
||||
lock_time = login_param.get("time", 0)
|
||||
print(f"[登录] 失败！错误码: {res_ack}, 剩余尝试次数: {count}, 锁定时间: {lock_time}秒")
|
||||
return None
|
||||
|
||||
|
||||
import math
|
||||
|
||||
def calculate_target_gps(ptz_lat, ptz_lon, ptz_alt, pan, tilt, distance):
|
||||
"""
|
||||
根据光电自身GPS、当前姿态角和距离，反推目标WGS84经纬度坐标
|
||||
:param ptz_lat: 光电自身纬度 (度, 例如 31.2304)
|
||||
:param ptz_lon: 光电自身经度 (度, 例如 121.4737)
|
||||
:param ptz_alt: 光电自身海拔高度 (米)
|
||||
:param pan: 绝对水平角 (度, 0为正北, 顺时针)
|
||||
:param tilt: 绝对俯仰角 (度, 水平为0, 仰角为正, 俯角为负)
|
||||
:param distance:目标斜距 (米)
|
||||
:return: (目标经度, 目标纬度, 目标高度)
|
||||
"""
|
||||
if distance <= 0:
|
||||
return ptz_lon, ptz_lat, ptz_alt
|
||||
|
||||
# 1. 弧度转换
|
||||
pan_rad = math.radians(pan)
|
||||
tilt_rad = math.radians(tilt)
|
||||
|
||||
# 2. 计算相对光电的水平投影距离和垂直高度差
|
||||
horizontal_dist = distance * math.cos(tilt_rad) # 地面投影距离
|
||||
delta_alt = distance * math.sin(tilt_rad) # 高度差
|
||||
|
||||
# 3. 在东北天(ENU)坐标系下的米级位移
|
||||
east = horizontal_dist * math.sin(pan_rad) # 偏东距离
|
||||
north = horizontal_dist * math.cos(pan_rad) # 偏北距离
|
||||
|
||||
# 4. WGS84地球椭球体精确参数（消除地球曲率导致的经纬度偏离）
|
||||
a = 6378137.0 # 赤道半径
|
||||
b = 6356752.3142 # 极半径
|
||||
e2 = 1 - (b ** 2 / a ** 2) # 第一偏心率平方
|
||||
|
||||
lat_rad = math.radians(ptz_lat)
|
||||
|
||||
# 计算当前纬度下的子午圈和卯酉圈曲率半径
|
||||
num = 1 - e2 * (math.sin(lat_rad) ** 2)
|
||||
radius_v = a / math.sqrt(num) # 卯酉圈
|
||||
radius_m = a * (1 - e2) / (num ** 1.5) # 子午圈
|
||||
|
||||
# 5. 换算为经纬度变化量
|
||||
delta_lat = (north / radius_m) * (180.0 / math.pi)
|
||||
delta_lon = (east / (radius_v * math.cos(lat_rad))) * (180.0 / math.pi)
|
||||
|
||||
# 6. 计算最终目标坐标
|
||||
target_lat = ptz_lat + delta_lat
|
||||
target_lon = ptz_lon + delta_lon
|
||||
target_alt = ptz_alt + delta_alt
|
||||
|
||||
return round(target_lon, 6), round(target_lat, 6), round(target_alt, 1)
|
||||
|
||||
def main():
|
||||
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
|
||||
# 实际测试时请修改为光电外设的正确 IP
|
||||
sock.connect(('192.168.1.64', 39020))
|
||||
|
||||
# 【修复 Bug 2】：传入文档推测的默认密码 'Abc.12345'
|
||||
token = login_to_ipc(sock, 'admin', 'Abc.12345')
|
||||
|
||||
if not token:
|
||||
print("登录失败")
|
||||
sock.close()
|
||||
return
|
||||
|
||||
token_dict = {"token": token}
|
||||
t = threading.Thread(target=heartbeat_thread, args=(sock, token_dict), daemon=True)
|
||||
t.start()
|
||||
|
||||
print("\n[系统] 开始实时监听光电回传状态...")
|
||||
while True:
|
||||
# 【修复 Bug 3】：删除了 time.sleep(1)，交由 unpack_message 内部阻塞控制
|
||||
res = unpack_message(sock)
|
||||
if not res:
|
||||
print("[系统] 与光电断开连接")
|
||||
break
|
||||
|
||||
msg_id, json_data = res
|
||||
cmd = json_data.get("cmd")
|
||||
param = json_data.get("param", {})
|
||||
|
||||
# 4. 在这里动态捕获你想要的数据并打印出来
|
||||
if cmd == "PTZStatusReport":
|
||||
print(
|
||||
f"[云台数据] 水平角: {param.get('pan')}, 俯仰角: {param.get('tilt')}, 当前视场(倍率): {param.get('camView')}")
|
||||
elif cmd == "trackingReport":
|
||||
status_map = {0: "未跟踪", 1: "跟踪正常 ✅", 3: "目标失锁 ⚠️", 4: "目标丢失 ❌"}
|
||||
status_code = param.get("status", 0)
|
||||
print(
|
||||
f"[跟踪数据] 状态: {status_map.get(status_code, '未知')}, 目标距离: {param.get('trackDistance')}m, 脱靶量H: {param.get('offsetH')}")
|
||||
elif cmd == "ivpReport":
|
||||
print(f"[AI识别数据] 画面中检测到目标数量: {len(param.get('targets', []))}")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -1,20 +0,0 @@
|
||||
import cv2
|
||||
import os
|
||||
|
||||
os.environ['OPENCV_FFMPEG_CAPTURE_OPTIONS'] = 'rtsp_transport;tcp'
|
||||
|
||||
# 改用正确的地址
|
||||
rtsp_url = "rtsp://192.168.144.25:8554/main.264"
|
||||
|
||||
print(f"[测试] 正在尝试连接: {rtsp_url}")
|
||||
cap = cv2.VideoCapture(rtsp_url, cv2.CAP_FFMPEG)
|
||||
|
||||
if not cap.isOpened():
|
||||
print("[结果] 无法打开视频流。")
|
||||
else:
|
||||
ret, frame = cap.read()
|
||||
if not ret or frame is None:
|
||||
print("[结果] 读取帧失败，视频流可能为空。")
|
||||
else:
|
||||
print(f"[结果] 成功读取第一帧！图像尺寸为: {frame.shape}")
|
||||
cap.release()
|
||||
Reference in New Issue
Block a user