04.5 最后一公里：时间契约、监督状态机与 MAVLink 接收语义

零基础入口：收到数字，不等于知道它是什么意思

协议是发送和接收双方约定的字段、编码、单位及解释规则；消息是一次传输的数据；模式决定接收端当前运行哪套控制逻辑；状态机则用有限几个状态和转移条件描述任务怎样进行。模式名称不是装饰：同样的数值 $0.3$ ，可以代表 m/s，也可以是无量纲操纵比例，不能只复制数字。

掩码 mask 是一排开关，不是一种速度。 二进制每位只能是 0 或 1，零起始第 $k$ 位权重为 $2^k$ 。例如第 0 位和第 2 位置一，数值是 $1+4=5$ ；检查第三个位是否开，可在 C++ 写成 mask & (1u << 2)。左移生成单个开关，按位与只保留共同为一的位置。十六进制的每一位表示四个二进制位，前缀 0x 告诉编译器按十六进制读数。

本页目标消息的掩码多数开关是忽略开关：置一表示不用此字段，置零才保留。因而“想要速度”不是把速度三位置一，而是把位置和加速度等不用的字段置一、速度位留零。第 9 位另有 force 解释语义，不能把所有位都概括成忽略。下面逐位拆开 0x0DC7，避免背一个无法解释的常数。

任务状态和飞控模式必须分开问：任务说“正在环绕”，不证明飞控已进入允许该参考的模式。一次转移至少经过“撤销旧控制权→初始化新任务→确认模式遥测→发送相容目标”。发送模式切换请求只是请求；确认状态需读取反馈。人工接管则应先撤销自主权，不能让下一帧好图像立即抢回来。

再区分三种事件：有效零速度是“现在请不要有该速度”；消息过期是“这个旧意图已经不能信”；任务完成是“任务层确认目标达成”。它们可能触发不同飞控分支，不能以停止发包代替完成，也不能假设发零必定等于位置保持。下文把本项目的分支语义单独写清，不推广到所有固件。

对应练习： CRC-16 字节帧校验只检查传输字节是否与校验约定一致。CRC 不验证单位、模式、时间戳或授权；一条校验正确的旧命令仍必须被拒绝。先做字节层练习，再用本页 C++ 测试重复、过期与人工接管。

到这里已经能算出方向、速度和偏航，但一条数值正确的命令仍可能因过期、坐标错误、模式错误或控制权竞争而产生错误动作。本课把整条链闭合：曝光与估计时间 → 任务状态 → 唯一控制者 → 带期限的参考 → 飞控接收分支。输入不只有命令数值，还包括来源、时间、有效性和模式；输出必须同时说明运动目标和失效后的行为。

时间与命令变量

符号	含义	维数 / 单位	坐标或约定
$t_c,t_g,t_r,t_a$	采样、生成、接收、作用时刻	标量 / s	映射到同一单调时基
$\tau,\tau_{\max},T_{\rm ttl}$	总年龄、允许图像年龄、命令有效期	标量 / s	TTL 是持续时间
$t_{\rm exp}$	到期时刻	标量 / s	接收端单调时钟
$\omega_c,\phi_{\rm lag}$	闭环角频率、延迟附加相位	rad/s；rad	频域解释，非相机角速率
$v_N,v_E,v_D$	北东下速度参考	各为标量 / m/s	LOCAL_NED
session、seq、owner	会话、业务序号、租约拥有者	离散字段 / 无量纲	与链路包序号分开
type_mask	选择忽略的目标字段	uint16 / 位集合	置一表示忽略对应字段

时间从采样一直算到作用

\begin{aligned}\tau&=t_a-t_c\\&=(t_g-t_c)+(t_r-t_g)+(t_a-t_r),\\e^{-s\tau}\big|_{s=j\omega_c}&=e^{-j\omega_c\tau},\qquad\phi_{\rm lag}=-\omega_c\tau.\end{aligned}

(4.21)

