# 关键接口与数据约定

## 坐标系和单位

`Shared/` 的统一约定：

- 位置：m；线速度：m/s；角度：rad；角速度：rad/s。
- 真实车体坐标系：X向前、Y向左、逆时针为正。
- 原始世界坐标来自Detour；位姿在 `DetourVehicleStateProvider.ReadDetourObservation()` 边界由mm/deg转换为m/rad，再通过任务坐标变换生成连续控制位姿。
- `AngleMath` 的弧度归一化范围是 `[-π, π)`，角度范围是 `[-180°, 180°)`。
- `Pose2D` 表示局部坐标系在父坐标系中的位姿；变量名使用 `XxxInYyy` 说明关系。
- `Twist2D` 不携带坐标系标签，必须由变量名、外层类型或接口契约说明。

旧版 `CommonUsage` 底盘接口使用混合单位：轮子位置和 `ControlPointRadius` 为mm，`SendMotion` 舵角与 `CarSpeed.Vw` 为deg/deg/s，线速度为m/s。单位转换应只出现在Shared适配边界。

## Shared 数据模型

文件：`Shared/Models/MotionModels.cs`。

| 类型 | 语义 |
| --- | --- |
| `Point2D` | 二维位置或向量，单位m |
| `Pose2D` | 二维位置和朝向，单位m/rad |
| `Twist2D` | 同一点处的 `Vx`、`Vy`、`Omega`，单位m/s、rad/s |
| `VehicleLayout` | 单车车体系在车队系中的固定目标位姿 |
| `FleetLayout` | 不可变的成员布局快照，构造时校验成员数量、车号唯一性和位姿有限值 |
| `FleetMotionCommand` | 车队参考点及该点在车队系中表达的刚体速度 |
| `FleetMemberCommand` | 指定车辆及其真实车体系中表达的成员中心速度 |
| `FleetState` | 同一采样时刻的车队虚拟中心位姿和速度快照 |

坐标变换集中在 `FrameTransform2D`；有限值检查集中在 `NumericGuard`；角度处理集中在 `AngleMath`。

`FleetKinematics.Decompose()` 依据 `v_i = v_ref + omega × (r_i-r_ref)` 生成每车命令，再按 `VehicleLayout.PoseInFleet` 的朝向把线速度从车队系转换到成员车体系；所有刚性连接成员的 `Omega` 保持相同。

`MultiWheelC/Fleet/FleetState.cs` 中，`FleetPoseInWorld` 表示车队虚拟中心位姿，其 `Yaw` 同时定义车队坐标系 `+X` 在世界系中的方向；车队系采用 `+X` 前、`+Y` 左、逆时针为正。`TwistAtFleetOriginInWorld` 与 `TwistAtFleetOriginInFleet` 是同一参考点、同一物理速度在两个坐标系中的表达，后者由前者和 `FleetPoseInWorld` 推导，不是第二份独立测量。`HasValidVelocityEstimate=false` 时速度按零保存，用于区分尚未形成可靠速度估计与真实零速。

`FleetState.SampleTimestampSeconds` 表示主车/协调器生成该车队状态快照时的本机单调时间，不是Detour全局时间。各成员电脑的本机时钟和Detour `tick` 当前不能直接互相比较；未来接收层应另行保存来源时间并完成新鲜度和时间对齐。

### `FleetLayoutCapture`

文件：`MultiWheelC/Fleet/FleetLayoutCapture.cs`。

- `FleetMemberPose`：用于布局采集的车号和成员世界位姿输入。
- `FleetLayoutCaptureResult`：同时返回采集时刻的 `FleetPoseInWorld` 和固定的 `FleetLayout`；车队当前世界位姿不存入布局。
- `Capture(members, leaderVehicleId)`：车队原点X/Y取成员车体中心的算术平均，Yaw取主车Yaw，并通过 `inverse(FleetPoseInWorld) ∘ VehiclePoseInWorld` 得到每车 `PoseInFleet`。
- 空成员、非法或重复车号、非有限位姿、主车不存在均拒绝采集；主车缺失时不使用其他车辆降级代替。

该接口是纯几何计算，调用者必须在外部保证成员位姿处于同一世界坐标系，并完成夹紧、静止、数据新鲜度和时间对齐检查；布局的存储与原子激活也不属于该类。

