From 1c0258ec75e9cc214498d22c02016b7823ea3325 Mon Sep 17 00:00:00 2001 From: "ruifeng.zhou" Date: Wed, 1 Jul 2026 22:47:47 +0800 Subject: [PATCH 1/4] Add dedicated fleet crab control parameters --- MultiWheel/MultiWheelC/MovementTests.cs | 55 +++++++++++++++++++++---- MultiWheel/MultiWheelC/PilotConfig.cs | 15 +++++++ docs/FleetCrabWalkWorkContext.md | 11 +++-- docs/MultiVehicleConfig.md | 17 ++++++-- 4 files changed, 83 insertions(+), 15 deletions(-) diff --git a/MultiWheel/MultiWheelC/MovementTests.cs b/MultiWheel/MultiWheelC/MovementTests.cs index f09ed91..90e3ed4 100644 --- a/MultiWheel/MultiWheelC/MovementTests.cs +++ b/MultiWheel/MultiWheelC/MovementTests.cs @@ -387,6 +387,21 @@ public class FleetCrabWalk : MovementDefinition /// 行驶速度(m/s)。 public float CrabSpeed = 0.2f; + /// 速度命令加速度限制(m/s^2)，小于等于 0 表示不限制。 + public float FleetCrabAccel = 0.2f; + + /// 末端开始减速距离(mm)。 + public float FleetCrabSlowDistance = 2000f; + + /// 完成距离(mm)，低于该剩余距离结束动作。 + public float FleetCrabFinishDistance = 20f; + + /// 末端最低速度(m/s)。 + public float FleetCrabFinishSpeed = 0.02f; + + /// 末端减速曲线指数。 + public float FleetCrabSlowingPow = 0.8f; + /// 前后 GCP 舵角修正上限(deg)。 public float GcpThetaThreshold = 95f; @@ -429,6 +444,14 @@ public class FleetCrabWalk : MovementDefinition return value; } + private static float Slew(float current, float target, float maxDelta) + { + if (maxDelta <= 0) return target; + if (target > current + maxDelta) return current + maxDelta; + if (target < current - maxDelta) return current - maxDelta; + return target; + } + public override IEnumerable Get() { var self = PilotDefinition.Self; @@ -470,7 +493,9 @@ public class FleetCrabWalk : MovementDefinition DLog.Log( $"START center=({x0:0},{y0:0},{theta:0.0}) pathAngle={CrabAngleDeg:0.0} bodyToPath={BodyToPathAngleDeg:0.0} " + $"phi={phi:0.0} targetBody={targetBodyTh:0.0} " + - $"len={CrabLengthMm:0} dst=({dst.X:0},{dst.Y:0}) speed={CrabSpeed:0.000}", + $"len={CrabLengthMm:0} dst=({dst.X:0},{dst.Y:0}) speed={CrabSpeed:0.000} accel={FleetCrabAccel:0.000} " + + $"slow={FleetCrabSlowDistance:0} finishDist={FleetCrabFinishDistance:0} " + + $"finishSpeed={FleetCrabFinishSpeed:0.000} slowingPow={FleetCrabSlowingPow:0.00}", "FleetCrabDbg"); self.MultiVehicleScriptEnabled = false; @@ -534,10 +559,14 @@ public class FleetCrabWalk : MovementDefinition var iter = 0; var lastLog = DateTime.MinValue; - var finishDistance = Math.Max(20f, conf.FinishDistance); - var slowDistance = Math.Max(finishDistance + 1f, conf.SlowDistance); + var finishDistance = Math.Max(0f, FleetCrabFinishDistance); + var slowDistance = Math.Max(finishDistance + 1f, FleetCrabSlowDistance); var baseSpeed = Math.Abs(CrabSpeed); - var finishSpeed = Math.Min(baseSpeed, Math.Abs(conf.FinishSpeed)); + var finishSpeed = Math.Min(baseSpeed, Math.Abs(FleetCrabFinishSpeed)); + var slowingPow = Math.Max(0.01f, FleetCrabSlowingPow); + var accel = Math.Abs(FleetCrabAccel); + var cmdSpeed = 0f; + var lastTick = DateTime.Now; var gcpLimit = Math.Max(1f, Math.Abs(GcpThetaThreshold)); var stopReason = "done"; @@ -558,12 +587,17 @@ public class FleetCrabWalk : MovementDefinition if (remain <= finishDistance) break; - var speed = baseSpeed; + var targetSpeed = baseSpeed; if (remain < slowDistance) { - var ratio = (float)Math.Pow(Clamp(Math.Max(0, remain) / slowDistance, 0f, 1f), conf.SlowingPow); - speed = ratio * (baseSpeed - finishSpeed) + finishSpeed; + var ratio = (float)Math.Pow(Clamp(Math.Max(0, remain) / slowDistance, 0f, 1f), slowingPow); + targetSpeed = ratio * (baseSpeed - finishSpeed) + finishSpeed; } + var now = DateTime.Now; + var dt = Math.Max(0.001f, (float)(now - lastTick).TotalSeconds); + lastTick = now; + var speed = accel > 0 ? Slew(cmdSpeed, targetSpeed, accel * dt) : targetSpeed; + cmdSpeed = speed; var baseCrabTh = (float)CommonMath.ThDiff(phi, cth); var headingErr = (float)CommonMath.ThDiff(targetBodyTh, cth); @@ -600,7 +634,7 @@ public class FleetCrabWalk : MovementDefinition $"ITER#{iter} center=({cx:0},{cy:0},{cth:0.0}) snap=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) " + $"along={along:0} lateral={lateral:0} remain={remain:0} headingErr={headingErr:0.0} " + $"baseTh={baseCrabTh:0.0} bias={biasItem:0.0} dth={dthItem:0.0} " + - $"auto=(vx:{speed:0.000},fTh:{frontTh:0.0},rTh:{rearTh:0.0}) " + + $"targetV={targetSpeed:0.000} auto=(vx:{speed:0.000},fTh:{frontTh:0.0},rTh:{rearTh:0.0}) " + $"ideal=({ideal.X:0},{ideal.Y:0},{targetBodyTh:0.0}) scriptEn={self.MultiVehicleScriptEnabled} " + $"cnt={fleetCnt}/{conf.MultiVehicleFleetNum}", "FleetCrabDbg"); @@ -648,6 +682,11 @@ public class FleetCrabWalkTest : MovementTest BodyToPathAngleDeg = PilotDefinition.Conf.FleetCrabAngleDeg, CrabLengthMm = PilotDefinition.Conf.FleetCrabLengthMm, CrabSpeed = PilotDefinition.Conf.FleetCrabSpeed, + FleetCrabAccel = PilotDefinition.Conf.FleetCrabAccel, + FleetCrabSlowDistance = PilotDefinition.Conf.FleetCrabSlowDistance, + FleetCrabFinishDistance = PilotDefinition.Conf.FleetCrabFinishDistance, + FleetCrabFinishSpeed = PilotDefinition.Conf.FleetCrabFinishSpeed, + FleetCrabSlowingPow = PilotDefinition.Conf.FleetCrabSlowingPow, GcpThetaThreshold = PilotDefinition.Conf.FleetCrabGcpThetaThreshold }; _task = new DriveTask(_proc.Get()); diff --git a/MultiWheel/MultiWheelC/PilotConfig.cs b/MultiWheel/MultiWheelC/PilotConfig.cs index 912e607..d44e3d5 100644 --- a/MultiWheel/MultiWheelC/PilotConfig.cs +++ b/MultiWheel/MultiWheelC/PilotConfig.cs @@ -165,6 +165,21 @@ public class PilotConfig : MultiWheelPilotConfig [FieldMember(desc = "车队蟹行：行驶速度(m/s)")] public float FleetCrabSpeed = 0.2f; + [FieldMember(desc = "车队蟹行：速度命令加速度限制(m/s^2，<=0表示不限制)")] + public float FleetCrabAccel = 0.2f; + + [FieldMember(desc = "车队蟹行：末端开始减速距离(mm)")] + public float FleetCrabSlowDistance = 2000f; + + [FieldMember(desc = "车队蟹行：完成距离(mm)，低于该剩余距离结束动作")] + public float FleetCrabFinishDistance = 20f; + + [FieldMember(desc = "车队蟹行：末端最低速度(m/s)")] + public float FleetCrabFinishSpeed = 0.02f; + + [FieldMember(desc = "车队蟹行：末端减速曲线指数")] + public float FleetCrabSlowingPow = 0.8f; + [FieldMember(desc = "车队蟹行：GCP舵角修正上限(deg)")] public float FleetCrabGcpThetaThreshold = 95f; diff --git a/docs/FleetCrabWalkWorkContext.md b/docs/FleetCrabWalkWorkContext.md index 0970702..57ef759 100644 --- a/docs/FleetCrabWalkWorkContext.md +++ b/docs/FleetCrabWalkWorkContext.md @@ -270,10 +270,15 @@ MovementTest「车队联动-自动蟹行」 |------|------|------| | `FleetCrabAngleDeg` | 45 | 路径方向相对启动时车队朝向的夹角 (deg)。MovementTest 同时把车身-路径夹角设为该值；若输入“路径与小车夹角 x 度”，应填 `-x` 以保持当前车身角度 | | `FleetCrabLengthMm` | 2000 | 路径长度 (mm) | -| `FleetCrabSpeed` | 0.2 | 巡航速度 (m/s)，接近终点时由通用减速参数下调 | +| `FleetCrabSpeed` | 0.2 | 巡航速度 (m/s)，接近终点时由自动蟹行专用减速参数下调 | +| `FleetCrabAccel` | 0.2 | 速度命令加速度限制 (m/s^2)，限制 `MultiVehicleAutoVx` 每拍变化量；`<=0` 表示不限制 | +| `FleetCrabSlowDistance` | 2000 | 末端开始减速距离 (mm) | +| `FleetCrabFinishDistance` | 20 | 完成距离 (mm)，剩余距离低于该值时结束动作 | +| `FleetCrabFinishSpeed` | 0.02 | 末端最低速度 (m/s) | +| `FleetCrabSlowingPow` | 0.8 | 末端减速曲线指数；越大越靠近终点才明显降速，越小越早降速 | | `FleetCrabGcpThetaThreshold` | 95 | 自动蟹行输出 `frontTh/rearTh` 的绝对值上限，应给实际舵角限位与 `AngleLimitMarginDeg` 留余量 | -已删除旧字段：`FleetCrabCorrectionGain`、`FleetCrabCorrectionAngleDeg`、`FleetCrabCommandAccel`。旧 `clumsy.json` 若残留这些 key，会被配置反序列化忽略，不能再作为有效调参项。 +已删除旧字段：`FleetCrabCorrectionGain`、`FleetCrabCorrectionAngleDeg`、`FleetCrabCommandAccel`。旧 `clumsy.json` 若残留这些 key，会被配置反序列化忽略；新的自动链路使用 `FleetCrabAccel` 控制 `MultiVehicleAutoVx` 速度命令斜率。 动作行为：当前不再改 `MultiVehicleAutoUseIdealCenter`，结束/急停会清零 `MultiVehicleAuto*`，并保持 `MultiVehicleScriptEnabled=false`。 @@ -288,7 +293,7 @@ MovementTest「车队联动-自动蟹行」 | `DriveTaskInterval` | 50 | `clumsy.json` 顶层 | | `BiasFac` / `BiasThreshold` | 继承 `MultiWheelPilotConfig` | 横向偏差 `lateral` → 前后 GCP 同向修正 | | `DthLinearFac` / `DthLinearThreshold` | 继承 `MultiWheelPilotConfig` | 车身目标朝向偏差 `headingErr` → 前后 GCP 反向修正 | -| `SlowDistance` / `SlowingPow` / `FinishDistance` / `FinishSpeed` | 继承 `BasicPilotConfig` | 自动蟹行终点减速和结束判定 | +| `FleetCrabSlowDistance` / `FleetCrabSlowingPow` / `FleetCrabFinishDistance` / `FleetCrabFinishSpeed` | `PilotConfig` | 自动蟹行专用终点减速和结束判定 | | `MultiVehicleAutoUseIdealCenter` | true（默认） | 使用自动蟹行发布的 ideal center 给从车做前馈 | | `MultiVehicleAutoRequireFleetCenter` | true（默认） | 自动模式无有效车队中心时整队停车 | | `MultiVehicleAutoCmdTimeoutMs` | 0（auto） | 自动命令新鲜度超时，避免控制器停发后沿末速度滑行 | diff --git a/docs/MultiVehicleConfig.md b/docs/MultiVehicleConfig.md index 75c8f0e..25403c2 100644 --- a/docs/MultiVehicleConfig.md +++ b/docs/MultiVehicleConfig.md @@ -155,7 +155,12 @@ TwoLegGuessX = -(TestCarSyncDistance - DeltaDetectCenter) |------|------|--------| | `FleetCrabAngleDeg` | 蟹行路径方向相对**启动时车队朝向**的夹角(deg，逆时针为正)。MovementTest 同时把车身-路径夹角设为该值，因此 `FleetCrabAngleDeg=-x` 会让车身保持启动朝向，并以 `x` 度夹角追踪路径 | `45` | | `FleetCrabLengthMm` | 蟹行路径**长度**(mm)，沿夹角方向行驶该距离后停车结束 | `2000` | -| `FleetCrabSpeed` | 蟹行巡航速度(m/s)，写入 `MultiVehicleAutoVx`；接近终点时会被 `SlowDistance/SlowingPow/FinishSpeed` 降速 | `0.2` | +| `FleetCrabSpeed` | 蟹行巡航速度(m/s)，写入 `MultiVehicleAutoVx`；接近终点时会被自动蟹行专用减速参数下调 | `0.2` | +| `FleetCrabAccel` | 蟹行速度命令加速度限制(m/s^2)，限制 `MultiVehicleAutoVx` 每拍变化量；`<=0` 表示不限制 | `0.2` | +| `FleetCrabSlowDistance` | 蟹行末端开始减速距离(mm) | `2000` | +| `FleetCrabFinishDistance` | 蟹行完成距离(mm)，剩余距离低于该值时结束动作 | `20` | +| `FleetCrabFinishSpeed` | 蟹行末端最低速度(m/s) | `0.02` | +| `FleetCrabSlowingPow` | 蟹行末端减速曲线指数；越大越靠近终点才明显降速，越小越早降速 | `0.8` | | `FleetCrabGcpThetaThreshold` | 自动蟹行输出 `frontTh/rearTh` 的绝对值上限(deg)。应小于实际舵角可行范围，并给 `AngleLimitMarginDeg` 留余量 | `95` | 自动蟹行还会使用下列通用控制参数： @@ -164,7 +169,6 @@ TwoLegGuessX = -(TestCarSyncDistance - DeltaDetectCenter) |------|------|----------| | `BiasFac` / `BiasThreshold` | 横向偏差 `lateral` → 前后 GCP 同向修正。增大后收敛更快，但过大可能摆动 | `MultiWheelPilotConfig` | | `DthLinearFac` / `DthLinearThreshold` | 车身目标朝向偏差 `headingErr` → 前后 GCP 反向修正，用于保持车身与路径夹角 | `MultiWheelPilotConfig` | -| `SlowDistance` / `SlowingPow` / `FinishDistance` / `FinishSpeed` | 终点减速和结束判定 | `BasicPilotConfig` | | `MultiVehicleAutoUseIdealCenter` | 是否把自动蟹行计算出的 `IdealX/Y/Th` 广播给从车做前馈 | `true` | | `MultiVehicleAutoRequireFleetCenter` | 自动模式是否要求有效车队中心；定位/车队中心失效时整队停车 | `true` | | `MultiVehicleAutoCmdTimeoutMs` | 自动命令新鲜度超时，0 表示按联动周期自动计算 | `0` | @@ -177,12 +181,12 @@ TwoLegGuessX = -(TestCarSyncDistance - DeltaDetectCenter) |------------|--------|----------| | `FleetCrabCorrectionGain` | 旧脚本链路的横向误差纠偏增益 | `BiasFac` | | `FleetCrabCorrectionAngleDeg` | 旧脚本链路的最大改向角 | `BiasThreshold` / `FleetCrabGcpThetaThreshold` | -| `FleetCrabCommandAccel` | 旧脚本链路的 `Vx/Vy` 斜率限制 | 由自动联动周期、底盘速度斜坡和终点减速共同约束 | +| `FleetCrabCommandAccel` | 旧脚本链路的 `Vx/Vy` 斜率限制 | 新自动链路使用 `FleetCrabAccel` 限制 `MultiVehicleAutoVx` | **行为要点 / 注意** - 当前实现走 `MultiVehicleAuto*` 自动字段链路，不再开启 `MultiVehicleScriptEnabled`，也不受 `MultiVehicleCrabSteerLimitDeg` 影响（该字段只影响手动 `mode=1` 蟹行）。 -- `FleetCrabDbg` 会记录 `along/lateral/remain/headingErr/baseTh/bias/dth/auto(vx,fTh,rTh)/ideal`；`MultiVehicleDbg` 可继续对照最终 `BASE/SEND`、POS/Detect 补偿、ready/stop 状态。 +- `FleetCrabDbg` 会记录 `along/lateral/remain/headingErr/baseTh/bias/dth/targetV/auto(vx,fTh,rTh)/ideal`；`MultiVehicleDbg` 可继续对照最终 `BASE/SEND`、POS/Detect 补偿、ready/stop 状态。 - `MultiVehicleUseDetect=true` 时仍受 2 腿检测安全门约束（检测丢失会被置零停车）。 - Playground 双车场景的 `actuator.maxSteeringAngle` 也必须与该上限一致；若仍为 `90`，Clumsy 发出的 `-98°` 纠偏会在仿真执行层被夹回 `-90°`，表现为纯横移路径无法收敛。 @@ -219,6 +223,11 @@ TwoLegGuessX = -(TestCarSyncDistance - DeltaDetectCenter) "FleetCrabAngleDeg": 45, "FleetCrabLengthMm": 2000, "FleetCrabSpeed": 0.2, +"FleetCrabAccel": 0.2, +"FleetCrabSlowDistance": 2000, +"FleetCrabFinishDistance": 20, +"FleetCrabFinishSpeed": 0.02, +"FleetCrabSlowingPow": 0.8, "FleetCrabGcpThetaThreshold": 95 ``` From f127e145e15a5ce37c97a980290dd3cf65815c04 Mon Sep 17 00:00:00 2001 From: "ruifeng.zhou" Date: Wed, 1 Jul 2026 23:08:57 +0800 Subject: [PATCH 2/4] Add fleet correction diagnostics --- MultiWheel/MultiWheelC/PilotDefinition.cs | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/MultiWheel/MultiWheelC/PilotDefinition.cs b/MultiWheel/MultiWheelC/PilotDefinition.cs index 4128399..33fcd08 100644 --- a/MultiWheel/MultiWheelC/PilotDefinition.cs +++ b/MultiWheel/MultiWheelC/PilotDefinition.cs @@ -1284,8 +1284,12 @@ public class PilotDefinition : MultiWheelPilotDefinition comp x:{xDetectCompensate:F1} y:{yDetectCompensate:F1} th:{thDetectCompensate:F2} " + + $"fac({Conf.MultiVehicleDetectBiasXFac:F2},{Conf.MultiVehicleDetectBiasYFac:F2},{Conf.MultiVehicleDetectBiasThFac:F2}) " + + $"lim({Conf.MultiVehicleDetectBiasXThreshold:F1},{Conf.MultiVehicleDetectBiasYThreshold:F1},{Conf.MultiVehicleDetectBiasThThreshold:F1}) " + $"| POS self({selfX:F0},{selfY:F0},{selfTh:F1}) center({CenterX:F0},{CenterY:F0},{CenterTh:F1}) " + $"bias({posBiasX:F0},{posBiasY:F0},{posBiasTh:F1}) -> comp x:{xPosCompensate:F1} y:{yPosCompensate:F1} th:{thPosCompensate:F2} " + + $"fac({Conf.MultiVehiclePosBiasXFac:F2},{Conf.MultiVehiclePosBiasYFac:F2},{Conf.MultiVehiclePosBiasThFac:F2}) " + + $"lim({Conf.MultiVehiclePosBiasXThreshold:F1},{Conf.MultiVehiclePosBiasYThreshold:F1},{Conf.MultiVehiclePosBiasThThreshold:F1}) " + $"| LAYOUT({layoutX:F0},{layoutY:F0},{layoutTh:F0}) R:{syncDistance / 2f:F0} " + $"| SEND mode:{fleetMode} omega:{fleetOmega:F1} vx:{fleetVx:F3} fTh:{fleetFrontTh:F2} rTh:{fleetRearTh:F2} cx:{cx:F1} cy:{cy:F1} cth:{cth:F2} " + $"rotComp(vx:{_mvRotCompVx:F1} vy:{_mvRotCompVy:F1} om:{_mvRotCompOmega:F2}) " + From d4d2b1a052ae32f4b93e877fdc71deab4e487420 Mon Sep 17 00:00:00 2001 From: "ruifeng.zhou" Date: Thu, 2 Jul 2026 11:04:33 +0800 Subject: [PATCH 3/4] fix: keep fleet crab steering angle when stopping --- MultiWheel/MultiWheelC/MovementTests.cs | 22 +++++++++++++++------- 1 file changed, 15 insertions(+), 7 deletions(-) diff --git a/MultiWheel/MultiWheelC/MovementTests.cs b/MultiWheel/MultiWheelC/MovementTests.cs index 90e3ed4..bef2980 100644 --- a/MultiWheel/MultiWheelC/MovementTests.cs +++ b/MultiWheel/MultiWheelC/MovementTests.cs @@ -568,6 +568,8 @@ public class FleetCrabWalk : MovementDefinition var cmdSpeed = 0f; var lastTick = DateTime.Now; var gcpLimit = Math.Max(1f, Math.Abs(GcpThetaThreshold)); + var holdFrontTh = ClampAbs((float)CommonMath.ThDiff(phi, theta), gcpLimit); + var holdRearTh = holdFrontTh; var stopReason = "done"; while (!_stopping) @@ -588,10 +590,11 @@ public class FleetCrabWalk : MovementDefinition break; var targetSpeed = baseSpeed; + var slowRatio = 1f; if (remain < slowDistance) { - var ratio = (float)Math.Pow(Clamp(Math.Max(0, remain) / slowDistance, 0f, 1f), slowingPow); - targetSpeed = ratio * (baseSpeed - finishSpeed) + finishSpeed; + slowRatio = (float)Math.Pow(Clamp(Math.Max(0, remain) / slowDistance, 0f, 1f), slowingPow); + targetSpeed = slowRatio * (baseSpeed - finishSpeed) + finishSpeed; } var now = DateTime.Now; var dt = Math.Max(0.001f, (float)(now - lastTick).TotalSeconds); @@ -606,6 +609,8 @@ public class FleetCrabWalk : MovementDefinition biasItem = ClampAbs(biasItem, conf.BiasThreshold); var frontTh = ClampAbs(baseCrabTh + biasItem + dthItem, gcpLimit); var rearTh = ClampAbs(baseCrabTh + biasItem - dthItem, gcpLimit); + holdFrontTh = frontTh; + holdRearTh = rearTh; var idealAlong = Clamp(along, 0f, CrabLengthMm); var ideal = new Vector2(x0, y0) + pathDir * idealAlong; @@ -634,7 +639,7 @@ public class FleetCrabWalk : MovementDefinition $"ITER#{iter} center=({cx:0},{cy:0},{cth:0.0}) snap=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) " + $"along={along:0} lateral={lateral:0} remain={remain:0} headingErr={headingErr:0.0} " + $"baseTh={baseCrabTh:0.0} bias={biasItem:0.0} dth={dthItem:0.0} " + - $"targetV={targetSpeed:0.000} auto=(vx:{speed:0.000},fTh:{frontTh:0.0},rTh:{rearTh:0.0}) " + + $"slowRatio={slowRatio:0.000} targetV={targetSpeed:0.000} auto=(vx:{speed:0.000},fTh:{frontTh:0.0},rTh:{rearTh:0.0}) " + $"ideal=({ideal.X:0},{ideal.Y:0},{targetBodyTh:0.0}) scriptEn={self.MultiVehicleScriptEnabled} " + $"cnt={fleetCnt}/{conf.MultiVehicleFleetNum}", "FleetCrabDbg"); @@ -646,9 +651,12 @@ public class FleetCrabWalk : MovementDefinition stopReason = "stop"; self.MultiVehicleAutoVx = 0; - self.MultiVehicleAutoFrontTh = 0; - self.MultiVehicleAutoRearTh = 0; + self.MultiVehicleAutoFrontTh = holdFrontTh; + self.MultiVehicleAutoRearTh = holdRearTh; self.MultiVehicleAutoCmdTime = DateTime.Now; + DLog.Log( + $"STOP_HOLD iter={iter} reason={stopReason} hold=(fTh:{holdFrontTh:0.0},rTh:{holdRearTh:0.0}) cmdSpeed={cmdSpeed:0.000}", + "FleetCrabDbg"); var settleEnd = DateTime.Now.AddMilliseconds(Math.Max(100, conf.MultiVehicleSyncInterval * 3)); while (!_stopping && DateTime.Now < settleEnd) { @@ -656,8 +664,8 @@ public class FleetCrabWalk : MovementDefinition self.MultiVehicleScriptMode = 0; self.MultiVehicleAutoEnabled = true; self.MultiVehicleAutoVx = 0; - self.MultiVehicleAutoFrontTh = 0; - self.MultiVehicleAutoRearTh = 0; + self.MultiVehicleAutoFrontTh = holdFrontTh; + self.MultiVehicleAutoRearTh = holdRearTh; self.MultiVehicleAutoCmdTime = DateTime.Now; yield return true; } From bf83bdb4b38af0f43e6fb99a6a470bb53587e33e Mon Sep 17 00:00:00 2001 From: "ruifeng.zhou" Date: Thu, 2 Jul 2026 14:38:00 +0800 Subject: [PATCH 4/4] Fix fleet in-place rotate sync --- MultiWheel/MultiWheelC/MovementTests.cs | 67 +++- MultiWheel/MultiWheelC/PilotConfig.cs | 10 +- MultiWheel/MultiWheelC/PilotDefinition.cs | 292 ++++++++++++++++-- .../MultiWheelC/VehicleSyncBinaryCodec.cs | 24 ++ MultiWheel/MultiWheelC/VehicleSyncModels.cs | 13 + 5 files changed, 367 insertions(+), 39 deletions(-) diff --git a/MultiWheel/MultiWheelC/MovementTests.cs b/MultiWheel/MultiWheelC/MovementTests.cs index bef2980..34c0f26 100644 --- a/MultiWheel/MultiWheelC/MovementTests.cs +++ b/MultiWheel/MultiWheelC/MovementTests.cs @@ -206,7 +206,11 @@ public class FleetRotateInPlace : MovementDefinition var start = DateTime.Now; var lastTime = start; var lastLog = DateTime.MinValue; + var lastCenterLog = DateTime.MinValue; var cmdMag = 0f; // 当前实际下发角速度大小(deg/s)，缓启动从 0 斜坡爬升 + var centerTracking = false; + float centerStartX = 0, centerStartY = 0, centerStartTh = 0; + float centerLastX = 0, centerLastY = 0, centerLastTh = 0, centerMaxDrift = 0; // 无定位按时长估算时，补上缓启动斜坡少转的等效时间(≈ maxOmega/(2·accel))，使时长更接近目标角。 var estDuration = maxOmega > 1e-3 ? targetMag / maxOmega : 0; if (accel > 1e-3) estDuration += maxOmega / (2 * accel); @@ -239,20 +243,43 @@ public class FleetRotateInPlace : MovementDefinition yield return true; } + float centerStartCarX = 0, centerStartCarY = 0, centerStartCarTh = 0; if (hasPos) { - prevTh = (float)DetourInterface.getCartLocation().th; + var startPos = DetourInterface.getCartLocation(); + centerStartCarX = (float)startPos.x; + centerStartCarY = (float)startPos.y; + centerStartCarTh = (float)startPos.th; + prevTh = centerStartCarTh; startTh = prevTh; + if (self.TryGetFleetCenterFromPose(centerStartCarX, centerStartCarY, centerStartCarTh, + out centerStartX, out centerStartY, out centerStartTh)) + { + centerLastX = centerStartX; + centerLastY = centerStartY; + centerLastTh = centerStartTh; + centerMaxDrift = 0; + centerTracking = true; + } } accumulated = 0f; start = DateTime.Now; lastTime = start; lastLog = DateTime.MinValue; + lastCenterLog = DateTime.MinValue; cmdMag = 0f; DLog.Log( $"START target={TargetDeltaDeg:0.0} dir={dir} omega={maxOmega:0.0} startTh={startTh:0.00} " + $"fleetAligned={self.MultiVehicleRotateFleetReady}", "FleetRotateDbg"); + if (centerTracking) + { + DLog.Log( + $"START center=({centerStartX:0.0},{centerStartY:0.0},{centerStartTh:0.00}) " + + $"car=({centerStartCarX:0.0},{centerStartCarY:0.0},{centerStartCarTh:0.00}) " + + $"target={TargetDeltaDeg:0.0} omega={maxOmega:0.0}", + "FleetRotateCenterDbg"); + } var stopReason = "stop()"; while (true) @@ -266,7 +293,8 @@ public class FleetRotateInPlace : MovementDefinition float curTh = 0f, remaining = 0f, actualRate = 0f; if (hasPos) { - curTh = (float)DetourInterface.getCartLocation().th; + var carPos = DetourInterface.getCartLocation(); + curTh = (float)carPos.th; var step = (float)CommonMath.ThDiff(curTh, prevTh); // 本帧实际转角(逆时针为正) accumulated += step; actualRate = dt > 1e-3 ? step / dt : 0f; // 实际角速率(deg/s)，用于对比指令 @@ -280,6 +308,28 @@ public class FleetRotateInPlace : MovementDefinition desiredMag = remaining < slowDeg ? Math.Max(minOmega, maxOmega * (remaining / slowDeg)) : maxOmega; + + if (centerTracking && + self.TryGetFleetCenterFromPose((float)carPos.x, (float)carPos.y, (float)carPos.th, + out centerLastX, out centerLastY, out centerLastTh)) + { + var centerDx = centerLastX - centerStartX; + var centerDy = centerLastY - centerStartY; + var centerDrift = (float)Math.Sqrt(centerDx * centerDx + centerDy * centerDy); + centerMaxDrift = Math.Max(centerMaxDrift, centerDrift); + var centerDth = (float)CommonMath.ThDiff(centerLastTh, centerStartTh); + if ((now - lastCenterLog).TotalMilliseconds >= 250) + { + lastCenterLog = now; + DLog.Log( + $"ACTION t={elapsed:0.00}s center=({centerLastX:0.0},{centerLastY:0.0},{centerLastTh:0.00}) " + + $"start=({centerStartX:0.0},{centerStartY:0.0},{centerStartTh:0.00}) " + + $"drift=({centerDx:0.0},{centerDy:0.0}) dist={centerDrift:0.0} max={centerMaxDrift:0.0} dth={centerDth:0.00} " + + $"cmdW={dir * cmdMag:0.000} actualW={actualRate:0.000} acc={accumulated:0.0} remain={remaining:0.0} " + + $"wheelReady={self.MultiVehicleRotateWheelsReady} fleetReady={self.MultiVehicleRotateFleetReady}", + "FleetRotateCenterDbg"); + } + } } else { @@ -329,6 +379,19 @@ public class FleetRotateInPlace : MovementDefinition $"DONE reason={stopReason} 累计转角={accumulated:0.0}° 目标={TargetDeltaDeg:0.0}° " + $"用时={(DateTime.Now - start).TotalSeconds:0.00}s useDetourHeading={hasPos}", "FleetRotateDbg"); + if (centerTracking) + { + var centerDx = centerLastX - centerStartX; + var centerDy = centerLastY - centerStartY; + var centerDrift = (float)Math.Sqrt(centerDx * centerDx + centerDy * centerDy); + var centerDth = (float)CommonMath.ThDiff(centerLastTh, centerStartTh); + DLog.Log( + $"DONE reason={stopReason} center=({centerLastX:0.0},{centerLastY:0.0},{centerLastTh:0.00}) " + + $"start=({centerStartX:0.0},{centerStartY:0.0},{centerStartTh:0.00}) " + + $"drift=({centerDx:0.0},{centerDy:0.0}) dist={centerDrift:0.0} max={centerMaxDrift:0.0} dth={centerDth:0.00} " + + $"acc={accumulated:0.0} target={TargetDeltaDeg:0.0}", + "FleetRotateCenterDbg"); + } Hedingben.ToastText($"车队原地旋转完成({stopReason}) 累计{accumulated:0.0}°", "FleetRotate"); } } diff --git a/MultiWheel/MultiWheelC/PilotConfig.cs b/MultiWheel/MultiWheelC/PilotConfig.cs index d44e3d5..7eb4116 100644 --- a/MultiWheel/MultiWheelC/PilotConfig.cs +++ b/MultiWheel/MultiWheelC/PilotConfig.cs @@ -87,10 +87,9 @@ public class PilotConfig : MultiWheelPilotConfig // 仅当车队实际被指令旋转(|fleetOmega|超过此阈值)时才运行纠偏 PI；否则清零并复位积分， // 避免松开摇杆后积分残留持续驱动车辆"自行旋转停不下来"。 [FieldMember(desc = "原地旋转纠偏：生效的最小角速度阈值(deg/s)")] public float MultiVehicleRotateActiveOmega = 0.5f; - // 可选硬安全网：每轮纠偏速度幅值 ≤ 该比例×本轮旋转切向速度，限制合速度相对纯切向的最大偏角。 - // 默认 <0 关闭——纠偏随转速缩放(代码 #1)已让"纠偏:切向"比例全程恒定，匀速段不应再被削弱。 - // 仅在极端启动偏差导致匀速段仍乱打方向时，可设为 ~1.0(偏角≤45°) 兜底。 - [FieldMember(desc = "原地旋转纠偏：纠偏/旋转切向比例硬上限(默认-1关闭)")] public float MultiVehicleRotateCompTangentFrac = -1f; + // 安全网：每轮纠偏速度幅值 <= 该比例 * 本轮旋转切向速度，限制合速度相对纯切向的最大偏角。 + // 旧配置若仍为 <0，运行时按安全默认 0.10 处理；确需放宽时可在主车显式调大并同步给从车。 + [FieldMember(desc = "原地旋转纠偏：纠偏/旋转切向比例硬上限，<0使用安全默认0.10")] public float MultiVehicleRotateCompTangentFrac = 0.10f; [FieldMember(desc = "单车同步 xy 精度(mm)")] public float SingleCarSyncPrecisionXy = 10f; [FieldMember(desc = "单车同步 th 精度(deg)")] public float SingleCarSyncPrecisionTh = 0.2f; @@ -125,6 +124,9 @@ public class PilotConfig : MultiWheelPilotConfig [FieldMember(desc = "原地旋转：起转前舵轮对齐精度(deg)")] public float InPlaceRotateWheelAlignDeg = 2f; + [FieldMember(desc = "原地旋转：旋转过程中舵轮偏差重对齐阈值(deg)")] + public float InPlaceRotateActiveWheelAlignDeg = 10f; + // ===== 车队联动-原地旋转动作（FleetRotateInPlace / 对应 FleetRemote 原地旋转模式）===== // 通过 Clumsy 内部脚本字段驱动 TickMultiVehicle 的 mode2 旋转（绕车队中心 + PI 纠偏），需主车运行。 [FieldMember(desc = "车队原地旋转：角速度大小(deg/s，方向由目标角符号决定)")] diff --git a/MultiWheel/MultiWheelC/PilotDefinition.cs b/MultiWheel/MultiWheelC/PilotDefinition.cs index 33fcd08..bc7a704 100644 --- a/MultiWheel/MultiWheelC/PilotDefinition.cs +++ b/MultiWheel/MultiWheelC/PilotDefinition.cs @@ -146,12 +146,31 @@ public class PilotDefinition : MultiWheelPilotDefinition Conf.MultiVehicleRotateActiveOmega) + if (Math.Abs(requestedOmega) > rotateParams.ActiveOmega) _multiVehicleRotateDirectionHint = Math.Sign(requestedOmega); } private bool PrepareMultiVehicleRotateWheels(MultiWheelChassis chassis, float requestedOmega, + RotateControlParams rotateParams, float localCompensateX = 0f, float localCompensateY = 0f, float localCompensateTh = 0f, bool rampStop = true) { - var hint = Math.Abs(requestedOmega) > Conf.MultiVehicleRotateActiveOmega + var hint = Math.Abs(requestedOmega) > rotateParams.ActiveOmega ? Math.Sign(requestedOmega) : Math.Sign(_multiVehicleRotateDirectionHint); if (hint == 0) hint = 1; @@ -291,13 +312,13 @@ public class PilotDefinition : MultiWheelPilotDefinition Conf.MultiVehicleRotateActiveOmega + var alignOmega = Math.Abs(requestedOmega) > rotateParams.ActiveOmega ? requestedOmega : 0.01f * hint; var motionOk = chassis.SendRotateMotion(alignOmega, TimeSpan.Zero, localCompensateX: localCompensateX, localCompensateY: localCompensateY, localCompensateTh: localCompensateTh); - var wheelAligned = TryCheckRotateWheelAlignment(chassis, Conf.InPlaceRotateWheelAlignDeg, + var wheelAligned = TryCheckRotateWheelAlignment(chassis, rotateParams.StartWheelAlignDeg, out var alignDetail); var aligned = motionOk && wheelAligned; if (!motionOk) @@ -315,13 +336,13 @@ public class PilotDefinition : MultiWheelPilotDefinition Conf.MultiVehicleRotateActiveOmega; + var rotating = Math.Abs(fleetOmega) > rotateParams.ActiveOmega; if (canMove && rotating) { // #3 抗饱和：上一拍舵轮未对齐(gate=0、车没真正转动)时冻结积分，避免卡死时积分越积越大。 - var allowInteg = chassis.LastRotateAligned; + var allowInteg = chassis.LastRotateAligned && + MultiVehicleRotateWheelsReady && + MultiVehicleRotateFleetReady && + !rotateHoldForAlignment; var errX = detectDx + posBiasX; // mm，车体系：本车纵向(前+)应移动量 var errY = detectDy + posBiasY; // mm，车体系：本车横向(左+)应移动量 var errTh = detectDth + posBiasTh; // deg，本车应转角 rotCompVx = RotatePiTerm(errX, ref _rotIntegX, Conf.SingleCarSyncPrecisionXy, - Conf.MultiVehicleRotateCompXyFac, Conf.MultiVehicleRotateCompXyIFac, - Conf.MultiVehicleRotateCompXyMax, dt, allowInteg); + rotateParams.CompXyFac, rotateParams.CompXyIFac, + rotateParams.CompXyMax, dt, allowInteg); rotCompVy = RotatePiTerm(errY, ref _rotIntegY, Conf.SingleCarSyncPrecisionXy, - Conf.MultiVehicleRotateCompXyFac, Conf.MultiVehicleRotateCompXyIFac, - Conf.MultiVehicleRotateCompXyMax, dt, allowInteg); + rotateParams.CompXyFac, rotateParams.CompXyIFac, + rotateParams.CompXyMax, dt, allowInteg); rotCompOmega = RotatePiTerm(errTh, ref _rotIntegTh, Conf.SingleCarSyncPrecisionTh, - Conf.MultiVehicleRotateCompThFac, Conf.MultiVehicleRotateCompThIFac, - Conf.MultiVehicleRotateCompThMax, dt, allowInteg); + rotateParams.CompThFac, rotateParams.CompThIFac, + rotateParams.CompThMax, dt, allowInteg); // #1 纠偏随旋转指令缩放：comp ×= |fleetOmega| / 本次峰值。 // 加速+匀速段峰值≈当前 → 系数≈1（全力纠偏，不削弱）；减速段当前<峰值 → 系数随转速同步下降。 @@ -1140,6 +1179,8 @@ public class PilotDefinition : MultiWheelPilotDefinition Math.Sign(value) * Math.Min(Math.Abs(value), threshold); + private RotateControlParams BuildRotateControlParams(VehicleSyncNotification notification) + { + var parameters = new RotateControlParams + { + ActiveOmega = Conf.MultiVehicleRotateActiveOmega, + CompXyFac = Conf.MultiVehicleRotateCompXyFac, + CompXyIFac = Conf.MultiVehicleRotateCompXyIFac, + CompXyMax = Conf.MultiVehicleRotateCompXyMax, + CompThFac = Conf.MultiVehicleRotateCompThFac, + CompThIFac = Conf.MultiVehicleRotateCompThIFac, + CompThMax = Conf.MultiVehicleRotateCompThMax, + CompTangentFrac = NormalizeRotateCompTangentFrac(Conf.MultiVehicleRotateCompTangentFrac), + StartWheelAlignDeg = Conf.InPlaceRotateWheelAlignDeg, + ActiveWheelAlignDeg = NormalizeRotateWheelAlignDeg(Conf.InPlaceRotateActiveWheelAlignDeg, + Conf.InPlaceRotateWheelAlignDeg) + }; + + if (notification == null || !notification.RotateParamsValid) + return parameters; + + parameters.ActiveOmega = notification.RotateActiveOmega; + parameters.CompXyFac = notification.RotateCompXyFac; + parameters.CompXyIFac = notification.RotateCompXyIFac; + parameters.CompXyMax = notification.RotateCompXyMax; + parameters.CompThFac = notification.RotateCompThFac; + parameters.CompThIFac = notification.RotateCompThIFac; + parameters.CompThMax = notification.RotateCompThMax; + parameters.CompTangentFrac = NormalizeRotateCompTangentFrac(notification.RotateCompTangentFrac); + parameters.StartWheelAlignDeg = NormalizeRotateWheelAlignDeg(notification.RotateStartWheelAlignDeg, + Conf.InPlaceRotateWheelAlignDeg); + parameters.ActiveWheelAlignDeg = NormalizeRotateWheelAlignDeg(notification.RotateActiveWheelAlignDeg, + parameters.StartWheelAlignDeg); + return parameters; + } + + private static void FillRotateNotificationParams(VehicleSyncNotification notification, RotateControlParams parameters) + { + notification.RotateParamsValid = true; + notification.RotateActiveOmega = parameters.ActiveOmega; + notification.RotateCompXyFac = parameters.CompXyFac; + notification.RotateCompXyIFac = parameters.CompXyIFac; + notification.RotateCompXyMax = parameters.CompXyMax; + notification.RotateCompThFac = parameters.CompThFac; + notification.RotateCompThIFac = parameters.CompThIFac; + notification.RotateCompThMax = parameters.CompThMax; + notification.RotateCompTangentFrac = parameters.CompTangentFrac; + notification.RotateStartWheelAlignDeg = parameters.StartWheelAlignDeg; + notification.RotateActiveWheelAlignDeg = parameters.ActiveWheelAlignDeg; + } + + private static float NormalizeRotateWheelAlignDeg(float value, float fallback) + { + if (float.IsNaN(value) || float.IsInfinity(value) || value <= 0) + return fallback; + return value; + } + + private static float NormalizeRotateCompTangentFrac(float frac) + { + if (float.IsNaN(frac) || float.IsInfinity(frac) || frac < 0) + return DefaultRotateCompTangentFrac; + return frac; + } + + private void LimitRotateCompensation(ref float compVx, ref float compVy, ref float compOmega, + float absOmega, float controlRadius, float tangentFrac) + { + var frac = NormalizeRotateCompTangentFrac(tangentFrac); + if (frac < 0 || absOmega <= 1e-6f) return; + + var rawVx = compVx; + var rawVy = compVy; + var rawOmega = compOmega; + var tangentMmps = absOmega / 180f * (float)Math.PI * Math.Max(1f, Math.Abs(controlRadius)); + var xyLimit = tangentMmps * frac; + var xyMag = (float)Math.Sqrt(compVx * compVx + compVy * compVy); + if (xyMag > xyLimit && xyMag > 1e-6f) + { + var scale = xyLimit / xyMag; + compVx *= scale; + compVy *= scale; + } + + var omegaLimit = absOmega * frac; + compOmega = ClampBias(compOmega, omegaLimit); + + var changed = Math.Abs(rawVx - compVx) > 1e-3f || + Math.Abs(rawVy - compVy) > 1e-3f || + Math.Abs(rawOmega - compOmega) > 1e-3f; + var now = DateTime.Now; + if (changed && (now - _mvRotateCompLimitLastLog).TotalMilliseconds >= 200) + { + _mvRotateCompLimitLastLog = now; + DLog.Log( + $"car{CarNum} ROTATE_COMP_LIMIT frac={frac:0.00} omega={absOmega:0.000} radius={controlRadius:0} " + + $"tan={tangentMmps:0.0} xyLimit={xyLimit:0.0} omLimit={omegaLimit:0.000} " + + $"raw=({rawVx:0.0},{rawVy:0.0},{rawOmega:0.000}) " + + $"limited=({compVx:0.0},{compVy:0.0},{compOmega:0.000})", + "MultiVehicleRemoteDbg"); + } + } + + private void ResetMultiVehicleRotateCenterDrift() + { + _mvRotateCenterDriftActive = false; + _mvRotateCenterMaxDrift = 0; + _mvRotateCenterLastLog = DateTime.MinValue; + } + + private void LogMultiVehicleRotateCenterDrift(bool isMaster, bool manualEnabled, bool autoEnabled, + bool fleetReady, bool canMove, bool rotating, float centerX, float centerY, float centerTh, + float requestedOmega, float fleetOmega, float compVx, float compVy, float compOmega) + { + if (!_mvRotateCenterDriftActive) + { + _mvRotateCenterDriftActive = true; + _mvRotateCenterStartX = centerX; + _mvRotateCenterStartY = centerY; + _mvRotateCenterStartTh = centerTh; + _mvRotateCenterMaxDrift = 0; + _mvRotateCenterLastLog = DateTime.MinValue; + } + + var dx = centerX - _mvRotateCenterStartX; + var dy = centerY - _mvRotateCenterStartY; + var drift = (float)Math.Sqrt(dx * dx + dy * dy); + _mvRotateCenterMaxDrift = Math.Max(_mvRotateCenterMaxDrift, drift); + var dth = (float)CommonMath.ThDiff(centerTh, _mvRotateCenterStartTh); + var now = DateTime.Now; + if ((now - _mvRotateCenterLastLog).TotalMilliseconds < 250) + return; + + _mvRotateCenterLastLog = now; + DLog.Log( + $"car{CarNum} CTRL master={isMaster} manual={manualEnabled} auto={autoEnabled} " + + $"ready={fleetReady} canMove={canMove} rotating={rotating} " + + $"center=({centerX:0.0},{centerY:0.0},{centerTh:0.00}) " + + $"start=({_mvRotateCenterStartX:0.0},{_mvRotateCenterStartY:0.0},{_mvRotateCenterStartTh:0.00}) " + + $"drift=({dx:0.0},{dy:0.0}) dist={drift:0.0} max={_mvRotateCenterMaxDrift:0.0} dth={dth:0.00} " + + $"omegaReq={requestedOmega:0.000} omega={fleetOmega:0.000} comp=({compVx:0.0},{compVy:0.0},{compOmega:0.000})", + "FleetRotateCenterDbg"); + } + /// /// 原地旋转纠偏单轴 PI 控制器。err 为车体系偏差(mm 或 deg)，输出为修正速度(mm/s 或 deg/s)， /// 带死区、积分抗饱和(限制积分贡献在 ±max 内)与总输出限幅。死区内冻结积分(保留稳态修正以抵消恒定扰动)。 diff --git a/MultiWheel/MultiWheelC/VehicleSyncBinaryCodec.cs b/MultiWheel/MultiWheelC/VehicleSyncBinaryCodec.cs index 11ef343..e557edc 100644 --- a/MultiWheel/MultiWheelC/VehicleSyncBinaryCodec.cs +++ b/MultiWheel/MultiWheelC/VehicleSyncBinaryCodec.cs @@ -55,6 +55,16 @@ internal static class VehicleSyncBinaryCodec writer.Write(notification.SyncTh); writer.Write(notification.SyncDistance); writer.Write(notification.DeltaDetectCenter); + writer.Write(notification.RotateActiveOmega); + writer.Write(notification.RotateCompXyFac); + writer.Write(notification.RotateCompXyIFac); + writer.Write(notification.RotateCompXyMax); + writer.Write(notification.RotateCompThFac); + writer.Write(notification.RotateCompThIFac); + writer.Write(notification.RotateCompThMax); + writer.Write(notification.RotateCompTangentFrac); + writer.Write(notification.RotateStartWheelAlignDeg); + writer.Write(notification.RotateActiveWheelAlignDeg); writer.Write(notification.IdealX); writer.Write(notification.IdealY); writer.Write(notification.IdealTh); @@ -99,6 +109,16 @@ internal static class VehicleSyncBinaryCodec notification.SyncTh = reader.ReadSingle(); notification.SyncDistance = reader.ReadSingle(); notification.DeltaDetectCenter = reader.ReadSingle(); + notification.RotateActiveOmega = reader.ReadSingle(); + notification.RotateCompXyFac = reader.ReadSingle(); + notification.RotateCompXyIFac = reader.ReadSingle(); + notification.RotateCompXyMax = reader.ReadSingle(); + notification.RotateCompThFac = reader.ReadSingle(); + notification.RotateCompThIFac = reader.ReadSingle(); + notification.RotateCompThMax = reader.ReadSingle(); + notification.RotateCompTangentFrac = reader.ReadSingle(); + notification.RotateStartWheelAlignDeg = reader.ReadSingle(); + notification.RotateActiveWheelAlignDeg = reader.ReadSingle(); notification.IdealX = reader.ReadSingle(); notification.IdealY = reader.ReadSingle(); notification.IdealTh = reader.ReadSingle(); @@ -209,6 +229,8 @@ internal static class VehicleSyncBinaryCodec if (notification.AutoEnabled) flags |= 1 << 4; if (notification.ManualEnabled) flags |= 1 << 5; if (notification.HasIdeal) flags |= 1 << 6; + if (notification.RotateParamsValid) flags |= 1 << 7; + if (notification.UseDetourCorrection) flags |= 1 << 8; return flags; } @@ -221,6 +243,8 @@ internal static class VehicleSyncBinaryCodec notification.AutoEnabled = (flags & (1 << 4)) != 0; notification.ManualEnabled = (flags & (1 << 5)) != 0; notification.HasIdeal = (flags & (1 << 6)) != 0; + notification.RotateParamsValid = (flags & (1 << 7)) != 0; + notification.UseDetourCorrection = (flags & (1 << 8)) != 0; } private static void WriteString(BinaryWriter writer, string value) diff --git a/MultiWheel/MultiWheelC/VehicleSyncModels.cs b/MultiWheel/MultiWheelC/VehicleSyncModels.cs index 8c8e7b1..9f5ca3f 100644 --- a/MultiWheel/MultiWheelC/VehicleSyncModels.cs +++ b/MultiWheel/MultiWheelC/VehicleSyncModels.cs @@ -47,9 +47,22 @@ public class VehicleSyncNotification [JsonProperty("FleetStopSourceCar")] public int FleetStopSourceCar { get; set; } [JsonProperty("AutoEnabled")] public bool AutoEnabled { get; set; } [JsonProperty("ManualEnabled")] public bool ManualEnabled { get; set; } + [JsonProperty("UseDetourCorrection")] public bool UseDetourCorrection { get; set; } [JsonProperty("SyncTh")] public float SyncTh { get; set; } [JsonProperty("SyncDistance")] public float SyncDistance { get; set; } [JsonProperty("DeltaDetectCenter")] public float DeltaDetectCenter { get; set; } + // 原地旋转纠偏参数由主车广播，从车运行时使用同一套增益/限幅，避免主从补偿强度不一致。 + [JsonProperty("RotateParamsValid")] public bool RotateParamsValid { get; set; } + [JsonProperty("RotateActiveOmega")] public float RotateActiveOmega { get; set; } + [JsonProperty("RotateCompXyFac")] public float RotateCompXyFac { get; set; } + [JsonProperty("RotateCompXyIFac")] public float RotateCompXyIFac { get; set; } + [JsonProperty("RotateCompXyMax")] public float RotateCompXyMax { get; set; } + [JsonProperty("RotateCompThFac")] public float RotateCompThFac { get; set; } + [JsonProperty("RotateCompThIFac")] public float RotateCompThIFac { get; set; } + [JsonProperty("RotateCompThMax")] public float RotateCompThMax { get; set; } + [JsonProperty("RotateCompTangentFrac")] public float RotateCompTangentFrac { get; set; } + [JsonProperty("RotateStartWheelAlignDeg")] public float RotateStartWheelAlignDeg { get; set; } + [JsonProperty("RotateActiveWheelAlignDeg")] public float RotateActiveWheelAlignDeg { get; set; } // F: 单调递增序列号，从车据此丢弃乱序到达的旧 notify 包。 [JsonProperty("Seq")] public long Seq { get; set; } // D: 自动模式下主车路径控制器算出的车队中心理想位姿（世界系），由 idealPos/idealAngle 透传而来。