keita
ded17d7995
- utils/task_runner: 6态状态机(WAIT→ARM_TAKEOFF→GOTO(L2)→TRACK(L1)→APPROACH→RTL) + 编排器(快照采集/状态动作) + 双层日志(事件JSONL/遥测CSV) + 吊舱控制指令(0x07/0x0F/0x0E/0x08) - pod_bridge: ground_link 地面坐标UDP注入(超龄判无效,无激光替代表4); set_heading_servo_enabled() 运行时L1/L2切换(切换即reset清积分防输出跳变); pod_locked 提为公共门控; config 新增 ground_inject 段 - virtual_pod 模拟吊舱(无电机语义) + verify_px4 增量④仅飞控验证 + pod_sitl_test L1 SITL全流程(飞行编排不塞进 run_bridge, 保持纯offboard闭环入口) - 入口: task_runner_sitl(VirtualPod+虚拟目标, SITL端到端跑通约1m24s) / task_runner_real(真串口+UDP注入+吊舱控制) - 地面站 ground_station: 坐标注入 + 状态可视化(Flask) - S1标定回填(2026-10-09): beta_sign=+1 / alpha_sign=-1, 加标定回归测试守护 - 新增测试: task_runner x4 + ground_link/virtual_pod*/bridge/px4_gateway 增量; 全量 483 passed - 文档: 预定流程(需求) + 预定流程-操作指南(操作/待办) + L1方案补S1结论与实现状态
144 lines
5.2 KiB
Python
144 lines
5.2 KiB
Python
"""virtual_pod.py —— 模拟吊舱（SITL/台架工具，非飞行组件；类比 sim_fc.py 模拟飞控）。
|
||
|
||
按"无电机吊舱"的实测语义合成 表2-2 遥测，实现 PodLink 接口可直插 PodBridge：
|
||
* 相机刚性固连机体朝前 → 框架角(frame_roll/pitch/yaw)恒 0；
|
||
* 无激光 → laser_range_m 恒 None；
|
||
* off_target 按目标相对机体的视线角合成，符号与 S1 台架标定一致：
|
||
目标偏右 → off_target_x_deg 为正（右正）
|
||
目标偏下 → off_target_y_deg 为负（上正）
|
||
* 目标不可见（身后 / 超视场 / 被 set_visible(False)）→ 状态字节1 bit2=0（搜索态）
|
||
且脱靶量为 0（对齐真实吊舱"丢失时脱靶量恒 0、锁定位不复位"的坑）。
|
||
|
||
载机状态由注入的 own_state_fn 提供（CarrierView：NED 位置 + 航向），与 PodBridge
|
||
解耦——SITL 脚本注入遥测 hub，单测注入固定值。
|
||
"""
|
||
|
||
import math
|
||
import time
|
||
from dataclasses import dataclass
|
||
|
||
from .protocol import Telemetry
|
||
|
||
|
||
@dataclass
|
||
class CarrierView:
|
||
"""虚拟吊舱所需的载机视角状态（NED 位置 + 航向）。"""
|
||
|
||
north_m: float = 0.0
|
||
east_m: float = 0.0
|
||
down_m: float = 0.0
|
||
yaw_rad: float = 0.0
|
||
|
||
|
||
@dataclass
|
||
class VirtualTargetConfig:
|
||
"""虚拟目标放置与运动参数（锚点机体系偏移 + 水平圆弧）。"""
|
||
|
||
ahead_m: float = 30.0
|
||
right_m: float = 15.0
|
||
down_m: float = 0.0
|
||
arc_radius_m: float = 12.0
|
||
arc_speed_mps: float = 1.0
|
||
arc_ccw: bool = True
|
||
|
||
|
||
class VirtualTarget:
|
||
"""虚拟目标：按锚点机头朝向放置，随后在 NED 水平面做匀速圆弧运动。
|
||
|
||
几何与 test.py 的 VirtualTarget 一致（去掉了针孔投影部分——吊舱路径
|
||
直接给角度，不需要检测框）。
|
||
"""
|
||
|
||
def __init__(self, cfg: VirtualTargetConfig,
|
||
anchor_ned: tuple[float, float, float],
|
||
anchor_yaw_rad: float) -> None:
|
||
cos_y = math.cos(anchor_yaw_rad)
|
||
sin_y = math.sin(anchor_yaw_rad)
|
||
|
||
self._start_n = anchor_ned[0] + cfg.ahead_m * cos_y - cfg.right_m * sin_y
|
||
self._start_e = anchor_ned[1] + cfg.ahead_m * sin_y + cfg.right_m * cos_y
|
||
self._start_d = anchor_ned[2] + cfg.down_m
|
||
|
||
self._radius_m = max(0.1, float(cfg.arc_radius_m))
|
||
self._omega = (float(cfg.arc_speed_mps) / self._radius_m
|
||
* (1.0 if cfg.arc_ccw else -1.0))
|
||
|
||
right_n = -sin_y
|
||
right_e = cos_y
|
||
self._center_n = self._start_n + self._radius_m * right_n
|
||
self._center_e = self._start_e + self._radius_m * right_e
|
||
self._theta0 = math.atan2(self._start_e - self._center_e,
|
||
self._start_n - self._center_n)
|
||
|
||
self._pos_n = self._start_n
|
||
self._pos_e = self._start_e
|
||
self._pos_d = self._start_d
|
||
self._t0 = time.monotonic()
|
||
self._visible = True
|
||
|
||
@property
|
||
def visible(self) -> bool:
|
||
return self._visible
|
||
|
||
def set_visible(self, visible: bool) -> None:
|
||
self._visible = visible
|
||
|
||
def update_position(self) -> None:
|
||
elapsed = max(0.0, time.monotonic() - self._t0)
|
||
theta = self._theta0 + self._omega * elapsed
|
||
self._pos_n = self._center_n + self._radius_m * math.cos(theta)
|
||
self._pos_e = self._center_e + self._radius_m * math.sin(theta)
|
||
self._pos_d = self._start_d
|
||
|
||
@property
|
||
def pos_ned(self) -> tuple[float, float, float]:
|
||
return self._pos_n, self._pos_e, self._pos_d
|
||
|
||
|
||
class VirtualPod:
|
||
"""模拟吊舱：poll() 按当前载机视角合成一帧 表2-2 遥测（锁定/搜索态）。"""
|
||
|
||
def __init__(self, target: VirtualTarget, own_state_fn,
|
||
fov_h_deg: float = 60.0, fov_v_deg: float = 35.0) -> None:
|
||
self._target = target
|
||
self._own = own_state_fn
|
||
self._fov_h = fov_h_deg
|
||
self._fov_v = fov_v_deg
|
||
self.attitude_frames_rx = 0 # 收到的表3 帧数（诊断）
|
||
self.closed = False
|
||
|
||
def poll(self) -> list:
|
||
self._target.update_position()
|
||
own = self._own()
|
||
n, e, d = self._target.pos_ned
|
||
rel_n = n - own.north_m
|
||
rel_e = e - own.east_m
|
||
rel_d = d - own.down_m
|
||
cos_y = math.cos(own.yaw_rad)
|
||
sin_y = math.sin(own.yaw_rad)
|
||
fwd = rel_n * cos_y + rel_e * sin_y
|
||
right = -rel_n * sin_y + rel_e * cos_y
|
||
|
||
off_x = math.degrees(math.atan2(right, fwd)) # 右正
|
||
off_y = math.degrees(math.atan2(-rel_d, fwd)) # 上正（S1 标定）
|
||
locked = (self._target.visible
|
||
and fwd > 0.5
|
||
and abs(off_x) < self._fov_h / 2.0
|
||
and abs(off_y) < self._fov_v / 2.0)
|
||
|
||
return [Telemetry(
|
||
status1=0b00000100 if locked else 0, # bit2 锁定 / 搜索
|
||
off_target_x_deg=off_x if locked else 0.0,
|
||
off_target_y_deg=off_y if locked else 0.0,
|
||
frame_roll_deg=0.0, # 无电机：框架角恒 0
|
||
frame_pitch_deg=0.0,
|
||
frame_yaw_deg=0.0,
|
||
laser_range_m=None, # 无激光：恒无效
|
||
)]
|
||
|
||
def send(self, data: bytes) -> None:
|
||
self.attitude_frames_rx += 1 # 表3 载机姿态下发：计数即可，不解析
|
||
|
||
def close(self) -> None:
|
||
self.closed = True
|