## 车队通信与安全接口

`Shared/Fleet/FleetProtocol.cs` 只定义与具体无线模块无关的值类型：

- `FleetCommand`：任务号、主车命令序号、目标车号、命令类型、成员β、成员车体系速度、有效期和原因码。目标车号0表示广播。
- `FleetMemberReport`：车号、任务号、本车报告序号、来源采样时间、公共世界系位姿/速度、状态有效性、本地任务阶段、最近执行命令序号和故障码；它同时承担心跳和命令确认。
- `FleetCommandKind` 当前覆盖滚动准备、自转准备、激活、运动和停止；`FleetMemberState` 覆盖空闲、准备、就绪、活动和故障。

`MultiWheelC/Fleet/IFleetTransport.cs` 仅提供 `SendCommand`、`SendReport`、`TryReceiveCommand` 和 `TryReceiveReport`。它不负责串口初始化、字节序列化、任务/序列校验、安全判定或时钟同步；测试项目的 `InMemoryFleetTransport` 按车号路由FIFO消息，广播只送从车，主车本地命令由 `FleetRuntime` 直接执行。

时间语义分层：报告的 `SampleTimestampSeconds` 是来源车的采样时刻，真实跨机接收层必须换算到主车时间轴后才能用于 `FleetMemberStateSample` 对齐；`FleetMemberSafetyStatus.LastAcceptedReportTimeSeconds` 必须使用主车本地单调接收时间；`FleetMemberAgent` 的命令接收时间和截止时间必须使用本车本地单调时钟。Detour `tick` 不能直接替代这三类时间。

`FleetSafetySupervisor` 只在主车侧检查成员缺失、任务不一致、通信超时、状态不可用和故障，并锁存首个 `ShouldStop` 原因；它不直接操作底盘。`FleetMemberAgent.UpdateCommandWatchdog()` 是每车本地最后一道失联保护，活动状态下命令过期或本地时钟倒退会立即 `StopImmediately()` 并进入 `Faulted`。运行循环即使没有收到新命令也必须周期调用该方法。

### `FleetRuntime`

- 构造时传入本车ID、主车ID、`IFleetTransport`、本车 `FleetMemberAgent` 和 `IVehicleStateProvider`；本车ID等于主车ID时，还必须传入 `FleetPreparationCoordinator`、`FleetCoordinator` 和 `FleetSafetySupervisor`。
- `StartRollingPlan(planId, layout, trajectory)` 只允许主车调用；成员准备β直接取自 `FleetCoordinator.MotionDirectionInFleetRadians`，避免准备方向与控制方向出现两份配置。
- `Update(currentTimeSeconds, deltaTimeSeconds)` 使用本机单调时间驱动准备超时、主车报告新鲜度和本地命令看门狗。主车缓存的远端报告采样时间必须已经换算到主车时间轴，当前运行层不会自行估计跨机时钟偏移。
- `Stop(reason)` 正常取消本车任务；主车同时广播当前任务停止。运行状态覆盖 `Idle`、`Preparing`、`Ready`、`Active`、`Completed` 和锁存的 `Faulted`。
- 第一版公开启动入口只支持固定布局滚动轨迹；协议和从车执行器虽已保留 `PrepareSpin`，但整队自转尚无对应主车任务入口。

## 轨迹契约

文件：`MultiWheelC/Trajectory/`。

### `TrajectoryPoint`

- `ArcLengthMeters`：按预定执行点序从起点累计的弧长，首点必须为0。
- `PoseInWorld`：车体中心参考位姿；Yaw始终表示车头方向，不因倒车改为车尾方向。
- `CurvaturePerMeter`：沿弧长增加/执行点序定义，左弯为正。
- `ReferenceSpeedMetersPerSecond`：轨迹切线方向的有符号车体中心参考平移速度，绝对值是速度模长，正值前进、负值倒车、0停车。

### `Trajectory2D`

- 至少两个点，弧长严格递增，相邻位置不能重合。
- 弧长增量必须与离散线段长度在容差内一致。
- 当前是空间轨迹，以弧长插值位置、Yaw、曲率和参考速度；没有时间戳，也不是时间参数化轨迹。
- `SampleAtArcLength()` 和 `TrajectoryProjector` 共用 `InterpolateSegment()`，避免两套插值语义。

