Update MultiWheel fleet sync and restore clamp support

2026-06-30 22:20:10 +08:00
parent 3411654a34
commit 574987222a
14 changed files with 287 additions and 225 deletions
@@ -100,6 +100,18 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin

    [AsLowerIO(desc = "车号")] public int CarNum = 1;

+    #region 夹臂变量
+    [AsUpperIO(desc = "左夹臂下发速度")] public float SpeedLeftArm;
+
+    [AsUpperIO(desc = "右夹臂下发速度")] public float SpeedRightArm;
+
+    [AsLowerIO(desc = "左夹臂实际位置")] public float ActualPosLeftArm;
+
+    [AsLowerIO(desc = "右夹臂实际位置")] public float ActualPosRightArm;
+
+    [AsUpperIO(desc = "夹臂不同步报警")] public bool ClampOutOfSync = false;
+    #endregion
+
    [AsLowerIO(desc = "左前左轮实际位置")] public float LFLActualPos;
    [AsLowerIO(desc = "左前右轮实际位置")] public float LFRActualPos;
    [AsLowerIO(desc = "右前左轮实际位置")] public float RFLActualPos;
@@ -109,6 +121,11 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
    [AsLowerIO(desc = "右后左轮实际位置")] public float RRLActualPos;
    [AsLowerIO(desc = "右后右轮实际位置")] public float RRRActualPos;

+    [AsLowerIO(desc = "左夹臂低限位")] public float LeftArmLowerPos;
+    [AsLowerIO(desc = "左夹臂高限位")] public float LeftArmUpperPos;
+    [AsLowerIO(desc = "右夹臂低限位")] public float RightArmLowerPos;
+    [AsLowerIO(desc = "右夹臂高限位")] public float RightArmUpperPos;
+
    [AsUpperIO(desc = "从C往驱动器下使能")] public bool DisableFromC = false;
    [AsUpperIO(desc = "从C上复位")] public bool ResetFromC = false;

@@ -117,6 +134,10 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
    private readonly object _multiVehicleNotificationLock = new();
    private DateTime _multiVehicleLastNotifyTime = DateTime.MinValue;
    private bool _multiVehicleSyncInitialized;
+    private bool _multiVehicleWasActive;
+    private bool _multiVehicleMotionFeasible = true;
+    private string _multiVehicleMotionInfeasibleReason = "";
+    private DateTime _multiVehicleStopLastLog = DateTime.MinValue;

    // 诊断日志：节流计时 + 最近一次检测几何（中心/朝向/距离），用于定位剧烈运动来源
    private DateTime _mvDbgLastLog = DateTime.MinValue;
@@ -177,6 +198,23 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
        DLog.Log($"car{CarNum} {msg}", "MultiVehicleRemoteDbg");
    }

+    private void SetMultiVehicleMotionFeasible(bool feasible, string reason = "")
+    {
+        _multiVehicleMotionFeasible = feasible;
+        _multiVehicleMotionInfeasibleReason = feasible ? "" : (reason ?? "");
+    }
+
+    private void LogMultiVehicleStop(string reason, bool force = false)
+    {
+        var now = DateTime.Now;
+        if (!force && (now - _multiVehicleStopLastLog).TotalMilliseconds < 500) return;
+        _multiVehicleStopLastLog = now;
+        var msg = $"FLEET_STOP car={CarNum} reason={reason}";
+        DLog.Log(msg, "MultiVehicleSafety");
+        FleetDiag(msg);
+        Hedingben.ToastText($"Fleet stop: {reason}", $"MultiVehicle{CarNum}-stop");
+    }
+
    private readonly object _neighborDetectLock = new();
    private readonly List<(DateTime Time, Vector2 Src, Vector2 Dst)> _neighborDetects = new();

@@ -427,6 +465,13 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin

        if (!manualEnabled && !autoEnabled)
        {
+            if (_multiVehicleWasActive)
+            {
+                chassis.RampStop();
+                SetMultiVehicleMotionFeasible(true);
+                LogMultiVehicleStop("fleet control disabled; ramp stop previous fleet command", true);
+            }
+            _multiVehicleWasActive = false;
            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}");
@@ -448,11 +493,15 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin

            return;
        }
