增加车队轨迹控制核心与轮组自转模式

docs/interfaces.md

@@ -26,11 +26,16 @@
 | `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` 当前不能直接互相比较；未来接收层应另行保存来源时间并完成新鲜度和时间对齐。
+
 ## 轨迹契约
 
 文件：`MultiWheelC/Trajectory/`。
@@ -93,13 +98,18 @@ bool TryGetState(out VehicleState state)
 `ParkingVehicleStateProviderFactory.Create()` 创建：
 
 - `DetourVehicleStateProvider`：读取Detour位姿，处理源时间、重复帧、运动合理性、预测创新、静止确认和跳变候选；候选确认期间使用轮组速度短时预测控制位姿。确认后的有限小坐标偏移可更新 `controlFromDetour` 以保持当前任务坐标连续，超限或未恢复时将状态置为不可用。
-- `WheelFeedbackVehicleStateProvider`：调用 `MultiWheelChassis.GetCarSpeed(true)`，低通滤波车体 `Vx`、`Vy` 和由deg/s转换为rad/s的 `Vw`。最终 `VehicleState` 的平面速度使用轮组 `Vx/Vy`，Omega仍使用Detour估计；轮组 `Vw` 只提供给Detour短时运动预测和动态航向合理性判断。
+- `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()` 要求位置和航向均有效。原地自转则由 `WheelFeedbackVehicleStateProvider.TryGetHeadingRadians()` 获取独立可靠航向：仅位置异常时可进入 `PositionUnavailableHeadingAvailable`，不会中断正在进行的航向闭环；航向异常仍会停止动作。到达目标并停止驱动后，`MultiWheelRotateInPlace` 调用 `BeginPostRotationPositionRecovery()`，在原有3帧、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` 去重或分组。
 
@@ -107,7 +117,7 @@ bool TryGetState(out VehicleState state)
 
 ### `PathTrackingContext`
 
-一次控制周期的只读快照，包含：`VehicleState`、`TrajectoryProjection`、处理后的控制参考速度、预瞄曲率、真实 `deltaTime` 和运动方向β。
+一次控制周期的只读快照，包含：受控刚体的车体系 `Twist2D`、速度有效标志、`TrajectoryProjection`、处理后的控制参考速度、预瞄曲率、真实 `deltaTime` 和运动方向β。该类型不依赖单车 `VehicleState` 或车队 `FleetState`。
 
 `ActualLongitudinalSpeedMetersPerSecond` 是完整车体平面速度沿β方向的投影：
 
@@ -117,6 +127,14 @@ 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` 调用同一核心，再将GCP结果转换为车队原点处的 `FleetMotionCommand`。
+
 当前速度闭环和执行边界的语义并不完全相同：纵向PID与Stanley实际速度分母使用 `Vβ`（Stanley也可按配置改用参考速度），而 `GcpMotionCommand.SpeedMetersPerSecond` 和旧版 `SendMotion.speed` 表示带行驶方向符号的车体中心平移速度模长。正常圆弧理想跟踪时 `Vy=0`，两者相等；只有横向误差共同转角产生非零 `Vy` 时，模长与 `Vβ` 才相差余弦因子。当前最大命令速度仍限制最终发送的模长。
 
 ### `ILateralController`