From cdb9815ec85998fe8657ff6660d27dc6a037e41f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E4=B8=80=E5=96=B5?= Date: Tue, 16 Jun 2026 18:06:38 +0800 Subject: [PATCH] =?UTF-8?q?2026.6.16=E5=B0=8F=E6=9B=B4=E6=96=B0=E4=B8=8E?= =?UTF-8?q?=E5=85=89=E7=94=B5=E5=8D=8F=E8=B0=83=E5=AE=A2=E6=88=B7=E7=AB=AF?= =?UTF-8?q?=E7=9A=84=E5=88=9D=E7=89=88?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- README.md | 2 +- infer_camera_modular.py | 21 --- transform_app.py | 282 ++++++++++++++++++++++++++++++++++++++++ videotest.py | 20 --- 4 files changed, 283 insertions(+), 42 deletions(-) create mode 100644 transform_app.py delete mode 100644 videotest.py diff --git a/README.md b/README.md index 71f0e65..a0935a1 100644 --- a/README.md +++ b/README.md @@ -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 diff --git a/infer_camera_modular.py b/infer_camera_modular.py index a630bb7..6b0aaad 100644 --- a/infer_camera_modular.py +++ b/infer_camera_modular.py @@ -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) diff --git a/transform_app.py b/transform_app.py new file mode 100644 index 0000000..49175c0 --- /dev/null +++ b/transform_app.py @@ -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 = ' 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(' 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(' 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() \ No newline at end of file diff --git a/videotest.py b/videotest.py deleted file mode 100644 index ab54a20..0000000 --- a/videotest.py +++ /dev/null @@ -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() \ No newline at end of file