### `TrajectoryProjector`

- 首周期可全轨迹搜索；后续控制器使用上次弧长附近窗口，当前常量为后退0.10m、前进1.00m。
- 距离并列时优先接近上次进度，降低交叉或平行轨迹跳段风险。
- `LateralErrorMeters` 相对轨迹点序判断左右，轨迹位于车辆左侧时为正。
- `HeadingErrorRadians` 是参考车头航向减实际车头航向的最短角差。

测试轨迹由 `Experiments/TestTrajectoryFactory.cs` 生成，不是正式规划层。直线和曲线速度同号；负速度表示倒车，测试工厂不支持在同一条曲线中直接切换前进/倒车方向。

## 状态接口

### `DetourInterface.getCartLocation()`

当前引用接口返回的定位对象包含：

- `x`、`y`：Detour世界坐标，单位mm。
- `th`：车体航向，单位deg。
- `tick`：定位源时间；实车静态记录确认可按`.NET DateTime.Ticks`转换，同时必须保留原始整数用于诊断。
- `l_step`：Detour定位过程的质量/步骤相关指标，精确定义和正式阈值待Detour文档确认；实车正常静态基线包含2和周期性单帧3，不能将3直接判为异常。

`DetourVehicleStateProvider` 使用 `x/y/th` 生成控制位姿，使用 `tick` 区分重复、新到和倒退帧，并保留 `l_step` 供诊断。`l_step` 当前不作为单一硬门限：实车数据中既出现过高 `l_step` 后恢复，也出现过低 `l_step` 但位姿创新异常的情况。状态时间仍使用本机单调 `Stopwatch`；`Experiments/DetourStaticDiagnosticTest.cs` 可只读记录原始字段、接口耗时、帧间差和轮组反馈。

当前继续设计状态层所需的Detour侧最小信息是：`getCartLocation()` 返回的是可能重定位跳变的全局/map位姿还是连续里程计位姿；`tick` 对应采集、解算还是发布时刻及缓存语义；`l_step` 的状态含义；现有接口是否另有连续里程计位姿/速度或定位有效、重定位状态。无需以取得Detour源码为前提，部署版本、里程计和单线激光SLAM配置截图可用于核对实际运行配置。以上信息尚待Detour侧确认。

### `IVehicleStateProvider`

```text
bool TryGetState(out VehicleState state)
```

返回 `false` 表示当前状态不可用；`ParkingGeometricController` 会主动停车、重置反馈控制器并等待下一周期恢复。

### `VehicleState`

- `SampleTimestampSeconds`：状态源单调时钟时间。
- `PoseInWorld`：当前任务控制世界系中的车体中心位姿；初始化时与Detour世界系对齐，确认有限坐标阶跃后可通过内部变换保持任务连续，不保证始终等于原始Detour坐标。
- `TwistInWorld`、`TwistInBody`：同一刚体速度的两种表达。
- `HasValidVelocityEstimate`：速度反馈是否已建立有效时间基准；无效时构造器将速度置零。

### 默认组合状态源

`ParkingVehicleStateProviderFactory.Create()` 创建：

- `DetourVehicleStateProvider`：读取Detour位姿，处理源时间、重复帧、运动合理性、预测创新、静止确认和跳变候选；候选确认期间使用轮组速度短时预测控制位姿。确认后的有限小坐标偏移可更新 `controlFromDetour` 以保持当前任务坐标连续，超限或未恢复时将状态置为不可用。
- `WheelFeedbackVehicleStateProvider`：调用 `MultiWheelChassis.GetCarSpeed(true)`，低通滤波车体 `Vx`、`Vy` 和由deg/s转换为rad/s的 `Vw`。轮组估计有效后，最终 `VehicleState` 的 `Vx/Vy/Omega` 均使用滤波后的轮组反馈；首帧速度标记为无效，并由 `VehicleState` 对外置零。轮组 `Vw` 同时提供给Detour短时运动预测和动态航向合理性判断。

默认滤波参数来自 `PilotConfig.ParkingControl.cs`：Detour线速度0.15s、Detour角速度0.20s、轮组反馈 `Vx/Vy/Vw` 统一为0.10s。