第一项包含采集、排队和推理，第二项是传输，第三项是桥接、飞控调度与执行响应。检测器每秒输出很多结果，不表示它处理的是新画面；队列积压会让帧率稳定而年龄持续增长。频域式来自时间延迟的拉普拉斯变换：延迟不改变纯正弦幅值，却消耗相位裕量。若闭环关注 2 rad/s 的运动，延迟 0.25 s 就增加约负 28.65° 相位；滤波又会增加额外滞后，提高比例增益并不能消除它。

跨设备的两个开机计时器没有共同零点，不能直接相减。可维护带误差界的时钟偏移映射，或让发布端携带采样到发布的年龄，并由接收端添加可保证的传输年龄界。接收时再记一个本地单调时间，用它判断接收后的老化。MAVLink TIMESYNC 提供同步消息机制，TB04-S8；真正的契约还需说明剩余同步误差，而不是出现一个 time 字段就视为已经同步。

\begin{aligned}t_{\rm exp}&=\min(t_g+T_{\rm ttl},\ t_c+\tau_{\max}),\\\operatorname{fresh}(t)&\iff t<t_{\rm exp},\\a_{\rm image}(t)&=t-t_c.\end{aligned}

(4.22)

上式是在统一时基下的教学到期定义：既不超过命令本身生命，也不超过观测新鲜度。若图像采样 10.00 s，生成 10.18 s，TTL 为 0.30 s，而图像最大年龄为 0.25 s，则到期是 10.25 s，不是 10.48 s；即使 10.20 s 才收到，新包也仅剩 0.05 s 使用窗口。实际执行还需预留到作用时刻的调度界。不能每重发一次相同决策就重新赋予完整 TTL，那会让陈旧图像永不过期。

把时间误差写成剩余预算，而不是只检查收包频率

假设图像时刻已经映射到接收端时钟，但映射误差界为 $\epsilon_t\ge0$ ，从本次控制计算到真正执行的最坏延迟为 $\tau_{\rm act}\ge0$ ，两者单位都是秒。为了保证作用时图像仍未超龄，应检查

(t_{\rm now}-\hat t_c)+\epsilon_t+\tau_{\rm act}<\tau_{\max}.

其中 $\hat t_c$ 是映射后的采样时刻，而不是真实采样时刻的绝对真值。误差界取正号，是在按“图像可能比我们以为的更旧”保守计算。若当前估计年龄 $0.20$ s、同步误差界 $0.01$ s、执行预算 $0.03$ s、允许最大年龄 $0.25$ s，那么还剩 $0.01$ s；再排一次 $0.02$ s 的队列就超限。收到新序号本身完全不能补回这段预算。

租约也有自己的到期时间 $t_{\rm lease}$ ：它限制某个来源控制任务的权利，而命令 TTL 限制具体目标的生命。若两者同时使用，最终有效期限应取 $\min(t_{\rm exp},t_{\rm lease})$ 。租约尚有效不表示其中最后一个旧目标仍有效；某条新目标尚新鲜也不表示它有权抢别人的租约。这是把“谁能发”和“这次还能用多久”分开建模的原因。

监督器怎样避免多个闭环抢同一机器人

任务状态与 ArduSub 飞行模式是两层状态。导航、捕获、跟随、环绕、丢失、人工是任务状态；GUIDED、CIRCLE、POSHOLD 等决定飞控怎样解释输入。改变任务状态时，先撤销旧源租约，再初始化新的目标与控制器状态，确认所需模式和估计条件后才发新参考。恢复也不是“把旧包重新发一遍”，而是重新确认身份、状态、时刻与目标。

任务状态	进入 / 保持条件	参考与转移
NAVIGATE	定位、路径与模式有效	LOS 速度；目标候选出现转 CONFIRM
CONFIRM	身份连续且图像新鲜	受限接近；确认时间足够转 FOLLOW 或 ORBIT
ORBIT	目标关联、几何、模式、估计均相容	环绕参考；过期立即离开依赖视觉的控制
LOST	图像短暂不可用	仅在限定窗口内预测或搜索，窗口到期进入预定降级
DEGRADED	导航或任务前提不足	取消相应闭环；按剩余能力选择已定义保持/退出
MANUAL	人工接管	撤销自主租约，不被下一帧自动抢回