+        _multiVehicleWasActive = true;

        // 自动模式整队姿态依赖 Detour 全局定位（与 MultiVehicleSyncUseDetour 无关）：要求编队至少一台车有定位。
        if (isMaster && autoEnabled && !manualEnabled && !FleetHasPosAvailable())
        {
            MultiVehicleAutoEnabled = false;
+            chassis.RampStop();
+            SetMultiVehicleMotionFeasible(true);
+            LogMultiVehicleStop("auto mode requires at least one Detour-positioned fleet member", true);
            Hedingben.ToastText("自动多车联动需要至少一台车有 Detour 定位", "MultiVehicle-auto-gate");
            return;
        }
@@ -482,6 +531,10 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
        var deltaDetectCenter = Conf.DeltaDetectCenter;
        float fleetVx = 0, fleetFrontTh = 0, fleetRearTh = 0, fleetOmega = 0;
        var fleetMode = 0; // 0=常规 1=蟹行 2=原地旋转
+        var autoCommandTimedOut = false;
+        var notificationStopActive = false;
+        var notificationStopReason = "";
+        var notificationStopSourceCar = 0;
        float crabInputVx = 0, crabInputVy = 0, crabRawAngle = 0;
        float crabSteerLimit = 0;
        var crabReverseEquivalent = false;
@@ -504,15 +557,12 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                }
                else if (fleetMode == 1)
                {
-                    // 蟹行：把 (前后向Vx, 横向Vy) 合成速度矢量，四轮同向打到该方向（前后舵轮角相同）。
-                    // 舵轮物理/仿真限制约为 ±120°，不要把 ±90° 当边界；否则纯横移附近会在
-                    // -90° 与 +90°/反向速度两种等价表示之间跳变。只有超过蟹行舵角上限时才取等价反向。
-                    var vx = manualVx * Conf.ManualCarSyncVxFac;
-                    var vy = manualVy * Conf.ManualCarSyncVyFac;
-                    var speed = (float)Math.Sqrt(vx * vx + vy * vy);
-                    var crabAngle = (float)(Math.Atan2(vy, vx) * 180.0 / Math.PI);
-                    crabInputVx = vx;
-                    crabInputVy = vy;
+                    // 手动蟹行：Vx 只表示线速度，Vy 表示方向摇杆比例(-1..1)，由 VyFac 映射为舵角。
+                    var speed = manualVx * Conf.ManualCarSyncVxFac;
+                    var crabRatio = Math.Max(-60f, Math.Min(60f, manualVy));
+                    var crabAngle = crabRatio * Conf.ManualCarSyncVyFac;
+                    crabInputVx = speed;
+                    crabInputVy = crabRatio;
                    crabRawAngle = crabAngle;
                    crabSteerLimit = Math.Min(179f, Math.Max(1f, Math.Abs(Conf.MultiVehicleCrabSteerLimitDeg)));

@@ -554,6 +604,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                }
                else
                {
+                    autoCommandTimedOut = true;
                    fleetVx = fleetFrontTh = fleetRearTh = 0;
                    MultiVehicleAutoVx = MultiVehicleAutoFrontTh = MultiVehicleAutoRearTh = 0;
                    MultiVehicleAutoHasIdeal = false;
@@ -580,6 +631,9 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
            fleetRearTh = notification.FleetRearTh;
            fleetMode = notification.Mode;
            fleetOmega = notification.FleetOmega;
+            notificationStopActive = notification.FleetStopActive;
+            notificationStopReason = notification.FleetStopReason ?? "";
+            notificationStopSourceCar = notification.FleetStopSourceCar;
            // D: 从车采用主车广播的理想车队中心（弧线时由 idealPos/idealAngle 而来）做前馈目标。
            MultiVehicleAutoHasIdeal = notification.HasIdeal;
            MultiVehicleAutoIdealX = notification.IdealX;
@@ -648,13 +702,67 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                                  MultiVehicleFleet.Values.All(v => v.Aligned);
        }

+        var fleetReady = false;
+        var fleetCount = 0;
+        lock (FleetLock)
+        {
+            fleetCount = MultiVehicleFleet.Count;
+            fleetReady = fleetCount == Conf.MultiVehicleFleetNum;
+        }
+
        // 安全门：开启互识别时，本车或任一其它车检测不到邻车则整队停车（速度置零）。
        // ownDetectOk 是本轮新鲜值；其它车的 DetectOk 来自其上报/主车下发（滑动窗口已给 1s 去抖）。
        var ownDetectOk = !Conf.MultiVehicleUseDetect || detectValid;
        bool othersDetectOk;