跳变处理的重要边界：航向创新允许量随 `|Vw| × Detour源帧间隔` 增加；候选若在近似原地自转期间开始，则整个候选确认过程禁止自动改写任务坐标系。默认候选确认窗口为0.60s，窗口内输出轮组预测状态，超时后完整位姿安全返回不可用。

完整轨迹控制仍通过 `TryGetState()` 要求位置和航向均有效。原地自转提供两种反馈模式：

- `DetourAbsoluteHeading` 使用 `TryGetHeadingRadians()` 做世界航向闭环；仅位置异常时可继续，航向异常仍会停止。到达目标并停车后调用 `BeginPostRotationPositionRecovery()` 恢复完整位姿，恢复失败时不释放下一运动段。
- `RelativeWheelOdometry` 使用 `TryGetWheelTwist(out Twist2D, out timestamp)` 直接读取并滤波轮组 `Vx/Vy/Vw`，按轮组采样时间梯形积分相对转角；活动自转期间不调用Detour，也不执行Detour停车后恢复。该模式用于允许约±5°误差的相对角动作，不能提供绝对世界航向校正，轮胎打滑和轮径误差会累积到角度结果中。

`TryGetWheelTwist()` 是不依赖Detour的窄接口；首次尚未形成滤波样本时返回 `false`，其时间戳来自该状态源使用的本机单调时钟。

`TrackingExperimentRecorder` 已记录原始/滤波轮组 `Vx/Vy/Vw`、轮组采样时刻、Detour `tick/l_step`、数据年龄和源帧间隔、轮速预测时刻、实际/允许的位置与航向创新、候选触发原因、估计器状态及不可用原因。创新和源帧间隔只在收到Detour新帧时更新；若CSV记录频率高于Detour帧率，后续记录行会重复最近一个新帧的诊断值，分析时应按 `DetourTickRaw` 去重或分组。

## 控制接口

### `PathTrackingContext`

一次控制周期的只读快照，包含：受控刚体的车体系 `Twist2D`、速度有效标志、`TrajectoryProjection`、处理后的控制参考速度、预瞄曲率、真实 `deltaTime` 和运动方向β。该类型不依赖单车 `VehicleState` 或车队 `FleetState`。

`ActualLongitudinalSpeedMetersPerSecond` 是完整车体平面速度沿β方向的投影：

```text
Vβ = cos(β)·Vx_body + sin(β)·Vy_body
```

因此45°/90°蟹行不能只使用车体 `Vx` 判断纵向速度。

### `PathTrackingCore`

`PathTrackingCore.Compute(Pose2D, Twist2D, bool, double)` 是单车与车队共用的纯轨迹跟踪周期：负责连续投影、距离保护、起步释放、终点制动/完成判断、曲率预瞄、横纵向控制和GCP分配，返回 `PathTrackingCycleOutput`。输出只包含周期结果、可选 `GcpMotionCommand`、投影和计算耗时；核心不读取状态源、不发送底盘命令，也不处理通信。

状态暂时不可用时，外层调用 `PauseForUnavailableState()` 保留当前轨迹与投影连续性；明确失败或取消分别使用 `Fail()`、`Cancel()`。

`ParkingGeometricController` 是单车适配层，负责读取 `IVehicleStateProvider` 并通过 `GcpCommandExecutor` 发送实体底盘命令。`FleetController` 是车队虚拟中心适配层，使用 `FleetState.FleetPoseInWorld` 和 `TwistAtFleetOriginInFleet` 调用同一核心。构造参数 `motionDirectionInFleetRadians` 定义固定的 `β_fleet`：核心按该方向投影实际纵向速度，GCP结果先解释为运动坐标系Twist，再通过 `R(β_fleet)` 转换成车队坐标系下、车队原点处的 `FleetMotionCommand`。`β_fleet=0` 保持常规前向语义；当前控制器不负责运行中切换β。

当前速度闭环和执行边界的语义并不完全相同：纵向PID与Stanley实际速度分母使用 `Vβ`（Stanley也可按配置改用参考速度），而 `GcpMotionCommand.SpeedMetersPerSecond` 和旧版 `SendMotion.speed` 表示带行驶方向符号的车体中心平移速度模长。正常圆弧理想跟踪时 `Vy=0`，两者相等；只有横向误差共同转角产生非零 `Vy` 时，模长与 `Vβ` 才相差余弦因子。当前最大命令速度仍限制最终发送的模长。