session 区分进程重启，seq 拒绝同会话重复或乱序，owner 确定谁持有任务租约，TTL 确定这条目标还能活多久；四者不能互相替代。新 session 不应凭一个陌生包自动获得控制权，应通过明确的任务启动或租约更换接受。严重健康故障与人工接管优先于正常状态滞回，不能为了“连续三帧确认”延迟人工退出。

先手写，再展开参考实现

将本课公式写成一个 C++17 单文件程序。先列输入单位与退化分支，再写核心循环；不要先看答案再照抄。参考程序只依赖标准库，可用 g++ -std=c++17 -O2 lesson.cpp -o lesson && ./lesson 离线运行，不访问机器人。断言是对上文手算结果的检查，不是用程序输出代替推导。

参考解答（展开查看）

// 教学契约模型；所有时刻已换到同一单调时基，单位秒。
#include <array>
#include <string>
#include <optional>
#include <algorithm>
#include <cmath>
#include <cstdint>
#include <iostream>
#include <cassert>
struct Message {
    std::string owner,session;std::uint64_t seq;
    double capture,generated,ttl;
    int frame;std::uint16_t mask;std::array<double,3> ned;
};
struct Accepted {Message message;double expiry;};
struct Bridge {
    std::string owner="orbit",session="run-1";
    std::optional<std::uint64_t> lastSeq;
    std::optional<Accepted> accepted;
    bool leased=true;
    bool accept(const Message&m,double now){
        if(!leased||m.owner!=owner||m.session!=session)return false;
        if(lastSeq && m.seq<=*lastSeq)return false;
        if(!std::isfinite(now)||!std::isfinite(m.capture)||!std::isfinite(m.generated)
           ||!std::isfinite(m.ttl)||m.ttl<=0)return false;
        if(m.capture>m.generated||m.generated>now)return false;
        if(m.frame!=1||m.mask!=0x0DC7)return false;
        for(double v:m.ned)if(!std::isfinite(v))return false;
        double expiry=std::min(m.generated+m.ttl,m.capture+.25);
        if(now>=expiry)return false;
        accepted=Accepted{m,expiry};lastSeq=m.seq;return true;
    }
    bool usable(double now,bool freshTelemetry,bool modeOk,bool armed)const{
        return leased&&accepted&&freshTelemetry&&modeOk&&armed&&now<accepted->expiry;
    }
    void manual(){leased=false;accepted.reset();}
};
int main(){
    Bridge b;Message m{"orbit","run-1",1,10.,10.18,.3,1,0x0DC7,{.3,0,.1}};
    assert(b.accept(m,10.20));assert(std::abs(b.accepted->expiry-10.25)<1e-12);
    assert(!b.accept(m,10.21)); // 重复不能刷新 expiry
    assert(b.usable(10.24,true,true,true));assert(!b.usable(10.25,true,true,true));
    m.seq=2;m.generated=10.30;assert(!b.accept(m,10.30)); // 新包内仍是旧图像
    m.capture=10.31;m.generated=10.32;m.seq=3;assert(b.accept(m,10.33));
    assert(!b.usable(10.34,true,false,true)); // 模式不符
    b.manual();m.seq=4;assert(!b.accept(m,10.34));
    std::array<double,3> neu{100*m.ned[0],100*m.ned[1],-100*m.ned[2]};
    std::cout<<"mask="<<m.mask<<", internal cm/s="<<neu[0]<<','<<neu[1]<<','<<neu[2]<<'\n';
}

把 0x0DC7 每一位拆开

