First_Dev_Dept/Tutorial
12
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fix fleet in-place rotate sync

Browse Source
This commit is contained in:
ruifeng.zhou
2026-07-02 14:38:00 +08:00
parent d4d2b1a052
commit bf83bdb4b3
5 changed files with 367 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MultiWheel/MultiWheelC/PilotDefinition.cs
+259 -33
View File
@@ -146,12 +146,31 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
private float _multiVehicleRotateDirectionHint = 1f;
private string _multiVehicleRotateAlignDetail = "";
private DateTime _multiVehicleRotateAlignLastLog = DateTime.MinValue;
private const float DefaultRotateCompTangentFrac = 0.10f;
private struct RotateControlParams
{
public float ActiveOmega;
public float CompXyFac;
public float CompXyIFac;
public float CompXyMax;
public float CompThFac;
public float CompThIFac;
public float CompThMax;
public float CompTangentFrac;
public float StartWheelAlignDeg;
public float ActiveWheelAlignDeg;
}
// 诊断日志：节流计时 + 最近一次检测几何（中心/朝向/距离），用于定位剧烈运动来源
private DateTime _mvDbgLastLog = DateTime.MinValue;
private float _mvLastDetCenterX, _mvLastDetCenterY, _mvLastDetDir, _mvLastDetDist;
// 原地旋转(mode2)实际叠加的车体系纠偏旋量(mm/s, mm/s, deg/s)，仅用于诊断日志。
private float _mvRotCompVx, _mvRotCompVy, _mvRotCompOmega;
private DateTime _mvRotateCompLimitLastLog = DateTime.MinValue;
private bool _mvRotateCenterDriftActive;
private float _mvRotateCenterStartX, _mvRotateCenterStartY, _mvRotateCenterStartTh, _mvRotateCenterMaxDrift;
private DateTime _mvRotateCenterLastLog = DateTime.MinValue;
// 原地旋转纠偏 PI 控制器的积分累加器(mm·s, mm·s, deg·s)与上次计算时刻。
private float _rotIntegX, _rotIntegY, _rotIntegTh;
private float _rotOmegaPeak; // 本次旋转过程中观测到的指令角速度峰值(deg/s)，用于纠偏随转速缩放
@@ -253,7 +272,8 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
return value;
}
private void UpdateMultiVehicleRotateModeState(int fleetMode, bool active, float requestedOmega)
private void UpdateMultiVehicleRotateModeState(int fleetMode, bool active, float requestedOmega,
RotateControlParams rotateParams)
{
var rotateActive = active && fleetMode == 2;
if (!rotateActive)
@@ -274,15 +294,16 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
}
_multiVehicleRotateModeActive = true;
if (Math.Abs(requestedOmega) > 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<PilotConfig, PilotDefin
// Reuse SendRotateMotion's decomposition and angle-limit checks, but keep wheel speed at zero.
if (rampStop)
chassis.RampStop();
var alignOmega = Math.Abs(requestedOmega) > 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<PilotConfig, PilotDefin
DLog.Log(
$"car{CarNum} ROTATE_PREPARE omegaReq={requestedOmega:0.000} alignOmega={alignOmega:0.000} " +
$"hint={hint} comp=({localCompensateX:0.0},{localCompensateY:0.0},{localCompensateTh:0.000}) " +
$"rampStop={rampStop} ok={motionOk} aligned={aligned} detail={alignDetail} " +
$"rampStop={rampStop} startTol={rotateParams.StartWheelAlignDeg:0.0} ok={motionOk} aligned={aligned} detail={alignDetail} " +
$"chassisReason={chassis.LastMotionDecomposeFailureReason}",
"MultiVehicleRemoteDbg");
FleetDiag(
$"ROTATE_PREPARE omegaReq={requestedOmega:0.000} alignOmega={alignOmega:0.000} hint={hint} " +
$"comp=({localCompensateX:0.0},{localCompensateY:0.0},{localCompensateTh:0.000}) " +
$"ok={motionOk} aligned={aligned} detail={alignDetail}");
$"startTol={rotateParams.StartWheelAlignDeg:0.0} ok={motionOk} aligned={aligned} detail={alignDetail}");
}
Hedingben.ToastText(
@@ -532,7 +553,12 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
$"car{CarNum} NOTIFY_APPLY seq={notification.Seq} mode={notification.Mode} " +
$"omega={notification.FleetOmega:0.000} reqOmega={notification.RequestedFleetOmega:0.000} " +
$"released={notification.FleetMotionReleased} stop={notification.FleetStopActive} " +
$"reason={notification.FleetStopReason}",
$"reason={notification.FleetStopReason} useDetourCorr={notification.UseDetourCorrection} " +
$"rotParam(valid={notification.RotateParamsValid} xyP={notification.RotateCompXyFac:0.###} " +
$"xyI={notification.RotateCompXyIFac:0.###} xyMax={notification.RotateCompXyMax:0.#} " +
$"thP={notification.RotateCompThFac:0.###} thI={notification.RotateCompThIFac:0.###} " +
$"thMax={notification.RotateCompThMax:0.#} frac={notification.RotateCompTangentFrac:0.###} " +
$"startTol={notification.RotateStartWheelAlignDeg:0.#} activeTol={notification.RotateActiveWheelAlignDeg:0.#})",
"MultiVehicleRemoteDbg");
}
@@ -594,6 +620,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
var manualVx = scriptOn ? MultiVehicleScriptVx : MultiVehicleManualVx;
var manualVy = scriptOn ? MultiVehicleScriptVy : MultiVehicleManualVy;
var manualVth = scriptOn ? MultiVehicleScriptVth : MultiVehicleManualVth;
VehicleSyncNotification activeNotification = null;
if (isMaster)
autoEnabled = MultiVehicleAutoEnabled;
@@ -607,11 +634,13 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
Math.Max(300, Conf.MultiVehicleSyncInterval * 5);
if (fresh && MultiVehicleNotification != null)
{
activeNotification = MultiVehicleNotification;
autoEnabled = MultiVehicleNotification.AutoEnabled;
manualEnabled = manualEnabled || MultiVehicleNotification.ManualEnabled;
}
}
}
var rotateParams = BuildRotateControlParams(isMaster ? null : activeNotification);
// 入口诊断（节流 ~300ms）：记录从 Medulla 收到的原始 IO 值与门控判定，
// 用于确认遥控指令是否真的传到了 Clumsy，以及为何提前 return。
@@ -644,7 +673,8 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
LogMultiVehicleStop("fleet control disabled; ramp stop previous fleet command", true);
}
_multiVehicleWasActive = false;
UpdateMultiVehicleRotateModeState(0, false, 0);
UpdateMultiVehicleRotateModeState(0, false, 0, rotateParams);
ResetMultiVehicleRotateCenterDrift();
LogMultiVehicleRemoteDecision(
$"RETURN_IDLE master={isMaster} rawEn={MultiVehicleManualEnabled} scriptOn={scriptOn} auto={autoEnabled} " +
$"rawMode={MultiVehicleManualMode} rawVx={MultiVehicleManualVx:0.000} rawVy={MultiVehicleManualVy:0.000} rawVth={MultiVehicleManualVth:0.000}");
@@ -684,10 +714,16 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
// false 仅表示"定位不参与车队内姿态纠正"，不影响下面整队姿态计算。
// - slamRead：本车本轮是否读取 Detour 全局位姿。"整个车队姿态的计算"(主车反推/广播车队中心、
// SLAM 间距、自动模式安全门)始终依赖全局定位 —— 故自动模式下主车必读，与开关无关；
// 手动外部遥控默认不读 Detour，避免 getCartLocation 阻塞拖慢 2 腿检测；确需手动 POS 纠偏时再开
// MultiVehicleManualUseDetourCorrection。
// 手动外部遥控默认不读 Detour，避免 getCartLocation 阻塞拖慢 2 腿检测；脚本自动原地旋转
// (FleetRotateInPlace) 已经依赖 Detour 判停，因此显式打开 POS 纠偏以保持旋转中心。
var autoMode = autoEnabled && !manualEnabled;
var manualDetourCorrection = manualEnabled && Conf.MultiVehicleManualUseDetourCorrection;
var scriptRotateDetourCorrection = isMaster && scriptOn && manualMode == 2 && Conf.FleetRotateUseDetourHeading;
var notificationDetourCorrection = !isMaster && activeNotification != null &&
activeNotification.UseDetourCorrection;
var manualDetourCorrection = manualEnabled &&
(Conf.MultiVehicleManualUseDetourCorrection ||
scriptRotateDetourCorrection ||
notificationDetourCorrection);
var useDetourCorrection = Conf.MultiVehicleSyncUseDetour && (!manualEnabled || manualDetourCorrection);
var slamRead = useDetourCorrection || (isMaster && autoMode);
float selfX = 0, selfY = 0, selfTh = 0;
@@ -825,7 +861,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
? notificationRequestedFleetOmega
: fleetOmega)
: fleetOmega;
UpdateMultiVehicleRotateModeState(fleetMode, manualEnabled || autoEnabled, requestedFleetOmega);
UpdateMultiVehicleRotateModeState(fleetMode, manualEnabled || autoEnabled, requestedFleetOmega, rotateParams);
var (layoutX, layoutY, layoutTh) = GetLayoutPose(syncTh, syncDistance);
@@ -1055,7 +1091,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
: syncDistance / 2f;
chassis.ControlPointRadius = controlRadius;
// #1 纠偏随旋转缩放：把每轮纠偏钳到旋转切向的比例，减速末段切向变小时纠偏同步缩小，杜绝轮向乱摆。
chassis.RotateCompTangentFrac = Conf.MultiVehicleRotateCompTangentFrac;
chassis.RotateCompTangentFrac = rotateParams.CompTangentFrac;
if (canMove && Conf.MultiVehicleUseDetect)
{
@@ -1109,23 +1145,26 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
// 仅在"被指令旋转"时才纠偏：松开摇杆(fleetOmega≈0)时绝不再下发补偿，
// 否则积分残留会持续驱动车辆平移/旋转，表现为"松杆后轮子来回打、自转停不下来"。
var rotating = Math.Abs(fleetOmega) > 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<PilotConfig, PilotDefin
rotCompVy *= compScale;
rotCompOmega *= compScale;
}
LimitRotateCompensation(ref rotCompVx, ref rotCompVy, ref rotCompOmega,
absOmega, controlRadius, rotateParams.CompTangentFrac);
}
else
{
@@ -1156,7 +1197,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
bool motionOk;
if (rotateHoldForAlignment)
{
motionOk = PrepareMultiVehicleRotateWheels(chassis, requestedFleetOmega,
motionOk = PrepareMultiVehicleRotateWheels(chassis, requestedFleetOmega, rotateParams,
rotCompVx, rotCompVy, rotCompOmega);
}
else if (rotating)
@@ -1164,7 +1205,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
// Preparation calls SendRotateMotion with zero delta; after release it would starve the speed ramp.
if (!MultiVehicleRotateWheelsReady)
{
motionOk = PrepareMultiVehicleRotateWheels(chassis, requestedFleetOmega,
motionOk = PrepareMultiVehicleRotateWheels(chassis, requestedFleetOmega, rotateParams,
rotCompVx, rotCompVy, rotCompOmega, rampStop: false);
rotateHoldForAlignment = true;
fleetOmega = 0;
@@ -1187,14 +1228,28 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
motionOk = chassis.SendRotateMotion(fleetOmega,
localCompensateX: rotCompVx, localCompensateY: rotCompVy,
localCompensateTh: rotCompOmega);
var activeWheelAlignDeg = rotateParams.ActiveWheelAlignDeg;
var activeAligned = TryCheckRotateWheelAlignment(chassis,
Conf.InPlaceRotateWheelAlignDeg, out _multiVehicleRotateAlignDetail);
activeWheelAlignDeg, out _multiVehicleRotateAlignDetail);
if (!motionOk)
MultiVehicleRotateWheelsReady = false;
if (!activeAligned)
if (motionOk && !activeAligned)
{
MultiVehicleRotateWheelsReady = false;
MultiVehicleRotateFleetReady = false;
rotateHoldForAlignment = true;
fleetOmega = 0;
_rotIntegX = _rotIntegY = _rotIntegTh = 0;
_rotPiLastTime = DateTime.MinValue;
_rotOmegaPeak = 0;
rotCompVx = rotCompVy = rotCompOmega = 0;
_mvRotCompVx = _mvRotCompVy = _mvRotCompOmega = 0;
motionOk = PrepareMultiVehicleRotateWheels(chassis, requestedFleetOmega, rotateParams);
LogMultiVehicleRemoteDecision(
$"ROTATE_ACTIVE_ALIGN_WAIT master={isMaster} manual={manualEnabled} auto={autoEnabled} " +
$"requestedOmega={requestedFleetOmega:0.000} align={_multiVehicleRotateAlignDetail}");
$"ROTATE_ACTIVE_REHOLD master={isMaster} manual={manualEnabled} auto={autoEnabled} " +
$"requestedOmega={requestedFleetOmega:0.000} activeTol={activeWheelAlignDeg:0.0} " +
$"align={_multiVehicleRotateAlignDetail}", true);
}
}
}
else
@@ -1203,7 +1258,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
localCompensateX: rotCompVx, localCompensateY: rotCompVy,
localCompensateTh: rotCompOmega);
MultiVehicleRotateWheelsReady = motionOk &&
TryCheckRotateWheelAlignment(chassis, Conf.InPlaceRotateWheelAlignDeg,
TryCheckRotateWheelAlignment(chassis, rotateParams.StartWheelAlignDeg,
out _multiVehicleRotateAlignDetail);
}
SetMultiVehicleMotionFeasible(motionOk, motionOk ? "" : chassis.LastMotionDecomposeFailureReason);
@@ -1216,11 +1271,22 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
}
LogRotateWheelOutputs(chassis, rotateHoldForAlignment ? "hold-align" : (rotating ? "rotate" : "idle"),
fleetOmega);
if (fleetPosValid)
LogMultiVehicleRotateCenterDrift(isMaster, manualEnabled, autoEnabled, fleetReady, canMove, rotating,
CenterX, CenterY, CenterTh, requestedFleetOmega, fleetOmega,
rotCompVx, rotCompVy, rotCompOmega);
else
ResetMultiVehicleRotateCenterDrift();
LogMultiVehicleRemoteDecision(
$"SEND_ROTATE master={isMaster} manual={manualEnabled} canMove={canMove} ready={fleetReady} ok={motionOk} " +
$"holdAlign={rotateHoldForAlignment} wheelReady={MultiVehicleRotateWheelsReady} fleetReady={MultiVehicleRotateFleetReady} " +
$"mode={fleetMode} omegaReq={requestedFleetOmega:0.000} omega={fleetOmega:0.000} " +
$"comp=({rotCompVx:0.0},{rotCompVy:0.0},{rotCompOmega:0.000}) align={_multiVehicleRotateAlignDetail} " +
$"rotParam(xyP={rotateParams.CompXyFac:0.###} xyI={rotateParams.CompXyIFac:0.###} " +
$"xyMax={rotateParams.CompXyMax:0.#} thP={rotateParams.CompThFac:0.###} " +
$"thI={rotateParams.CompThIFac:0.###} thMax={rotateParams.CompThMax:0.#} " +
$"frac={rotateParams.CompTangentFrac:0.###} activeOmega={rotateParams.ActiveOmega:0.###} " +
$"startTol={rotateParams.StartWheelAlignDeg:0.#} activeTol={rotateParams.ActiveWheelAlignDeg:0.#}) " +
$"fleetCnt={fleetCount}/{Conf.MultiVehicleFleetNum}");
// 仅主车：读取两车实际 sim 位姿，量化"开环横向滑移"来源（节流 ~200ms）。
@@ -1240,6 +1306,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
_rotPiLastTime = DateTime.MinValue;
_rotPoseEpisode = false;
_rotPosePrevTime = DateTime.MinValue;
ResetMultiVehicleRotateCenterDrift();
// 常规/蟹行：蟹行时 frontTh==rearTh（四轮同向）即为平移，与常规共用同一下发路径。
var motionOk = chassis.SendMotion(fleetVx, fleetFrontTh, fleetRearTh, localControlRadius: controlRadius,
localCompensateX: xDetectCompensate + xPosCompensate,
@@ -1345,6 +1412,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
AutoEnabled = MultiVehicleAutoEnabled,
// 脚本驱动等价于手动联动，广播为 ManualEnabled 让从车解锁跟随。
ManualEnabled = manualEnabled,
UseDetourCorrection = useDetourCorrection,
SyncTh = syncTh,
SyncDistance = syncDistance,
DeltaDetectCenter = deltaDetectCenter,
@@ -1357,13 +1425,20 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
IdealY = MultiVehicleAutoIdealY,
IdealTh = MultiVehicleAutoIdealTh
};
FillRotateNotificationParams(notification, rotateParams);
}
LogMultiVehicleRemoteDecision(
$"NOTIFY_SEND seq={notification.Seq} mode={notification.Mode} " +
$"omega={notification.FleetOmega:0.000} reqOmega={notification.RequestedFleetOmega:0.000} " +
$"released={notification.FleetMotionReleased} stop={notification.FleetStopActive} " +
$"reason={notification.FleetStopReason} fleetCnt={notification.Fleet.Count}/{Conf.MultiVehicleFleetNum}");
$"reason={notification.FleetStopReason} useDetourCorr={notification.UseDetourCorrection} " +
$"fleetCnt={notification.Fleet.Count}/{Conf.MultiVehicleFleetNum} " +
$"rotParam(valid={notification.RotateParamsValid} xyP={notification.RotateCompXyFac:0.###} " +
$"xyI={notification.RotateCompXyIFac:0.###} xyMax={notification.RotateCompXyMax:0.#} " +
$"thP={notification.RotateCompThFac:0.###} thI={notification.RotateCompThIFac:0.###} " +
$"thMax={notification.RotateCompThMax:0.#} frac={notification.RotateCompTangentFrac:0.###} " +
$"startTol={notification.RotateStartWheelAlignDeg:0.#} activeTol={notification.RotateActiveWheelAlignDeg:0.#})");
foreach (var kv in notification.Fleet)
{
@@ -1467,12 +1542,20 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
{
centerX = centerY = centerTh = 0;
var carPos = DetourInterface.getCartLocation();
return TryGetFleetCenterFromPose((float)carPos.x, (float)carPos.y, (float)carPos.th,
out centerX, out centerY, out centerTh);
}
public bool TryGetFleetCenterFromPose(float carX, float carY, float carTh,
out float centerX, out float centerY, out float centerTh)
{
centerX = centerY = centerTh = 0;
var (layoutX, layoutY, layoutTh) = GetLayoutPose(Conf.TestCarSyncTh, Conf.TestCarSyncDistance);
var self = new VehicleSyncInfo
{
X = (float)carPos.x,
Y = (float)carPos.y,
Th = (float)carPos.th,
X = carX,
Y = carY,
Th = carTh,
LayoutX = layoutX,
LayoutY = layoutY,
LayoutTh = layoutTh
@@ -1663,6 +1746,149 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
private static float ClampBias(float value, float threshold)
=> 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");
}
/// <summary>
/// 原地旋转纠偏单轴 PI 控制器。err 为车体系偏差(mm 或 deg)，输出为修正速度(mm/s 或 deg/s)，
/// 带死区、积分抗饱和(限制积分贡献在 ±max 内)与总输出限幅。死区内冻结积分(保留稳态修正以抵消恒定扰动)。