### `ILateralController`

- `Compute(PathTrackingContext)` 返回 `LateralControlCommand`。
- `Reset()` 清除跨周期状态。
- 默认实现 `StanleyLateralController` 输出前后GCP角度：横向误差形成共同转角，航向误差和曲率前馈形成差动转角；倒车时按行驶方向修正符号。
- `TrajectoryTrackingMovement.LateralControllerFactory` 是替换Stanley的动作级扩展点。

### `ILongitudinalController`

- `ComputeSpeedMetersPerSecond(PathTrackingContext)` 返回有符号中心命令速度。
- 默认 `PidLongitudinalController` 使用轨迹参考速度前馈叠加实际速度PID反馈，带死区、积分限制和最大命令速度；参考明确为0时禁止反向速度纠偏。

### GCP命令

- `LateralControlCommand`：前后GCP目标角度，以及共同/差动分量。
- `GcpCommandAllocator`：分别限制前后GCP角度并与纵向速度组合。
- `GcpMotionCommand`：进入底盘执行前的有符号速度和前后GCP角度，SI单位。
- `GcpCommandExecutor`：限制GCP角速度，保存 `LastRequestedCommand`/`LastSentCommand`，转换为 `Twist2D` 后执行。

对称前后GCP位于当前运动系的 `(±R, 0)`。依据刚体速度关系 `v(point)=v(center)+ω×r`：

```text
Vfront = (Vx, Vy + ωR)
Vrear  = (Vx, Vy - ωR)
```

因此 `Vy` 形成前后同向的共同转角，`ωR` 形成前后反向的差动转角。正常圆弧的理想车体速度为 `(v, 0, v·κ)`，曲率通过差动转角实现，并不要求车体系存在 `Vy`。

## 底盘命令接口

### `MultiWheelChassisAdapter.SendBodyTwist`

输入始终是真实车体系 `Twist2D`：

- 平移和角速度均近零：立即停车。
- 平移近零、角速度非零：要求真实车体系已激活，调用 `SendXYThSpeed` 做纯自转。
- 平移非零：使用动作开始前已经准备并激活的固定β运动系，转换为前后GCP方向并调用旧版 `SendMotion`。

运动中不通过 `Vx/Vy` 猜测模式；模式由“停车→舵轮预对齐→`ActivateMotionFrame(β)`”显式确定。命令若几乎垂直于当前运动系X轴会停车并拒绝执行。

`MultiWheelChassisAdapter` 构造时读取旧底盘 `GetOriginBias().Z`，按 `β=-biasZ` 同步内部缓存；这不是SLAM Yaw，也不会切换坐标系或转动舵轮。真正切换由 `ActivateMotionFrame(β)` 调用 `SetOriginBias(0, 0, -β)` 完成，旧底盘随后把真实轮位重新表达在β运动系中。

滚动命令在 `SendRollingTwistInActiveMotionFrame()` 中按 `R(-β)` 将真实车体系平面速度转换到已激活运动系；角速度在二维旋转变换下不变。传给 `SendMotion` 的前后GCP角度均相对该运动系X轴，速度绝对值为 `sqrt(Vx²+Vy²)`，正负号取运动系 `Vx` 的方向。倒车方向由速度符号表达，GCP角度保持为相对行驶方向的等效机械方向，避免无意义旋转180°。

### `MultiWheelChassis.SendMotion`

输入是车体中心有符号平移速度模长、当前运动系中的虚拟前后GCP方向。前后GCP法线交点确定ICR；每个真实轮子使用其运动系位置计算切线舵角和半径速度比例。机械限位通过等效舵角/反向轮速解析，无法满足时返回失败原因。

`SendMotion` 对每个轮子的目标速度应用 `AccPerSecond/DeAccPerSecond` 斜坡；加速或减速的判据是目标速度绝对值相对当前发送速度绝对值增大或减小。`PredefinedDriveStop()` 直接清零发送速度，不经过该减速斜坡。

## 配置接口