\begin{aligned}{\tt 0x0DC7}&=1+2+4+64+128+256+1024+2048\\&=3527,\\\begin{bmatrix}v_x^{\rm int}\\v_y^{\rm int}\\v_z^{\rm int}\end{bmatrix}&=100\begin{bmatrix}v_N\\v_E\\-v_D\end{bmatrix}\quad[\mathrm{cm/s}].\end{aligned}

(4.23)

按 POSITION_TARGET_TYPEMASK 的零起始位号：位 0、1、2 忽略位置；位 3、4、5 保留速度；位 6、7、8 忽略加速度；位 10、11 忽略 yaw 与 yaw_rate。位 9 是 force 解释开关而非又一个轴忽略位，这个掩码中为零。LOCAL_NED 的枚举值为 1，速度顺序是北、东、下，消息单位是 m/s；yaw 用 rad，yaw_rate 用 rad/s。协议定义见 TB04-S7。忽略字段最好填有限零值，不要用 NaN 表示“应该忽略”，因为不同接收校验路径可能先检查数值。

项目 mavlink_velocity.py 的 MavlinkVelocitySender 使用 SET_POSITION_TARGET_LOCAL_NED、LOCAL_NED 和上述掩码。随后沿 ArduSub/GCS_MAVLink_Sub.cpp 的 GUIDED 接收分支查看模式条件、帧、mask、单位转换，再追到 GUIDED 的水平速度闭环和垂向目标。内部相关控制量使用 cm/s 与向上爬升约定，因此下潜 0.1 m/s 在该转换中变为负 10 cm/s；北向 0.3 m/s 变为正 30 cm/s。不要把全仓库一概称为 NEU，只在实际转换边界标明。

若机器人艏向向东，机体前速 0.3 m/s 先经姿态旋转应得到 NED 速度 (0,0.3,0)。把 0.3 直接放 vx 会让它向北，而不是向前。BODY_NED 等名称还有各字段各自的语义，不能仅凭名字猜它等同完整 FRD；本项目既然选 LOCAL_NED，就在发送前完成明确旋转，避免依赖另一个未使用的帧分支。

读一段真实上游 C++：数值怎样跨过接收边界

下面两行原样摘自 ArduPilot ArduSub-4.1.1 的 ArduSub/GCS_Mavlink.cpp，函数为 GCS_MAVLINK_Sub::handleMessage 的本地位置目标分支。第一行在速度未忽略分支中，第二行在速度模式分支中；为聚焦接口，省略其间代码。它们不是项目定制代码的重写，也不是本课教学程序的一部分。

vel_vector = Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f);
sub.guided_set_velocity(vel_vector);

这两行分别承担“单位与垂向符号转换”和“把目标提交给控制器”。放大一百把 m/s 变成 cm/s，第三分量取负把消息向下为正改成这里内部向上为正；水平的北东顺序并未交换。前面的接收逻辑先限制模式和可用帧，再从 mask 计算位置、速度、加速度是否忽略；不是只要读到了三个浮点数就进入速度环。该版本的 body 帧路径对水平分量另做机体到北东的旋转，当前课程发送器采用 LOCAL_NED，因此不走那个旋转分支。

特别注意，这一版本把 yaw 与 yaw_rate 的处理保留在 future-use 注释中；消息规范“定义了字段”，不等于该接收实现会消费字段。导航课的纯速度掩码明确忽略这两项，所以与此接口相容。视觉环绕如果要实现转艏，必须沿自己实际的任务/模式转艏通道追踪，不能把偏航前馈悄悄塞入一个未消费的字段。

来源：ArduSub-4.1.1 原始 GCS_Mavlink.cpp，ArduPilot contributors，项目按 GNU GPLv3 分发。阅读完整分支时，从消息解码顺着看模式检查、frame、mask、速度转换、目标提交；本地定制报告里的 GCS_MAVLink_Sub.cpp 命名及零命令/超时策略则按本项目调用链理解。

“零”、过期和已完成是三件事

