优化差速转舵并增加纵向辨识测试

2026-08-27 12:39:22 +08:00
parent 17092c2766
commit 1c5181b327
24 changed files with 1748 additions and 266 deletions
@@ -16,6 +16,14 @@ namespace MedullaAdapter
    [UseManualController(manualController = typeof(Remote))]
    public class DiverCartDefinition : MultiWheelCartDefinition
    {
+        public DiverCartDefinition()
+        {
+            // 实车配置可覆盖这些回退值；四个舵轮在MotorRoutine中统一读取它们。
+            DiffSteerKp = 0.0042f;
+            DiffSteerKi = 0f;
+            DiffSteerKd = 0f;
+        }
+
        #region 基本成员
        public MCUSerialBridge Bridge;

@@ -65,16 +73,37 @@ namespace MedullaAdapter

        #region  初始参数

-        [AsInitParam(desc = "差速转舵目标角速度前馈增益")]
-        // public float DiffSteerRateFeedforwardGain = 0.9f;
+        // 旧参数仅用于兼容已有配置和历史日志，新模型逆前馈不读取它。
+        [AsInitParam(desc = "旧差速转舵目标角速度前馈增益（已停用）")]
        public float DiffSteerRateFeedforwardGain = 0f;

-        [AsInitParam(desc = "差速舵轮左右轮间距，单位mm")]
+        [AsInitParam(desc = "旧前馈差速舵轮左右轮间距，单位mm（已停用）")]
        public float DiffSteerWheelDistanceMillimeters = 85f;

        [AsInitParam(desc = "差速转舵前馈最大速度，单位m/s")]
        public float DiffSteerRateFeedforwardMaximumSpeed = 0.03f;

+        [AsInitParam(desc = "启用差速舵轮到位迟滞")]
+        public bool EnableDiffSteerSettlingHysteresis = false;
+
+        [AsInitParam(desc = "差速舵轮停止调整误差，单位deg")]
+        public float DiffSteerStopErrorDegrees = 0.5f;
+
+        [AsInitParam(desc = "差速舵轮重新启动误差，单位deg")]
+        public float DiffSteerRestartErrorDegrees = 0.8f;
+
+        [AsInitParam(desc = "差速舵轮到位确认周期数")]
+        public int DiffSteerSettlingCycles = 3;
+
+        [AsInitParam(desc = "启用差速转舵模型逆前馈")]
+        public bool EnableDiffSteerInverseFeedforward = false;
+
+        [AsInitParam(desc = "静止舵轮模型增益")]
+        public float DiffSteerPlantGain = 687.06f;
+
+        [AsInitParam(desc = "模型逆前馈滤波时间常数，单位s")]
+        public float DiffSteerInverseFeedforwardTimeConstantSeconds = 0.15f;
+
        [AsInitParam(desc = "MCU端口号")] public string MCUPort = "COM4";
        [AsInitParam(desc = "遥控器速度上限")] public float TransmitterSpeedUpperLimit = 1.0f;
        [AsInitParam(desc = "遥控器速度下限")] public float TransmitterSpeedLowerLimit = 0.0f;
@@ -119,6 +148,18 @@ namespace MedullaAdapter
        [IOObjectMonitor(desc = "左后舵轮转向合成差速输出")] public float DiffSteerTotalOutputLeftRear;
        [IOObjectMonitor(desc = "右前舵轮转向合成差速输出")] public float DiffSteerTotalOutputRightFront;
        [IOObjectMonitor(desc = "右后舵轮转向合成差速输出")] public float DiffSteerTotalOutputRightRear;
+        [IOObjectMonitor(desc = "左前舵轮进入停止调整区")] public bool DiffSteerInStopZoneLeftFront;
+        [IOObjectMonitor(desc = "左后舵轮进入停止调整区")] public bool DiffSteerInStopZoneLeftRear;
+        [IOObjectMonitor(desc = "右前舵轮进入停止调整区")] public bool DiffSteerInStopZoneRightFront;
+        [IOObjectMonitor(desc = "右后舵轮进入停止调整区")] public bool DiffSteerInStopZoneRightRear;
+        [IOObjectMonitor(desc = "左前舵轮连续到位周期数")] public int DiffSteerSettlingCountLeftFront;
+        [IOObjectMonitor(desc = "左后舵轮连续到位周期数")] public int DiffSteerSettlingCountLeftRear;
+        [IOObjectMonitor(desc = "右前舵轮连续到位周期数")] public int DiffSteerSettlingCountRightFront;
+        [IOObjectMonitor(desc = "右后舵轮连续到位周期数")] public int DiffSteerSettlingCountRightRear;
+        [IOObjectMonitor(desc = "左前舵轮已到位")] public bool DiffSteerSettledLeftFront;
+        [IOObjectMonitor(desc = "左后舵轮已到位")] public bool DiffSteerSettledLeftRear;
+        [IOObjectMonitor(desc = "右前舵轮已到位")] public bool DiffSteerSettledRightFront;
+        [IOObjectMonitor(desc = "右后舵轮已到位")] public bool DiffSteerSettledRightRear;
        [IOObjectMonitor(desc = "灯光模式")] public int LightMode = 0;
        [IOObjectMonitor(desc = "实体遥控器当前速度倍率")] public float TransmitterSpeed = 0.3f;
        [IOObjectMonitor(desc = "轮速诊断记录已启用")]
@@ -154,6 +195,14 @@ namespace MedullaAdapter
        internal float DiffSteerTargetRateRightFrontDegreesPerSecond;
        internal float DiffSteerTargetRateRightRearDegreesPerSecond;
        internal float DiffSteerFeedforwardDeltaTimeMilliseconds;
+        internal float DiffSteerInverseFeedforwardRawLeftFront;
+        internal float DiffSteerInverseFeedforwardRawLeftRear;
+        internal float DiffSteerInverseFeedforwardRawRightFront;
+        internal float DiffSteerInverseFeedforwardRawRightRear;
+        internal float DiffSteerInverseFeedforwardFilteredLeftFront;
+        internal float DiffSteerInverseFeedforwardFilteredLeftRear;
+        internal float DiffSteerInverseFeedforwardFilteredRightFront;
+        internal float DiffSteerInverseFeedforwardFilteredRightRear;
        internal bool DiffSteerFeedforwardLimitedLeftFront;
        internal bool DiffSteerFeedforwardLimitedLeftRear;
        internal bool DiffSteerFeedforwardLimitedRightFront;
@@ -12,14 +12,28 @@ namespace MedullaAdapter
    public class MotorRoutine : LadderLogic<DiverCartDefinition>
    {
        private const double MaximumFeedforwardIntervalSeconds = 0.2;
+
+        private sealed class DiffSteerWheelControlState
+        {
+            public float PreviousTargetAngleDegrees;
+            public double FilteredInverseFeedforwardMetersPerSecond;
+            public bool IsInStopZone;
+            public int SettlingCycleCount;
+            public bool IsSettled;
+        }
+
        private bool _wasTransmitterControlling;
        private DateTime _lastMoveTime = DateTime.Now;
        private bool _diffSteerFeedforwardInitialized;
        private long _lastDiffSteerFeedforwardTimestamp;
-        private float _previousThLeftFront;
-        private float _previousThLeftRear;
-        private float _previousThRightFront;
-        private float _previousThRightRear;
+        private readonly DiffSteerWheelControlState _leftFrontSteerState =
+            new DiffSteerWheelControlState();
+        private readonly DiffSteerWheelControlState _leftRearSteerState =
+            new DiffSteerWheelControlState();
+        private readonly DiffSteerWheelControlState _rightFrontSteerState =
+            new DiffSteerWheelControlState();
+        private readonly DiffSteerWheelControlState _rightRearSteerState =
+            new DiffSteerWheelControlState();
        private DiverCartDefinition.ManualControlMode?
            _pendingTransmitterControlMode;
        private DateTime _pendingTransmitterControlModeSince =
@@ -306,7 +320,16 @@ namespace MedullaAdapter
                out var feedforwardLf,
                out var feedforwardLr,
                out var feedforwardRf,
-                out var feedforwardRr);
+                out var feedforwardRr,
+                out var suppressLf,
+                out var suppressLr,
+                out var suppressRf,
+                out var suppressRr);
+
+            if (suppressLf) feedbackLf = 0f;
+            if (suppressLr) feedbackLr = 0f;
+            if (suppressRf) feedbackRf = 0f;
+            if (suppressRr) feedbackRr = 0f;

            var diffLf = feedbackLf + feedforwardLf;
            var diffLr = feedbackLr + feedforwardLr;
