新增pod_bridge吊舱桥接中间件(树莓派4B部署)

- pod_bridge: 协议G版编解码/地理位置引导律/PX4网关/串口链路/闭环主循环 - 含三项安全修复: position_time_ms拆分、表2-2锁定门控、run()异常护栏 - 测试92条全绿; 附树莓派4B(Ubuntu 24.04)部署注意事项与最小依赖清单
2026-09-23 14:38:23 +08:00
parent 3a8e2c7f4b
commit 477ad5f5de
17 changed files with 2758 additions and 0 deletions
@@ -0,0 +1,204 @@
+"""guidance.py —— 地理位置引导律：载机位置 + 目标经纬度 → offboard 速度/航向。
+
+吊舱（表4）给出目标经纬度，本律计算到目标的方位角/距离，生成"朝目标飞"的
+控制意图，输出复用 visual_track_control.types.ControlOutput，可直接喂现有
+MavsdkCommandGateway（速率式分支：vx_body_mps 前向 + yaw_rate_dps 偏航）。
+
+与 visual_track_control.TrackingControlLaw 的区别：后者由像素视线角驱动（视觉
+伺服），本律由绝对地理坐标驱动（航位推算）。复用其纯算法构件（PID / 限幅 /
+超龄判定），故 pod_bridge 依赖 visual_track_control 作为控制原语库。
+
+控制策略（速率式，避开"绝对航向为正北 0° 被网关误判为无航向指令"的歧义）：
+  * 方位误差 err = wrap180(目标方位 − 当前航向) → 偏航角速度（比例 + 变化率限幅）
+  * 距离误差 = 地面距离 − 停止半径 → 前向基准速度（PID + 限幅）
+  * 对准门控：前向 = 基准速度 · max(0, cos(err))，未对准时先原地转向再前进
+  * 垂直：高度保持（vz = 0）；高度逼近策略待后续 config 化
+  * 到达停止半径 / 目标或载机位置不可用 / 超龄 → 零速刹车（BRAKE_HOVER）
+"""
+
+import logging
+import math
+from dataclasses import dataclass
+
+from visual_track_control.pid_controller import PidController, PidParams
+from visual_track_control.rate_limiter import SlewRateLimiter, clamp
+from visual_track_control.staleness_checker import is_stale
+from visual_track_control.types import ControlMode, ControlOutput
+
+logger = logging.getLogger(__name__)
+
+_K_DEG_TO_RAD = math.pi / 180.0
+_K_RAD_TO_DEG = 180.0 / math.pi
+_K_EARTH_RADIUS_M = 6371008.8  # WGS84 平均半径
+_K_MAX_DT_S = 0.5              # 单步 dt 上限（秒）：异常节拍保护
+
+
+# --- 大地测量（纯函数）-----------------------------------------------------
+def haversine_distance_m(lat1: float, lon1: float, lat2: float, lon2: float) -> float:
+    """两点地面距离（米，haversine 大圆距离）。"""
+    phi1 = lat1 * _K_DEG_TO_RAD
+    phi2 = lat2 * _K_DEG_TO_RAD
+    dphi = (lat2 - lat1) * _K_DEG_TO_RAD
+    dlmb = (lon2 - lon1) * _K_DEG_TO_RAD
+    a = (math.sin(dphi / 2.0) ** 2
+         + math.cos(phi1) * math.cos(phi2) * math.sin(dlmb / 2.0) ** 2)
+    return 2.0 * _K_EARTH_RADIUS_M * math.asin(min(1.0, math.sqrt(a)))
+
+
+def initial_bearing_deg(lat1: float, lon1: float, lat2: float, lon2: float) -> float:
+    """从点 1 指向点 2 的初始方位角（度，正北为 0，顺时针 [0, 360)）。"""
+    phi1 = lat1 * _K_DEG_TO_RAD
+    phi2 = lat2 * _K_DEG_TO_RAD
+    dlmb = (lon2 - lon1) * _K_DEG_TO_RAD
+    x = math.sin(dlmb) * math.cos(phi2)
+    y = (math.cos(phi1) * math.sin(phi2)
+         - math.sin(phi1) * math.cos(phi2) * math.cos(dlmb))
+    return (math.atan2(x, y) * _K_RAD_TO_DEG) % 360.0
+
+
+def _wrap_deg_180(deg: float) -> float:
+    """角度回绕到 [-180, 180)。"""
+    d = math.fmod(deg + 180.0, 360.0)
+    if d < 0.0:
+        d += 360.0
+    return d - 180.0
+
+
+# --- 配置 / 输入 ------------------------------------------------------------
+@dataclass
+class GuidanceConfig:
+    """引导律参数（全部可调，后续 config.py 从 JSON 加载并校验）。"""
+
+    frequency_hz: float = 10.0
+    target_stale_ms: int = 1000      # 目标经纬度超龄门限
+    position_stale_ms: int = 1000    # 载机位置超龄门限
+    stop_radius_m: float = 5.0       # 到达此半径内悬停（不再前进）
+    max_speed_mps: float = 5.0       # 前向速度上限
+    speed_kp: float = 0.5            # 距离比例增益（mps / m）
+    speed_ki: float = 0.0
+    speed_kd: float = 0.0
+    speed_integral_limit: float = 0.0
+    speed_derivative_filter_coef: float = 0.0
+    speed_slew_mps2: float = 2.0     # 前向加速度限幅
+    max_yaw_rate_dps: float = 30.0   # 偏航角速度上限
+    yaw_kp: float = 1.0              # 方位误差比例增益（dps / deg）
+    yaw_slew_dps2: float = 60.0      # 偏航角速度变化率限幅
+
+
+@dataclass
+class GuidanceInput:
+    """单周期引导输入：载机状态（PX4 遥测）+ 目标（吊舱表4），各带取样时间戳。"""
+
+    own_lat_deg: float = 0.0
+    own_lon_deg: float = 0.0
+    own_yaw_deg: float = 0.0          # 当前航向（NED，度，正北 0 顺时针）
+    own_position_valid: bool = False  # GPS 定位/位置是否可用
+    own_position_time_ms: int = 0
+
+    target_lat_deg: float = 0.0
+    target_lon_deg: float = 0.0
+    target_valid: bool = False        # 表4 目标经纬度是否有效
+    target_time_ms: int = 0
+
+
+# --- 引导律 -----------------------------------------------------------------
+class GuidanceLaw:
+    """地理位置引导主控律。update() 单周期返回 ControlOutput。
+
+    诊断量（last_distance_m / last_bearing_deg / last_heading_err_deg）每周期
+    更新，供桥接层日志使用；刹车/降级时为最近一次有效计算值或 NaN。
+    """
+
+    def __init__(self, cfg: GuidanceConfig) -> None:
+        self._cfg = cfg
+        self._speed_pid = PidController(PidParams(
+            kp=cfg.speed_kp, ki=cfg.speed_ki, kd=cfg.speed_kd,
+            integral_limit=cfg.speed_integral_limit,
+            derivative_filter_coef=cfg.speed_derivative_filter_coef,
+            output_limit=cfg.max_speed_mps,
+        ))
+        self._speed_limiter = SlewRateLimiter(cfg.speed_slew_mps2)
+        self._yaw_limiter = SlewRateLimiter(cfg.yaw_slew_dps2)
+        self._last_now_ms = 0
+        self._last_mode = ControlMode.BRAKE_HOVER
+        self.last_distance_m = float("nan")
+        self.last_bearing_deg = float("nan")
+        self.last_heading_err_deg = float("nan")
+
+    def update(self, inp: GuidanceInput, now_ms: int) -> ControlOutput:
+        dt_s = self._advance_dt(now_ms)
+        out = ControlOutput()
+
+        # ---- 可用性 / 超龄判定 ----
+        position_ok = (inp.own_position_valid
+                       and not is_stale(inp.own_position_time_ms, now_ms,
+                                        self._cfg.position_stale_ms))
+        target_ok = (inp.target_valid
+                     and not is_stale(inp.target_time_ms, now_ms,
+                                      self._cfg.target_stale_ms))
+        if not position_ok or not target_ok:
+            return self._brake(out, "载机位置或目标不可用/超龄，零速刹车")
+
+        distance = haversine_distance_m(inp.own_lat_deg, inp.own_lon_deg,
+                                        inp.target_lat_deg, inp.target_lon_deg)
+        bearing = initial_bearing_deg(inp.own_lat_deg, inp.own_lon_deg,
+                                      inp.target_lat_deg, inp.target_lon_deg)
+        heading_err = _wrap_deg_180(bearing - inp.own_yaw_deg)
+        self.last_distance_m = distance
+        self.last_bearing_deg = bearing
+        self.last_heading_err_deg = heading_err
+
+        # ---- 到达停止半径 → 悬停 ----
+        if distance <= self._cfg.stop_radius_m:
+            return self._brake(out, "已到达停止半径，悬停")
+
+        # ---- 偏航角速度（比例 + 变化率限幅 + 上限）----
+        target_rate = clamp(self._cfg.yaw_kp * heading_err,
+                            -self._cfg.max_yaw_rate_dps, self._cfg.max_yaw_rate_dps)
+        out.yaw_rate_dps = self._yaw_limiter.limit(target_rate, dt_s)
+
+        # ---- 前向速度（距离 PID + 对准门控 + 加速度限幅 + 上限）----
+        base_speed = clamp(self._speed_pid.update(distance - self._cfg.stop_radius_m,
+                                                  dt_s),
+                           0.0, self._cfg.max_speed_mps)
+        alignment = max(0.0, math.cos(heading_err * _K_DEG_TO_RAD))
+        out.vx_body_mps = self._speed_limiter.limit(base_speed * alignment, dt_s)
+
+        out.vz_mps = 0.0  # 高度保持
+        out.mode = ControlMode.VELOCITY_HEADING
+        out.valid = True
+        return self._finish_and_log(out, "朝目标引导")
+
+    def reset(self) -> None:
+        """停止闭环时清全部内部状态。"""
+        self._speed_pid.reset()
+        self._speed_limiter.reset()
+        self._yaw_limiter.reset()
+        self._last_now_ms = 0
+        self._last_mode = ControlMode.BRAKE_HOVER
+
+    def _advance_dt(self, now_ms: int) -> float:
+        """节拍推算：异常节拍/时钟回拨用配置频率兜底，单步上限 0.5s。"""
+        dt_s = 1.0 / self._cfg.frequency_hz
+        if self._last_now_ms > 0 and now_ms > self._last_now_ms:
+            dt_s = min((now_ms - self._last_now_ms) / 1000.0, _K_MAX_DT_S)
+        self._last_now_ms = now_ms
+        return dt_s
+
+    def _brake(self, out: ControlOutput, reason: str) -> ControlOutput:
+        """零速刹车：进入时清积分与限幅器历史，恢复引导后指令从零渐变。"""
+        if self._last_mode is not ControlMode.BRAKE_HOVER:
+            self._speed_pid.reset()
+            self._speed_limiter.reset()
+            self._yaw_limiter.reset()
+        out.mode = ControlMode.BRAKE_HOVER
+        out.valid = True  # 速度分量保持默认全零
+        return self._finish_and_log(out, reason)
+
+    def _finish_and_log(self, out: ControlOutput, reason: str) -> ControlOutput:
+        """仅模式变化时打印状态切换日志（热路径零日志）。"""
+        if out.mode is not self._last_mode:
+            logger.info("[Guidance] 状态切换: %s -> %s (%s)",
+                        self._last_mode.name, out.mode.name, reason)
+            self._last_mode = out.mode
+        return out