项目当前 GUIDED 的有效非零 XY 命令进入速度闭环；有效零 XY 命令清水平推力并重置相关 PID；超时或没有有效指令则捕获位置进入保持。这是该项目修改分支的具体行为，不应拿通用上游版本的超时语义覆盖它。它也解释为什么“发送零速度”和“不再发送”可能产生不同结果：前者是有效目标，后者走失效状态。若保持所需估计不可靠，任务层要执行与剩余能力相容的降级，不能把切换 POSHOLD 当作万能操作。

桥接端沿 BridgeState.accept 阅读任务租约，再沿 _select_outgoing 阅读期限、模式、armed 和遥测新鲜度；视觉端沿 OrbitPublisher 阅读 session、seq、image_age 与 TTL。跟随端 v1/direction_magnitude 与 tracking v2/normalized 的字段语义不同，转换时要明确方向、符号和尺度。CIRCLE 的 normalized forward/lateral 不是 GUIDED 的地理速度，不能仅改字段名就跨模式互换。

SET_POSITION_TARGET_LOCAL_NED 是流式目标消息，不应期待每一帧都收到 COMMAND_ACK；命令协议的 ACK 与高频 setpoint 是不同机制。完整理解一次作用需依次观察应用目标、桥接接受、飞控采纳目标、估计运动响应。模式切换成功则以模式遥测确认，不以“串口 write 没报错”作为最终状态。

综合手写作业与解题路线

在纯内存中串联 A* 路点、LOS、视觉确认、环绕和人工接管。依次注入重复序号、乱序、进程重启、旧图像新包、模式不符和断流。先写下每个事件之后预期的任务状态、租约拥有者、接受参考与到期原因，再运行 C++ 参考模型比较。尤其回答：什么能更新租约，什么能更新命令，什么不能改变图像的采样时刻？

参考解答（展开查看）

用内存消息完成一条小任务：A* 路点 → LOS → 视觉确认 → 环绕 → 人工接管。构造同一时间线上的位置、图像框与消息，不接硬件。依次注入重复序号、乱序、进程重启、旧图像新发包、模式不符和命令断流，输出任务状态、租约拥有者、接受的参考与到期原因。答案要点：重复与乱序不能刷新期限；新会话须重新获得租约；旧图像不能靠新 seq 变新；模式不符不能继续翻译成原模式目标；人工接管后的下一张好图也不能抢回所有权。

最后把系统重新画成闭环：估计给制导位置和速度，制导给控制运动参考，控制给分配器广义力或归一化轴量，执行器改变机器人，传感器再观测。地图决定哪里允许走，视觉决定跟谁走，时间契约决定此刻还能否使用这条信息，监督器决定谁有权控制。能把每个接口的物理量、坐标、单位、时间和失效动作都讲清，就能从一个漂亮的算法框图走到可独立手写的完整系统。

一次成功的参考传递需要哪些闭环

应用计算参考只是第一步，桥接接受快照是第二步，飞控消息分支采纳目标是第三步，控制器产生输出是第四步，估计到的实际运动追上参考才是第五步。每一步都有自己的条件与时间。流式目标不靠逐帧 ACK 完成闭环，而由模式、目标和运动反馈共同确认语义；人工接管时则先撤销自动租约，防止下一帧恢复发送。项目 BridgeState.accept 负责接受与拥有权，_select_outgoing 负责此刻能否使用，飞控接收分支负责 frame/mask 和内部单位，底层闭环负责执行。把它们画成串联责任边界，比把所有错误统一称为“通讯问题”更能定位原因。

TB04-S7 · MAVLink Common Message Set：SET_POSITION_TARGET_LOCAL_NED / POSITION_TARGET_TYPEMASK

官方消息定义，查坐标枚举、各字段 SI 单位及逐位忽略规则；实际是否支持仍由项目接收分支决定。

TB04-S8 · MAVLink：Time Synchronization Protocol

官方时钟同步说明。先读不同组件时钟为何不能直接比较，再把估计偏移与本地过期时间结合。