@@ -345,150 +368,383 @@ namespace MedullaAdapter
        }

        /// <summary>
-        /// 根据四个机械目标舵角的实际变化率计算本周期差速轮线速度前馈。
+        /// 更新四轮独立的到位迟滞状态，并计算模型逆前馈。
        /// </summary>
        private void CalculateDiffSteerRateFeedforward(
            out float leftFront,
            out float leftRear,
            out float rightFront,
-            out float rightRear)
+            out float rightRear,
+            out bool suppressLeftFront,
+            out bool suppressLeftRear,
+            out bool suppressRightFront,
+            out bool suppressRightRear)
+        {
+            leftFront = leftRear = rightFront = rightRear = 0f;
+            suppressLeftFront = suppressLeftRear = false;
+            suppressRightFront = suppressRightRear = false;
+            ResetDiffSteerCycleDiagnostics();
+
+            var currentTimestamp = Stopwatch.GetTimestamp();
+            var deltaTimeSeconds = 0.0;
+            var hasValidControlPeriod = false;
+
+            if (_diffSteerFeedforwardInitialized)
+            {
+                deltaTimeSeconds =
+                    (currentTimestamp -
+                     _lastDiffSteerFeedforwardTimestamp) /
+                    (double)Stopwatch.Frequency;
+
+                if (double.IsFinite(deltaTimeSeconds) &&
+                    deltaTimeSeconds > 0.0)
+                {
+                    cart.DiffSteerFeedforwardDeltaTimeMilliseconds =
+                        (float)(deltaTimeSeconds * 1000.0);
+                    hasValidControlPeriod =
+                        deltaTimeSeconds <=
+                        MaximumFeedforwardIntervalSeconds;
+                }
+            }
+
+            suppressLeftFront = UpdateDiffSteerSettlingState(
+                _leftFrontSteerState,
+                cart.ThLeftFront,
+                cart.ActualThLeftFront,
+                hasValidControlPeriod);
+            suppressLeftRear = UpdateDiffSteerSettlingState(
+                _leftRearSteerState,
+                cart.ThLeftRear,
+                cart.ActualThLeftRear,
+                hasValidControlPeriod);
+            suppressRightFront = UpdateDiffSteerSettlingState(
+                _rightFrontSteerState,
+                cart.ThRightFront,
+                cart.ActualThRightFront,
+                hasValidControlPeriod);
+            suppressRightRear = UpdateDiffSteerSettlingState(
+                _rightRearSteerState,
+                cart.ThRightRear,
+                cart.ActualThRightRear,
+                hasValidControlPeriod);
+
+            leftFront = CalculateDiffSteerRateFeedforward(
+                _leftFrontSteerState,
+                cart.ThLeftFront,
+                deltaTimeSeconds,
+                hasValidControlPeriod,
+                suppressLeftFront,
+                out var targetRateLf,
+                out var rawLf,
+                out var limitedLf);
+            leftRear = CalculateDiffSteerRateFeedforward(
+                _leftRearSteerState,
+                cart.ThLeftRear,
+                deltaTimeSeconds,
+                hasValidControlPeriod,
+                suppressLeftRear,
+                out var targetRateLr,
+                out var rawLr,
+                out var limitedLr);
+            rightFront = CalculateDiffSteerRateFeedforward(
+                _rightFrontSteerState,
+                cart.ThRightFront,
+                deltaTimeSeconds,
+                hasValidControlPeriod,
+                suppressRightFront,
+                out var targetRateRf,
+                out var rawRf,
+                out var limitedRf);
+            rightRear = CalculateDiffSteerRateFeedforward(
+                _rightRearSteerState,
+                cart.ThRightRear,
+                deltaTimeSeconds,
+                hasValidControlPeriod,
+                suppressRightRear,
+                out var targetRateRr,
+                out var rawRr,
+                out var limitedRr);
+
+            cart.DiffSteerTargetRateLeftFrontDegreesPerSecond = targetRateLf;
+            cart.DiffSteerTargetRateLeftRearDegreesPerSecond = targetRateLr;
+            cart.DiffSteerTargetRateRightFrontDegreesPerSecond = targetRateRf;
+            cart.DiffSteerTargetRateRightRearDegreesPerSecond = targetRateRr;
+            cart.DiffSteerInverseFeedforwardRawLeftFront = rawLf;
+            cart.DiffSteerInverseFeedforwardRawLeftRear = rawLr;
+            cart.DiffSteerInverseFeedforwardRawRightFront = rawRf;
+            cart.DiffSteerInverseFeedforwardRawRightRear = rawRr;
+            cart.DiffSteerFeedforwardLimitedLeftFront = limitedLf;
+            cart.DiffSteerFeedforwardLimitedLeftRear = limitedLr;
+            cart.DiffSteerFeedforwardLimitedRightFront = limitedRf;
+            cart.DiffSteerFeedforwardLimitedRightRear = limitedRr;
+
+            // 无论周期是否有效，都保存本周期目标，避免补算过期阶跃。
+            _leftFrontSteerState.PreviousTargetAngleDegrees =
+                cart.ThLeftFront;
+            _leftRearSteerState.PreviousTargetAngleDegrees =
+                cart.ThLeftRear;
+            _rightFrontSteerState.PreviousTargetAngleDegrees =
+                cart.ThRightFront;
+            _rightRearSteerState.PreviousTargetAngleDegrees =
+                cart.ThRightRear;
+            _lastDiffSteerFeedforwardTimestamp = currentTimestamp;
+            _diffSteerFeedforwardInitialized = true;
+            UpdateDiffSteerStateDiagnostics();
+        }
+
+        /// <summary>
+        /// 更新单个舵轮的停止区、连续确认和重新启动状态。
+        /// </summary>
+        private bool UpdateDiffSteerSettlingState(
+            DiffSteerWheelControlState state,
+            float targetAngleDegrees,
+            float actualAngleDegrees,
+            bool hasValidControlPeriod)
+        {
+            var stopErrorDegrees = cart.DiffSteerStopErrorDegrees;
+            var restartErrorDegrees = cart.DiffSteerRestartErrorDegrees;
+            var settlingCycles = cart.DiffSteerSettlingCycles;
+            var hasValidAngles =
+                IsFinite(targetAngleDegrees) &&
+                IsFinite(actualAngleDegrees);
+            var hasValidParameters =
+                IsFinite(stopErrorDegrees) &&
+                stopErrorDegrees >= 0f &&
+                IsFinite(restartErrorDegrees) &&
+                restartErrorDegrees > stopErrorDegrees &&
+                settlingCycles > 0;
+
+            if (!hasValidAngles)
+            {
+                state.IsInStopZone = false;
+                state.SettlingCycleCount = 0;
+                state.IsSettled = false;
+                ClearDiffSteerFeedforwardState(state);
+                return true;
+            }
+
+            var absoluteErrorDegrees = Math.Abs(
+                (double)targetAngleDegrees - actualAngleDegrees);
+            state.IsInStopZone =
+                hasValidParameters &&
+                absoluteErrorDegrees <= stopErrorDegrees;
+
+            if (!cart.EnableDiffSteerSettlingHysteresis ||
+                !hasValidParameters)
+            {
+                state.SettlingCycleCount = 0;
+                state.IsSettled = false;
+                return false;
+            }
+
+            if (state.IsSettled)
+            {
+                if (absoluteErrorDegrees <= restartErrorDegrees)
+                {
+                    ClearDiffSteerFeedforwardState(state);
+                    return true;
+                }
+
+                state.IsSettled = false;
+                state.SettlingCycleCount = 0;
+            }
+
+            if (!state.IsInStopZone)
+            {
+                state.SettlingCycleCount = 0;
+                return false;
+            }
+
+            ClearDiffSteerFeedforwardState(state);
+            if (hasValidControlPeriod)
+            {
+                state.SettlingCycleCount = Math.Min(
+                    state.SettlingCycleCount + 1,
+                    settlingCycles);
+                state.IsSettled =
+                    state.SettlingCycleCount >= settlingCycles;
+            }
+            else
+            {
+                state.SettlingCycleCount = 0;
+            }
+
+            return true;
+        }
+
+        /// <summary>
+        /// 将目标机械舵角变化率转换为带一阶滤波的对象逆前馈。
+        /// </summary>
+        private float CalculateDiffSteerRateFeedforward(
+            DiffSteerWheelControlState state,
+            float targetAngleDegrees,
+            double deltaTimeSeconds,
+            bool hasValidControlPeriod,
+            bool suppressOutput,
+            out float targetRateDegreesPerSecond,
+            out float rawInverseFeedforward,
+            out bool limited)
+        {
+            targetRateDegreesPerSecond = 0f;
+            rawInverseFeedforward = 0f;
+            limited = false;
+
+            if (!hasValidControlPeriod ||
+                !IsFinite(targetAngleDegrees) ||
+                !IsFinite(state.PreviousTargetAngleDegrees) ||
+                !double.IsFinite(deltaTimeSeconds) ||
+                deltaTimeSeconds <= 0.0)
+            {
+                ClearDiffSteerFeedforwardState(state);
+                return 0f;
+            }
+
+            // 机械舵角受限，必须使用直接差值而不是圆周最短角差。
+            var targetRate =
+                ((double)targetAngleDegrees -
+                 state.PreviousTargetAngleDegrees) /
+                deltaTimeSeconds;
+            if (!double.IsFinite(targetRate))
+            {
+                ClearDiffSteerFeedforwardState(state);
+                return 0f;
+            }
+
+            targetRateDegreesPerSecond = (float)targetRate;
+            if (!IsFinite(targetRateDegreesPerSecond))
+            {
+                targetRateDegreesPerSecond = 0f;
+                ClearDiffSteerFeedforwardState(state);
+                return 0f;
+            }
+
+            var plantGain = cart.DiffSteerPlantGain;
+            var timeConstantSeconds =
+                cart.DiffSteerInverseFeedforwardTimeConstantSeconds;
+            var maximumSpeed =
+                cart.DiffSteerRateFeedforwardMaximumSpeed;
+
+            if (!IsFinite(plantGain) || plantGain <= 0f ||
+                !IsFinite(timeConstantSeconds) ||
+                timeConstantSeconds <= 0f ||
+                !IsFinite(maximumSpeed) || maximumSpeed <= 0f)
+            {
+                ClearDiffSteerFeedforwardState(state);
+                return 0f;
+            }
+
+            var inverseFeedforward = targetRate / plantGain;
+            if (!double.IsFinite(inverseFeedforward))
+            {
+                ClearDiffSteerFeedforwardState(state);
+                return 0f;
+            }
+
+            rawInverseFeedforward = (float)inverseFeedforward;
+            if (!IsFinite(rawInverseFeedforward))
+            {
+                rawInverseFeedforward = 0f;
+                ClearDiffSteerFeedforwardState(state);
+                return 0f;
+            }
+
+            if (!cart.EnableDiffSteerInverseFeedforward ||
+                suppressOutput)
+            {
+                ClearDiffSteerFeedforwardState(state);
+                return 0f;
+            }
+
+            var alpha = 1.0 - Math.Exp(
+                -deltaTimeSeconds / timeConstantSeconds);
+            var filteredFeedforward =
+                state.FilteredInverseFeedforwardMetersPerSecond +
+                alpha *
+                (inverseFeedforward -
+                 state.FilteredInverseFeedforwardMetersPerSecond);
+            if (!double.IsFinite(alpha) ||
+                !double.IsFinite(filteredFeedforward))
+            {
+                ClearDiffSteerFeedforwardState(state);
+                return 0f;
+            }
+
+            state.FilteredInverseFeedforwardMetersPerSecond =
+                filteredFeedforward;
+
+            var limitedSpeed = (float)Math.Clamp(
+                filteredFeedforward,
+                -maximumSpeed,
+                maximumSpeed);
+            limited = Math.Abs(
+                filteredFeedforward - limitedSpeed) > 1e-9;
+            return limitedSpeed;
+        }
+
+        private void ResetDiffSteerCycleDiagnostics()
        {
-            leftFront = 0f;
-            leftRear = 0f;
-            rightFront = 0f;
-            rightRear = 0f;
            cart.DiffSteerTargetRateLeftFrontDegreesPerSecond = 0f;
            cart.DiffSteerTargetRateLeftRearDegreesPerSecond = 0f;
            cart.DiffSteerTargetRateRightFrontDegreesPerSecond = 0f;
            cart.DiffSteerTargetRateRightRearDegreesPerSecond = 0f;
            cart.DiffSteerFeedforwardDeltaTimeMilliseconds = 0f;
+            cart.DiffSteerInverseFeedforwardRawLeftFront = 0f;
+            cart.DiffSteerInverseFeedforwardRawLeftRear = 0f;
+            cart.DiffSteerInverseFeedforwardRawRightFront = 0f;
+            cart.DiffSteerInverseFeedforwardRawRightRear = 0f;
            cart.DiffSteerFeedforwardLimitedLeftFront = false;
            cart.DiffSteerFeedforwardLimitedLeftRear = false;
            cart.DiffSteerFeedforwardLimitedRightFront = false;
            cart.DiffSteerFeedforwardLimitedRightRear = false;
-
-            var currentTimestamp = Stopwatch.GetTimestamp();
-
-            if (_diffSteerFeedforwardInitialized)
-            {
-                var deltaTimeSeconds =
-                    (currentTimestamp -
-                     _lastDiffSteerFeedforwardTimestamp) /
-                    (double)Stopwatch.Frequency;
-
-                if (deltaTimeSeconds > 0.0 &&
-                    deltaTimeSeconds <=
-                    MaximumFeedforwardIntervalSeconds)
-                {
-                    cart.DiffSteerFeedforwardDeltaTimeMilliseconds =
-                        (float)(deltaTimeSeconds * 1000.0);
-                    leftFront = CalculateDiffSteerRateFeedforward(
-                        cart.ThLeftFront,
-                        _previousThLeftFront,
-                        deltaTimeSeconds,
-                        out var targetRateLf,
-                        out var limitedLf);
-                    leftRear = CalculateDiffSteerRateFeedforward(
-                        cart.ThLeftRear,
-                        _previousThLeftRear,
-                        deltaTimeSeconds,
-                        out var targetRateLr,
-                        out var limitedLr);
-                    rightFront = CalculateDiffSteerRateFeedforward(
-                        cart.ThRightFront,
-                        _previousThRightFront,
-                        deltaTimeSeconds,
-                        out var targetRateRf,
-                        out var limitedRf);
-                    rightRear = CalculateDiffSteerRateFeedforward(
-                        cart.ThRightRear,
-                        _previousThRightRear,
-                        deltaTimeSeconds,
-                        out var targetRateRr,
-                        out var limitedRr);
-
-                    cart.DiffSteerTargetRateLeftFrontDegreesPerSecond =
-                        targetRateLf;
-                    cart.DiffSteerTargetRateLeftRearDegreesPerSecond =
-                        targetRateLr;
-                    cart.DiffSteerTargetRateRightFrontDegreesPerSecond =
-                        targetRateRf;
-                    cart.DiffSteerTargetRateRightRearDegreesPerSecond =
-                        targetRateRr;
-                    cart.DiffSteerFeedforwardLimitedLeftFront = limitedLf;
-                    cart.DiffSteerFeedforwardLimitedLeftRear = limitedLr;
-                    cart.DiffSteerFeedforwardLimitedRightFront = limitedRf;
-                    cart.DiffSteerFeedforwardLimitedRightRear = limitedRr;
-                }
-            }
-
-            _previousThLeftFront = cart.ThLeftFront;
-            _previousThLeftRear = cart.ThLeftRear;
-            _previousThRightFront = cart.ThRightFront;
-            _previousThRightRear = cart.ThRightRear;
-            _lastDiffSteerFeedforwardTimestamp = currentTimestamp;
-            _diffSteerFeedforwardInitialized = true;
        }

        /// <summary>
