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

2026-08-11 17:46:52 +08:00
parent 33a33af710
commit 3043febd91
30 changed files with 2946 additions and 30 deletions
@@ -0,0 +1,66 @@
+using System;
+using CommonUsage.Chassis;
+using MyParking.Shared;
+
+namespace MultiWheelC.StateEstimation
+{
+    /// <summary>
+    /// 根据车载配置创建Detour位姿过滤与电机反馈纵向速度组合的停车状态源。
+    /// </summary>
+    public static class ParkingVehicleStateProviderFactory
+    {
+        /// <summary>
+        /// 使用当前车辆运行配置创建停车轨迹控制的默认状态源。
+        /// </summary>
+        public static WheelFeedbackVehicleStateProvider Create(
+            MultiWheelChassis chassis)
+        {
+            return Create(
+                chassis,
+                PilotDefinition.Conf);
+        }
+
+        /// <summary>
+        /// 使用指定配置快照创建停车轨迹控制的默认状态源。
+        /// </summary>
+        public static WheelFeedbackVehicleStateProvider Create(
+            MultiWheelChassis chassis,
+            PilotConfig config)
+        {
+            if (chassis == null)
+            {
+                throw new ArgumentNullException(nameof(chassis));
+            }
+
+            if (config == null)
+            {
+                throw new ArgumentNullException(nameof(config));
+            }
+
+            var velocityEstimator =
+                new VelocityEstimator2D(
+                    config.ParkingDetourLinearVelocityFilterSeconds,
+                    config.ParkingDetourAngularVelocityFilterSeconds);
+            var detourStateProvider =
+                new DetourVehicleStateProvider(
+                    velocityEstimator,
+                    config.ParkingDetourMaximumLinearSpeed,
+                    AngleMath.DegreesToRadians(
+                        config
+                            .ParkingDetourMaximumAngularSpeedDegrees),
+                    config.ParkingDetourPositionJumpMargin,
+                    AngleMath.DegreesToRadians(
+                        config.ParkingDetourHeadingJumpMarginDegrees),
+                    config.ParkingDetourVelocityPositionResidual,
+                    AngleMath.DegreesToRadians(
+                        config
+                            .ParkingDetourVelocityHeadingResidualDegrees),
+                    config.ParkingDetourStationaryConfirmationSeconds);
+
+            return new WheelFeedbackVehicleStateProvider(
+                detourStateProvider,
+                chassis,
+                config.ParkingWheelVelocityFilterSeconds);
+        }
+    }
+}