- `PilotConfig` 是Clumsy运行配置模型；`[FieldMember]` 字段提供默认值和宿主显示/持久化元数据。
- `PilotDefinition.Conf` 是C层动作实际读取的运行配置对象。
- `MultiWheelC/Configuration/PilotConfig.ParkingControl.cs` 集中停车状态估计、Stanley、纵向PID、原地自转、舵轮准备、GCP与完成条件参数。
- 动作中的 nullable 覆盖字段用于特定动作段或测试；为空时使用车辆配置。车辆级限速和通用控制参数不应在普通实验中随意覆盖。
- 多舵轮底盘几何和 `MaxSpeed/AccPerSecond/DeAccPerSecond` 由 `MultiWheelChassisInitializer` 从宿主工作目录相对路径 `../chassis.json` 加载，并覆盖 `AbstractChassis` 的字段默认值；启动输出会打印最终加载值。当前实车参考副本 `参考文档/chassis参考.json` 为 `AccPerSecond=0.3m/s²`、`DeAccPerSecond=1.0m/s²`，但只有部署端实际命名和路径正确的 `chassis.json` 会影响运行。
- `参考文档/*.json` 是样例/实车复制资料；当前源码未发现这些JSON被 `PilotDefinition.Conf` 自动读取的入口。

运行时配置文件格式、位置和覆盖优先级由外部Clumsy宿主决定，当前仓库内待确认。

## M/C IO与MCU边界

- C层 `PilotDefinition` 和M层 `DiverCartDefinition` 使用 `[AsUpperIO]`/`[AsLowerIO]` 对齐夹臂命令、驱动使能、位置反馈和车号等字段。
- M层 `WheelSpeedDiagnosticLogger` 每次记录生成 `_can.csv` 和 `_snapshot.csv`：前者按CAN回调时刻保存八电机速度/位置与四舵角事件，后者按最多50Hz保存目标/实际舵角、目标角速度、PID/前馈/合成差速、限幅前后电机命令、速度/位置/电流反馈及各事件的本机单调时间、序号和数据年龄。MATLAB辨识时可使用 `TargetTh` 作为参考、`TotalDiff` 或 `Sent*` 作为执行输入、`ActualTh` 作为输出；左右轮公共/差速通道由方向统一后的左右命令和反馈在分析侧组合。
- C层纵向辨识复用 `TrackingExperimentRecorder` 保存目标 `CommandVx`、Detour位姿和轮组原始/滤波速度；M层诊断中的 `SentSpeed*` 才是经过底盘速度斜坡与最终限幅后的执行输入。正式辨识应一组C层CSV对应一组M层CSV，并以首个非零发送命令边沿对齐两套本机时间。
- `DiffSteerThresh` 通过 `PIDController.ChangeParameters()` 设置差速转舵PID的对称输出上限：例如配置0.3时，PID反馈项限制在[-0.3,+0.3]m/s。`MotorRoutine.UpdateDiffSteerWheelSpeeds()` 先将该反馈项与独立限幅的角速度前馈相加，再按左轮减、右轮加叠加到基础轮速；合成差速不会再次受 `DiffSteerThresh` 限制。最终八个单轮命令还会在 `MCURoutine` 中分别受 `SendThresSpeed` 限制。因此当前前馈增益为0、基础轮速为0且 `SendThresSpeed` 不低于0.3m/s的静止模式切换实验中，配置 `DiffSteerThresh=0.3` 可视为实际转舵差速的±0.3m/s上限；车辆行驶或重新启用前馈时，不能把它误当作每个电机最终总速度的唯一上限。
- `DiverCartDefinition.CommunicationInit()` 默认通过Windows `COM4`、`1,000,000 baud` 打开MCU桥；`MCUPort` 是可配置初始化参数。
- MCU内部配置为逻辑端口0：CAN `500,000 bit/s`；逻辑端口1～3：串口 `9,600 bit/s`。`MCURoutine.BatteryPortIndex = 3` 指MCU桥逻辑端口，不等同于Windows `COM3`。
- CAN命令/反馈范围集中在 `MCURoutine.cs`：驱动命令 `0x201～0x20A`，速度/位置反馈 `0x281～0x28A`，状态 `0x181～0x18A`，舵角 `0x18B～0x18E`，远程帧 `0x701～0x70A`。
- `MCUSerialBridgeCLR.cs` 是 `mcu_serial_bridge.dll` 的P/Invoke封装；本仓库源码目录未包含该本机库。

CAN ID、串口参数、IO位和驱动方向属于实车安全边界，未经明确要求不得修改。