-        /// 将单个机械目标舵角变化率转换为带限幅的左右轮差速线速度前馈。
+        /// 将四轮独立控制状态复制到M层监控和CSV数据源。
        /// </summary>
-        private float CalculateDiffSteerRateFeedforward(
-            float targetAngleDegrees,
-            float previousTargetAngleDegrees,
-            double deltaTimeSeconds,
-            out float targetRateDegreesPerSecond,
-            out bool limited)
+        private void UpdateDiffSteerStateDiagnostics()
        {
-            targetRateDegreesPerSecond = 0f;
-            limited = false;
+            cart.DiffSteerInStopZoneLeftFront =
+                _leftFrontSteerState.IsInStopZone;
+            cart.DiffSteerInStopZoneLeftRear =
+                _leftRearSteerState.IsInStopZone;
+            cart.DiffSteerInStopZoneRightFront =
+                _rightFrontSteerState.IsInStopZone;
+            cart.DiffSteerInStopZoneRightRear =
+                _rightRearSteerState.IsInStopZone;
+            cart.DiffSteerSettlingCountLeftFront =
+                _leftFrontSteerState.SettlingCycleCount;
+            cart.DiffSteerSettlingCountLeftRear =
+                _leftRearSteerState.SettlingCycleCount;
+            cart.DiffSteerSettlingCountRightFront =
+                _rightFrontSteerState.SettlingCycleCount;
+            cart.DiffSteerSettlingCountRightRear =
+                _rightRearSteerState.SettlingCycleCount;
+            cart.DiffSteerSettledLeftFront =
+                _leftFrontSteerState.IsSettled;
+            cart.DiffSteerSettledLeftRear =
+                _leftRearSteerState.IsSettled;
+            cart.DiffSteerSettledRightFront =
+                _rightFrontSteerState.IsSettled;
+            cart.DiffSteerSettledRightRear =
+                _rightRearSteerState.IsSettled;
+            cart.DiffSteerInverseFeedforwardFilteredLeftFront =
+                (float)_leftFrontSteerState
+                    .FilteredInverseFeedforwardMetersPerSecond;
+            cart.DiffSteerInverseFeedforwardFilteredLeftRear =
+                (float)_leftRearSteerState
+                    .FilteredInverseFeedforwardMetersPerSecond;
+            cart.DiffSteerInverseFeedforwardFilteredRightFront =
+                (float)_rightFrontSteerState
+                    .FilteredInverseFeedforwardMetersPerSecond;
+            cart.DiffSteerInverseFeedforwardFilteredRightRear =
+                (float)_rightRearSteerState
+                    .FilteredInverseFeedforwardMetersPerSecond;
+        }