+        string otherDetectLostCars;
+        List<KeyValuePair<int, VehicleSyncInfo>> motionInfeasibleMembers;
        lock (FleetLock)
+        {
            othersDetectOk = MultiVehicleFleet.Where(kv => kv.Key != CarNum).All(kv => kv.Value.DetectOk);
-        var canMove = !Conf.MultiVehicleUseDetect || (ownDetectOk && othersDetectOk);
+            otherDetectLostCars = string.Join(",", MultiVehicleFleet
+                .Where(kv => kv.Key != CarNum && !kv.Value.DetectOk)
+                .Select(kv => kv.Key.ToString()));
+            motionInfeasibleMembers = MultiVehicleFleet
+                .Where(kv => kv.Key != CarNum && !kv.Value.MotionFeasible)
+                .ToList();
+        }
+
+        var fleetStopActive = false;
+        var fleetStopReason = "";
+        var fleetStopSourceCar = 0;
+        void AddFleetStop(string reason, int sourceCar = 0)
+        {
+            if (string.IsNullOrWhiteSpace(reason)) return;
+            if (!fleetStopActive)
+            {
+                fleetStopReason = reason;
+                fleetStopSourceCar = sourceCar;
+            }
+            else
+            {
+                fleetStopReason += " | " + reason;
+                if (fleetStopSourceCar == 0) fleetStopSourceCar = sourceCar;
+            }
+            fleetStopActive = true;
+        }
+
+        if (!fleetReady)
+            AddFleetStop($"member not ready/stale fleetCnt={fleetCount}/{Conf.MultiVehicleFleetNum}");
+        if (autoCommandTimedOut)
+            AddFleetStop("auto command timeout");
+        if (Conf.MultiVehicleUseDetect && !ownDetectOk)
+            AddFleetStop("own two-leg detection lost", CarNum);
+        if (Conf.MultiVehicleUseDetect && !othersDetectOk)
+            AddFleetStop($"other two-leg detection lost cars=[{otherDetectLostCars}]");
+        if (notificationStopActive)
+            AddFleetStop($"master stop from car{notificationStopSourceCar}: {notificationStopReason}",
+                notificationStopSourceCar);
+        if (!_multiVehicleMotionFeasible)
+            AddFleetStop($"car{CarNum} motion infeasible: {_multiVehicleMotionInfeasibleReason}", CarNum);
+        foreach (var kv in motionInfeasibleMembers)
+            AddFleetStop($"car{kv.Key} motion infeasible: {kv.Value.MotionInfeasibleReason}", kv.Key);
+
+        var canMove = !fleetStopActive;

        // H: 自动模式（非手动）必须有有效车队中心——主车由 SLAM 反推、从车依赖主车广播 fleetPosValid。
        //    自动模式整队姿态始终依赖 Detour（与 MultiVehicleSyncUseDetour 无关）：定位全程丢失时强制停车。
@@ -665,6 +773,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
            var fleetCenterValid = fleetPosValid && (!isMaster || TryInferFleetCenter(out _, out _, out _));
            if (!fleetCenterValid)
            {
+                AddFleetStop("auto fleet center invalid/localization lost", CarNum);
                canMove = false;
                Hedingben.ToastText("自动模式无有效车队中心（定位丢失），已停车", $"MultiVehicle{CarNum}-autostop");
            }
@@ -685,15 +794,10 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
            Hedingben.ToastText("车队检测正常", $"MultiVehicle{CarNum}-stop");
        }

-        VisualizeFleet(contour, layoutX, layoutY, layoutTh);
+        if (fleetStopActive)
+            LogMultiVehicleStop(fleetStopReason);

-        var fleetReady = false;
-        var fleetCount = 0;
-        lock (FleetLock)
-        {
-            fleetCount = MultiVehicleFleet.Count;
-            fleetReady = fleetCount == Conf.MultiVehicleFleetNum;
-        }
+        VisualizeFleet(contour, layoutX, layoutY, layoutTh);

        // 补偿量提到块外，便于诊断日志统一记录三类来源（检测/SLAM）的贡献。
        float xDetectCompensate = 0, yDetectCompensate = 0, thDetectCompensate = 0;
