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 透传而来。