集中停车控制配置并完善终点逼近和周期诊断

MultiWheelC/Movements/TrajectoryTrackingMovement.cs

@@ -132,6 +132,21 @@ namespace MultiWheelC
         /// </summary>
         public double? TerminalBrakingPreviewMeters;
 
+        /// <summary>
+        /// 获取或设置本次动作进入终点单向低速逼近的剩余弧长覆盖值，单位为m；为空时读取车辆配置。
+        /// </summary>
+        public double? TerminalApproachDistanceMeters;
+
+        /// <summary>
+        /// 获取或设置本次动作由终点纵向剩余距离生成低速参考的比例增益覆盖值，单位为1/s；为空时读取车辆配置。
+        /// </summary>
+        public double? TerminalApproachGainPerSecond;
+
+        /// <summary>
+        /// 获取或设置本次动作终点单向逼近参考速度的最大绝对值覆盖值，单位为m/s；为空时读取车辆配置。
+        /// </summary>
+        public double? MaximumTerminalApproachSpeedMetersPerSecond;
+
         /// <summary>
         /// 获取或设置本次动作的最大轨迹偏离距离覆盖值，单位为m；为空时读取车辆配置。
         /// </summary>
@@ -212,6 +227,15 @@ namespace MultiWheelC
             var terminalBrakingPreviewMeters =
                 TerminalBrakingPreviewMeters ??
                 config.ParkingTerminalBrakingPreview;
+            var terminalApproachDistanceMeters =
+                TerminalApproachDistanceMeters ??
+                config.ParkingTerminalApproachDistance;
+            var terminalApproachGainPerSecond =
+                TerminalApproachGainPerSecond ??
+                config.ParkingTerminalApproachGain;
+            var maximumTerminalApproachSpeedMetersPerSecond =
+                MaximumTerminalApproachSpeedMetersPerSecond ??
+                config.ParkingTerminalMaximumApproachSpeed;
             var maximumDistanceToTrajectoryMeters =
                 MaximumDistanceToTrajectoryMeters ??
                 config.ParkingMaximumDistanceToTrajectory;
@@ -305,7 +329,10 @@ namespace MultiWheelC
                 finishSpeedMetersPerSecond,
                 finishHeadingToleranceRadians,
                 maximumDistanceToTrajectoryMeters,
-                terminalBrakingPreviewMeters);
+                terminalBrakingPreviewMeters,
+                terminalApproachDistanceMeters,
+                terminalApproachGainPerSecond,
+                maximumTerminalApproachSpeedMetersPerSecond);
 
             var clock = Stopwatch.StartNew();
             var previousCycleSeconds =
@@ -342,10 +369,9 @@ namespace MultiWheelC
                         Controller.ExecuteCycle(
                             deltaTimeSeconds);
 
-                    if (Controller.LastVehicleState.HasValue)
-                    {
-                        CycleObserver?.Invoke(Controller);
-                    }
+                    // 诊断观察器按真实控制周期触发，即使本周期状态不可用，
+                    // 也允许记录状态读取和主动停车所消耗的时间。
+                    CycleObserver?.Invoke(Controller);
 
                     if (result ==
                         ParkingControlCycleResult.Completed)