-            var gain = cart.DiffSteerRateFeedforwardGain;
-            var wheelDistanceMillimeters =
-                cart.DiffSteerWheelDistanceMillimeters;
-            var maximumSpeed =
-                cart.DiffSteerRateFeedforwardMaximumSpeed;
-
-            if (!IsFinite(targetAngleDegrees) ||
-                !IsFinite(previousTargetAngleDegrees) ||
-                !double.IsFinite(deltaTimeSeconds) ||
-                deltaTimeSeconds <= 0.0)
-            {
-                return 0f;
-            }
-
-            // 机械舵角受限，必须使用直接差值而不是圆周最短角差。
-            targetRateDegreesPerSecond =
-                (float)((targetAngleDegrees -
-                         previousTargetAngleDegrees) /
-                        deltaTimeSeconds);
-
-            if (!IsFinite(gain) || gain <= 0f ||
-                !IsFinite(wheelDistanceMillimeters) ||
-                wheelDistanceMillimeters <= 0f ||
-                !IsFinite(maximumSpeed) || maximumSpeed <= 0f)
-            {
-                return 0f;
-            }
-
-            var targetRateRadiansPerSecond =
-                targetRateDegreesPerSecond *
-                Math.PI / 180.0;
-            var wheelDistanceMeters =
-                wheelDistanceMillimeters / 1000.0;
-            var feedforwardSpeed =
-                0.5 *
-                wheelDistanceMeters *
-                targetRateRadiansPerSecond *
-                gain;
-
-            var limitedSpeed = (float)Math.Clamp(
-                feedforwardSpeed,
-                -maximumSpeed,
-                maximumSpeed);
-            limited = Math.Abs(
-                feedforwardSpeed - limitedSpeed) > 1e-9;
-            return limitedSpeed;
+        private static void ClearDiffSteerFeedforwardState(
+            DiffSteerWheelControlState state)
+        {
+            state.FilteredInverseFeedforwardMetersPerSecond = 0.0;
        }

        /// <summary>
