精简仓库结构并增强任务链路:归档双目可选后端、新增LOS判据与自定义返航
归档 legacy/:stereo_yolo/stereo_ranger/target_tracker 三包 + 175 条测试 + demo/配置/权重/三份规格;运行链路不再依赖,默认 pytest 446 条,归档测试按需运行。 目标有效性第二判据改为 LOS 运动学一致性(pod_bridge/los_consistency.py),删除 require_vision;pod_locked() 叠加该判据,状态机随之后退重搜。 自定义返航(task_runner/return_home.py):保持转场高度回起飞点 → 恢复初始航向 → 垂直降落,不再爬 RTL_RETURN_ALT;未起飞/无起飞点/超时/异常仍退回 PX4 原生 RTL。 任务链安全兜底:主循环异常→紧急返航、stop 幂等、landing_stop 保留遥测、in_air 流复用+超时、解锁/起飞与吊舱识别仅执行一次、rtl_timeout 上限。 入口共享件拆到 utils/sitl_support.py;pod_sitl_test.py→l1_sitl_test.py;删除旧架构 test.py;相机脚本移入 tools/。 新增 .gitignore 并剔除历史误跟踪的 59 个 .pyc;requirements 按实际依赖重写;文档全面对齐,新增 legacy/README.md 与 tools/README.md。
This commit is contained in:
@@ -3,6 +3,8 @@
|
||||
职责(桥接的飞控侧):
|
||||
* 订阅 PX4 遥测(position / attitude_euler / gps_info / velocity_ned),缓存为
|
||||
OwnState 快照,供桥接层构造 表3(载机姿态下发) 与 GuidanceInput(引导输入);
|
||||
* 订阅飞控状态文本 status_text()(STATUSTEXT),缓存供任务日志/地面站显示
|
||||
(飞控警告/异常信息);
|
||||
* 复用 visual_track_control.MavsdkCommandGateway 发送 offboard 速度设定点
|
||||
(不重造三分支映射,直接喂 GuidanceLaw 的 ControlOutput)。
|
||||
|
||||
@@ -15,6 +17,7 @@ import asyncio
|
||||
import logging
|
||||
import math
|
||||
import time
|
||||
from collections import deque
|
||||
from dataclasses import dataclass, replace
|
||||
from typing import Any, Callable, Optional
|
||||
|
||||
@@ -23,6 +26,7 @@ from visual_track_control.mavsdk_gateway import MavsdkCommandGateway
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
_GPS_FIX_3D = 3 # mavsdk GpsFixType.THREE_D_FIX 起视为定位有效
|
||||
_FC_MSG_BUFFER = 64 # 飞控状态文本缓存上限(低频流,仅供日志消费)
|
||||
|
||||
|
||||
def _enum_int(value: Any) -> int:
|
||||
@@ -69,6 +73,7 @@ class Px4Gateway:
|
||||
self._now_ms = now_ms_fn or (lambda: int(time.monotonic() * 1000))
|
||||
self._state = OwnState()
|
||||
self._tasks: list = []
|
||||
self._fc_msgs: deque = deque(maxlen=_FC_MSG_BUFFER)
|
||||
|
||||
# --- 连接(实机)-------------------------------------------------------
|
||||
@classmethod
|
||||
@@ -123,13 +128,25 @@ class Px4Gateway:
|
||||
"""返回当前缓存的快照副本(调用方修改不影响内部状态)。"""
|
||||
return replace(self._state)
|
||||
|
||||
def update_fc_status(self, st: Any) -> None:
|
||||
"""缓存一条飞控状态文本(STATUSTEXT);severity 取枚举名(如 WARNING)。"""
|
||||
sev = getattr(st, "type", None)
|
||||
sev_name = getattr(sev, "name", None) or str(sev or "UNKNOWN")
|
||||
self._fc_msgs.append((sev_name, str(getattr(st, "text", ""))))
|
||||
|
||||
def take_fc_messages(self) -> list:
|
||||
"""取走并清空飞控状态文本(供任务日志落盘;无则返回 [])。"""
|
||||
msgs = list(self._fc_msgs)
|
||||
self._fc_msgs.clear()
|
||||
return msgs
|
||||
|
||||
def _recompute_valid(self) -> None:
|
||||
self._state.position_valid = (self._state.has_position
|
||||
and self._state.fix_type >= _GPS_FIX_3D)
|
||||
|
||||
# --- 遥测订阅(异步胶水)----------------------------------------------
|
||||
async def start_telemetry(self) -> None:
|
||||
"""订阅四路遥测流,后台任务持续刷新缓存。幂等:已订阅则直接返回。"""
|
||||
"""订阅遥测流 + 飞控状态文本,后台任务持续刷新缓存。幂等:已订阅则直接返回。"""
|
||||
if self._tasks:
|
||||
return # 已订阅,避免重复起任务(runner 会先订阅,bridge.start 再触发时跳过)
|
||||
t = self._drone.telemetry
|
||||
@@ -140,6 +157,8 @@ class Px4Gateway:
|
||||
asyncio.ensure_future(self._consume(t.gps_info(), self.update_gps)),
|
||||
asyncio.ensure_future(
|
||||
self._consume(t.velocity_ned(), self.update_velocity_ned)),
|
||||
asyncio.ensure_future(
|
||||
self._consume(t.status_text(), self.update_fc_status)),
|
||||
]
|
||||
logger.info("[Px4Gateway] 遥测订阅已启动")
|
||||
|
||||
@@ -171,5 +190,9 @@ class Px4Gateway:
|
||||
async def send_brake_hover(self, time_boot_ms: Optional[int] = None) -> bool:
|
||||
return await self._cmd.send_brake_hover(time_boot_ms)
|
||||
|
||||
async def send_yaw_rate(self, yaw_rate_dps: float) -> bool:
|
||||
"""原地偏航扫视(SEARCH 态):零速 + 指定角速度,保持 offboard 心跳。"""
|
||||
return await self._cmd.send_yaw_rate(yaw_rate_dps)
|
||||
|
||||
async def stop_offboard(self) -> None:
|
||||
await self._cmd.stop()
|
||||
|
||||
Reference in New Issue
Block a user