@@ -706,7 +810,16 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                $"fleetCnt={fleetCount}/{Conf.MultiVehicleFleetNum} canMove={canMove} mode={fleetMode} " +
                $"cmdVx={fleetVx:0.000} cmdFTh={fleetFrontTh:0.00} cmdRTh={fleetRearTh:0.00} omega={fleetOmega:0.000}");

-        if (fleetReady)
+        if (fleetStopActive)
+        {
+            chassis.RampStop();
+            SetMultiVehicleMotionFeasible(true);
+            LogMultiVehicleRemoteDecision(
+                $"RAMP_STOP master={isMaster} manual={manualEnabled} auto={autoEnabled} ready={fleetReady} " +
+                $"reason={fleetStopReason}", true);
+        }
+
+        if (fleetReady && !fleetStopActive)
        {
            chassis.SetOriginBias(layoutX, layoutY, layoutTh);
            // E: 统一控制点半径——配置 >0 用配置值，否则取 syncDistance/2（与编队几何一致），不再硬编码 510。
@@ -813,10 +926,18 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                _mvRotCompVy = rotCompVy;
                _mvRotCompOmega = rotCompOmega;

-                chassis.SendRotateMotion(fleetOmega,
+                var motionOk = chassis.SendRotateMotion(fleetOmega,
                    localCompensateX: rotCompVx, localCompensateY: rotCompVy, localCompensateTh: rotCompOmega);
+                SetMultiVehicleMotionFeasible(motionOk, motionOk ? "" : chassis.LastMotionDecomposeFailureReason);
+                if (!motionOk)
+                {
+                    AddFleetStop($"car{CarNum} chassis rotate infeasible: {chassis.LastMotionDecomposeFailureReason}",
+                        CarNum);
+                    canMove = false;
+                    LogMultiVehicleStop(fleetStopReason, true);
+                }
                LogMultiVehicleRemoteDecision(
-                    $"SEND_ROTATE master={isMaster} manual={manualEnabled} canMove={canMove} ready={fleetReady} " +
+                    $"SEND_ROTATE master={isMaster} manual={manualEnabled} canMove={canMove} ready={fleetReady} ok={motionOk} " +
                    $"mode={fleetMode} omega={fleetOmega:0.000} comp=({rotCompVx:0.0},{rotCompVy:0.0},{rotCompOmega:0.000}) " +
                    $"fleetCnt={fleetCount}/{Conf.MultiVehicleFleetNum}");

@@ -835,12 +956,20 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                _rotPoseEpisode = false;
                _rotPosePrevTime = DateTime.MinValue;
                // 常规/蟹行：蟹行时 frontTh==rearTh（四轮同向）即为平移，与常规共用同一下发路径。
-                chassis.SendMotion(fleetVx, fleetFrontTh, fleetRearTh, localControlRadius: controlRadius,
+                var motionOk = chassis.SendMotion(fleetVx, fleetFrontTh, fleetRearTh, localControlRadius: controlRadius,
                    localCompensateX: xDetectCompensate + xPosCompensate,
                    localCompensateY: yDetectCompensate + yPosCompensate,
                    localCompensateTh: thDetectCompensate + thPosCompensate);
+                SetMultiVehicleMotionFeasible(motionOk, motionOk ? "" : chassis.LastMotionDecomposeFailureReason);
+                if (!motionOk)
+                {
+                    AddFleetStop($"car{CarNum} chassis motion infeasible: {chassis.LastMotionDecomposeFailureReason}",
+                        CarNum);
+                    canMove = false;
+                    LogMultiVehicleStop(fleetStopReason, true);
+                }
                LogMultiVehicleRemoteDecision(
-                    $"SEND_MOTION master={isMaster} manual={manualEnabled} canMove={canMove} ready={fleetReady} " +
+                    $"SEND_MOTION master={isMaster} manual={manualEnabled} canMove={canMove} ready={fleetReady} ok={motionOk} " +
                    $"mode={fleetMode} vx={fleetVx:0.000} fTh={fleetFrontTh:0.00} rTh={fleetRearTh:0.00} " +
                    $"comp=({xDetectCompensate + xPosCompensate:0.0},{yDetectCompensate + yPosCompensate:0.0},{thDetectCompensate + thPosCompensate:0.000}) " +
                    $"fleetCnt={fleetCount}/{Conf.MultiVehicleFleetNum}");
@@ -859,12 +988,12 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
            var cy = yDetectCompensate + yPosCompensate;
            var cth = thDetectCompensate + thPosCompensate;
            var crabDbg = isMaster && manualEnabled && fleetMode == 1
-                ? $"| CRAB in({crabInputVx:F3},{crabInputVy:F3}) raw:{crabRawAngle:F2} limit:{crabSteerLimit:F1} rev:{crabReverseEquivalent} "
+                ? $"| CRAB speed:{crabInputVx:F3} steerRatio:{crabInputVy:F3} raw:{crabRawAngle:F2} limit:{crabSteerLimit:F1} rev:{crabReverseEquivalent} "
                : "";

            var dbg =
                $"car{CarNum} master:{isMaster} manual:{manualEnabled} auto:{autoEnabled} slam:{slamRead} corr:{useDetourCorrection} fleetPos:{fleetPosValid} " +
-                $"ready:{fleetReady}({fleetCount}/{Conf.MultiVehicleFleetNum}) canMove:{canMove} useDetect:{Conf.MultiVehicleUseDetect} " +
+                $"ready:{fleetReady}({fleetCount}/{Conf.MultiVehicleFleetNum}) canMove:{canMove} stop:{fleetStopActive} stopReason:{fleetStopReason} useDetect:{Conf.MultiVehicleUseDetect} " +
                $"| BASE vx:{fleetVx:F3} fTh:{fleetFrontTh:F2} rTh:{fleetRearTh:F2} " +
                crabDbg +
                $"| DETECT valid:{detectValid} center({_mvLastDetCenterX:F0},{_mvLastDetCenterY:F0}) dir:{_mvLastDetDir:F1} ndist:{_mvLastDetDist:F0} " +
@@ -882,7 +1011,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
            // 屏幕分两行显示，便于直接观察（无需开启 DLog 磁盘转储）
            Hedingben.ToastText(
                $"BASE vx{fleetVx:F3} fTh{fleetFrontTh:F1} | SEND vx{fleetVx:F3} c({cx:F0},{cy:F0},{cth:F1}) " +
-                $"| ready{fleetReady} canMove{canMove}",
+                $"| ready{fleetReady} canMove{canMove} stop{fleetStopActive}",
                $"MultiVehicle{CarNum}-dbg");
            Hedingben.ToastText(
                $"DET v{detectValid} dx{detectDx:F0} dy{detectDy:F0} dth{detectDth:F1} cmp({xDetectCompensate:F0},{yDetectCompensate:F0},{thDetectCompensate:F1}) " +
@@ -898,7 +1027,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                    layoutX, layoutY, layoutTh, selfAligned, ownDetectOk);
            }

-            if (fleetReady)
+            if (fleetReady || fleetStopActive)
            {
                VehicleSyncNotification notification;
                lock (FleetLock)
@@ -912,11 +1041,14 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
                        CenterTh = CenterTh,
                        Aligned = MultiVehicleAligned,
                        Fleet = new Dictionary<int, VehicleSyncInfo>(MultiVehicleFleet),
-                        FleetVx = fleetVx,
-                        FleetFrontTh = fleetFrontTh,
-                        FleetRearTh = fleetRearTh,
+                        FleetVx = fleetStopActive ? 0 : fleetVx,
+                        FleetFrontTh = fleetStopActive ? 0 : fleetFrontTh,
+                        FleetRearTh = fleetStopActive ? 0 : fleetRearTh,
                        Mode = fleetMode,
-                        FleetOmega = fleetOmega,
+                        FleetOmega = fleetStopActive ? 0 : fleetOmega,
+                        FleetStopActive = fleetStopActive,
+                        FleetStopReason = fleetStopReason,
+                        FleetStopSourceCar = fleetStopSourceCar,
                        AutoEnabled = MultiVehicleAutoEnabled,
                        // 脚本驱动等价于手动联动，广播为 ManualEnabled 让从车解锁跟随。
                        ManualEnabled = manualEnabled,
@@ -1106,7 +1238,9 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
            LayoutY = layoutY,
            LayoutTh = layoutTh,
            Aligned = aligned,
-            DetectOk = detectOk
+            DetectOk = detectOk,
+            MotionFeasible = _multiVehicleMotionFeasible,
+            MotionInfeasibleReason = _multiVehicleMotionInfeasibleReason
        };
    }