@@ -498,6 +754,14 @@ namespace MedullaAdapter
        {
            _diffSteerFeedforwardInitialized = false;
            _lastDiffSteerFeedforwardTimestamp = 0;
+            ResetDiffSteerWheelState(_leftFrontSteerState);
+            ResetDiffSteerWheelState(_leftRearSteerState);
+            ResetDiffSteerWheelState(_rightFrontSteerState);
+            ResetDiffSteerWheelState(_rightRearSteerState);
+            cart.DiffSteerOutputLeftFront = 0f;
+            cart.DiffSteerOutputLeftRear = 0f;
+            cart.DiffSteerOutputRightFront = 0f;
+            cart.DiffSteerOutputRightRear = 0f;
            cart.DiffSteerRateFeedforwardLeftFront = 0f;
            cart.DiffSteerRateFeedforwardLeftRear = 0f;
            cart.DiffSteerRateFeedforwardRightFront = 0f;
@@ -507,6 +771,10 @@ namespace MedullaAdapter
            cart.DiffSteerTargetRateRightFrontDegreesPerSecond = 0f;
            cart.DiffSteerTargetRateRightRearDegreesPerSecond = 0f;
            cart.DiffSteerFeedforwardDeltaTimeMilliseconds = 0f;
+            cart.DiffSteerInverseFeedforwardRawLeftFront = 0f;
+            cart.DiffSteerInverseFeedforwardRawLeftRear = 0f;
+            cart.DiffSteerInverseFeedforwardRawRightFront = 0f;
+            cart.DiffSteerInverseFeedforwardRawRightRear = 0f;
            cart.DiffSteerFeedforwardLimitedLeftFront = false;
            cart.DiffSteerFeedforwardLimitedLeftRear = false;
            cart.DiffSteerFeedforwardLimitedRightFront = false;
@@ -515,6 +783,17 @@ namespace MedullaAdapter
            cart.DiffSteerTotalOutputLeftRear = 0f;
            cart.DiffSteerTotalOutputRightFront = 0f;
            cart.DiffSteerTotalOutputRightRear = 0f;
+            UpdateDiffSteerStateDiagnostics();
+        }
+
+        private static void ResetDiffSteerWheelState(
+            DiffSteerWheelControlState state)
+        {
+            state.PreviousTargetAngleDegrees = 0f;
+            state.FilteredInverseFeedforwardMetersPerSecond = 0.0;
+            state.IsInStopZone = false;
+            state.SettlingCycleCount = 0;
+            state.IsSettled = false;
        }

        /// <summary>
@@ -92,10 +92,17 @@ namespace MedullaAdapter
                    "VoltageV,AlarmLevel,ChassisMode,WheelAbleState," +
                    "DiffSteerKp,DiffSteerKi,DiffSteerKd,DiffSteerMaxI,DiffSteerDeadZone,DiffSteerThresh,DiffSteerSpeedAcc," +
                    "DiffSteerRateFeedforwardGain,DiffSteerWheelDistanceMillimeters,DiffSteerRateFeedforwardMaximumSpeed," +
-                    "FeedforwardDeltaTimeMs," +
+                    "EnableDiffSteerSettlingHysteresis,DiffSteerStopErrorDegrees,DiffSteerRestartErrorDegrees,DiffSteerSettlingCycles," +
+                    "EnableDiffSteerInverseFeedforward,DiffSteerPlantGain,DiffSteerInverseFeedforwardTimeConstantSeconds," +
+                    "FeedforwardDeltaTimeMs,DiffSteerDeltaTimeSeconds," +
                    "TargetRateThLeftFrontDegreesPerSecond,TargetRateThLeftRearDegreesPerSecond," +
                    "TargetRateThRightFrontDegreesPerSecond,TargetRateThRightRearDegreesPerSecond," +
+                    "InverseFeedforwardRawLeftFront,InverseFeedforwardRawLeftRear,InverseFeedforwardRawRightFront,InverseFeedforwardRawRightRear," +
+                    "InverseFeedforwardFilteredLeftFront,InverseFeedforwardFilteredLeftRear,InverseFeedforwardFilteredRightFront,InverseFeedforwardFilteredRightRear," +
                    "FeedforwardLimitedLeftFront,FeedforwardLimitedLeftRear,FeedforwardLimitedRightFront,FeedforwardLimitedRightRear," +
+                    "InStopZoneLeftFront,InStopZoneLeftRear,InStopZoneRightFront,InStopZoneRightRear," +
+                    "SettlingCountLeftFront,SettlingCountLeftRear,SettlingCountRightFront,SettlingCountRightRear," +
+                    "SettledLeftFront,SettledLeftRear,SettledRightFront,SettledRightRear," +
                    "PidOutLeftFront,PidOutLeftRear,PidOutRightFront,PidOutRightRear," +
                    "RateFeedforwardLeftFront,RateFeedforwardLeftRear,RateFeedforwardRightFront,RateFeedforwardRightRear," +
                    "TotalDiffLeftFront,TotalDiffLeftRear,TotalDiffRightFront,TotalDiffRightRear," +
@@ -352,15 +359,46 @@ namespace MedullaAdapter
                Format(cart.DiffSteerRateFeedforwardGain),
                Format(cart.DiffSteerWheelDistanceMillimeters),
                Format(cart.DiffSteerRateFeedforwardMaximumSpeed),
+                FormatBoolean(cart.EnableDiffSteerSettlingHysteresis),
+                Format(cart.DiffSteerStopErrorDegrees),
+                Format(cart.DiffSteerRestartErrorDegrees),
+                Format(cart.DiffSteerSettlingCycles),
+                FormatBoolean(cart.EnableDiffSteerInverseFeedforward),
+                Format(cart.DiffSteerPlantGain),
+                Format(
+                    cart.DiffSteerInverseFeedforwardTimeConstantSeconds),
                Format(cart.DiffSteerFeedforwardDeltaTimeMilliseconds),
+                Format(
+                    cart.DiffSteerFeedforwardDeltaTimeMilliseconds /
+                    1000f),
                Format(cart.DiffSteerTargetRateLeftFrontDegreesPerSecond),
                Format(cart.DiffSteerTargetRateLeftRearDegreesPerSecond),
                Format(cart.DiffSteerTargetRateRightFrontDegreesPerSecond),
                Format(cart.DiffSteerTargetRateRightRearDegreesPerSecond),
+                Format(cart.DiffSteerInverseFeedforwardRawLeftFront),
+                Format(cart.DiffSteerInverseFeedforwardRawLeftRear),
+                Format(cart.DiffSteerInverseFeedforwardRawRightFront),
+                Format(cart.DiffSteerInverseFeedforwardRawRightRear),
+                Format(cart.DiffSteerInverseFeedforwardFilteredLeftFront),
+                Format(cart.DiffSteerInverseFeedforwardFilteredLeftRear),
+                Format(cart.DiffSteerInverseFeedforwardFilteredRightFront),
+                Format(cart.DiffSteerInverseFeedforwardFilteredRightRear),
                FormatBoolean(cart.DiffSteerFeedforwardLimitedLeftFront),
                FormatBoolean(cart.DiffSteerFeedforwardLimitedLeftRear),
                FormatBoolean(cart.DiffSteerFeedforwardLimitedRightFront),
                FormatBoolean(cart.DiffSteerFeedforwardLimitedRightRear),
+                FormatBoolean(cart.DiffSteerInStopZoneLeftFront),
+                FormatBoolean(cart.DiffSteerInStopZoneLeftRear),
+                FormatBoolean(cart.DiffSteerInStopZoneRightFront),
+                FormatBoolean(cart.DiffSteerInStopZoneRightRear),
+                Format(cart.DiffSteerSettlingCountLeftFront),
+                Format(cart.DiffSteerSettlingCountLeftRear),
+                Format(cart.DiffSteerSettlingCountRightFront),
+                Format(cart.DiffSteerSettlingCountRightRear),
+                FormatBoolean(cart.DiffSteerSettledLeftFront),
+                FormatBoolean(cart.DiffSteerSettledLeftRear),
+                FormatBoolean(cart.DiffSteerSettledRightFront),
+                FormatBoolean(cart.DiffSteerSettledRightRear),
                Format(cart.DiffSteerOutputLeftFront),
                Format(cart.DiffSteerOutputLeftRear),
                Format(cart.DiffSteerOutputRightFront),