553 lines
24 KiB
C#
553 lines
24 KiB
C#
// 定义车型、上下层IO、参数和MCU初始化
|
|
using CartActivator;
|
|
using MCUSerialBridgeCLR;
|
|
using MDCSToolBox.Medulla.Chassis.MultiWheel;
|
|
using Medulla.Types;
|
|
using System;
|
|
using System.Collections.Generic;
|
|
using System.Threading;
|
|
using MyParking.Shared;
|
|
|
|
namespace MedullaAdapter
|
|
{
|
|
[UseLadderLogic(logic = typeof(AlarmRoutine), scanInterval = 50)]
|
|
[UseLadderLogic(logic = typeof(MotorRoutine), scanInterval = 50)]
|
|
[UseLadderLogic(logic = typeof(MCURoutine), scanInterval = 20)]
|
|
[UseManualController(manualController = typeof(Remote))]
|
|
public class DiverCartDefinition : MultiWheelCartDefinition
|
|
{
|
|
#region 基本成员
|
|
public MCUSerialBridge Bridge;
|
|
|
|
internal enum ManualControlMode
|
|
{
|
|
Normal = 0, // 正常模式
|
|
Crab = 1, // 螃蟹模式
|
|
Spin = 2, // 自旋模式
|
|
}
|
|
internal ManualControlMode TransmitterControlMode = ManualControlMode.Normal;
|
|
internal DateTime TransmitterLastTime = DateTime.Now; // 物理遥控器计算两次实体遥控器指令之间的时间间隔
|
|
private ManualControlMode? _pendingManualMode;
|
|
private ManualControlMode? _activeManualMode;
|
|
#endregion
|
|
|
|
#region AsUpperIO
|
|
[AsUpperIO(desc = "从C上复位")] public bool ResetFromC;
|
|
[AsUpperIO(desc = "从C将驱动轮下使能")] public bool DisableFromC;
|
|
[AsUpperIO(desc = "左夹臂下发速度", timeOutReset = true)] public float SpeedLeftArm;
|
|
[AsUpperIO(desc = "右夹臂下发速度", timeOutReset = true)] public float SpeedRightArm;
|
|
[AsUpperIO(desc = "夹臂不同步报警")] public bool ClampOutOfSync;
|
|
#endregion
|
|
|
|
#region AsLowerIO
|
|
[AsLowerIO(desc = "左前左轮实际位置")] public float LFLActualPos;
|
|
[AsLowerIO(desc = "左前右轮实际位置")] public float LFRActualPos;
|
|
[AsLowerIO(desc = "右前左轮实际位置")] public float RFLActualPos;
|
|
[AsLowerIO(desc = "右前右轮实际位置")] public float RFRActualPos;
|
|
[AsLowerIO(desc = "左后左轮实际位置")] public float LRLActualPos;
|
|
[AsLowerIO(desc = "左后右轮实际位置")] public float LRRActualPos;
|
|
[AsLowerIO(desc = "右后左轮实际位置")] public float RRLActualPos;
|
|
[AsLowerIO(desc = "右后右轮实际位置")] public float RRRActualPos;
|
|
[AsLowerIO(desc = "左夹臂实际速度")] public float ActualSpeedLeftArm;
|
|
[AsLowerIO(desc = "右夹臂实际速度")] public float ActualSpeedRightArm;
|
|
[AsLowerIO(desc = "左夹臂状态字")] public int LeftArmStateCode;
|
|
[AsLowerIO(desc = "右夹臂状态字")] public int RightArmStateCode;
|
|
[AsLowerIO(desc = "左夹臂错误字")] public int LeftArmErrorCode;
|
|
[AsLowerIO(desc = "右夹臂错误字")] public int RightArmErrorCode;
|
|
[AsLowerIO(desc = "左夹臂电流")] public float LeftArmElectric;
|
|
[AsLowerIO(desc = "右夹臂电流")] public float RightArmElectric;
|
|
[AsLowerIO(desc = "左夹臂实际位置")] public float ActualPosLeftArm;
|
|
[AsLowerIO(desc = "右夹臂实际位置")] public float ActualPosRightArm;
|
|
[AsLowerIO(desc = "驱动轮使能状态")] public bool WheelAbleState = true;
|
|
[AsLowerIO(desc = "电池健康状态")] public float SOH;
|
|
[AsInitParam(desc = "车号")][AsLowerIO] public int CarNum = 1;
|
|
#endregion
|
|
|
|
#region 初始参数
|
|
|
|
[AsInitParam(desc = "差速转舵目标角速度前馈增益")]
|
|
public float DiffSteerRateFeedforwardGain = 0.9f;
|
|
|
|
[AsInitParam(desc = "差速舵轮左右轮间距，单位mm")]
|
|
public float DiffSteerWheelDistanceMillimeters = 85f;
|
|
|
|
[AsInitParam(desc = "差速转舵前馈最大速度，单位m/s")]
|
|
public float DiffSteerRateFeedforwardMaximumSpeed = 0.03f;
|
|
|
|
[AsInitParam(desc = "MCU端口号")] public string MCUPort = "COM4";
|
|
[AsInitParam(desc = "遥控器速度上限")] public float TransmitterSpeedUpperLimit = 1.0f;
|
|
[AsInitParam(desc = "遥控器速度下限")] public float TransmitterSpeedLowerLimit = 0.0f;
|
|
[AsInitParam(desc = "实体遥控器SB模式防抖时间，单位ms")]
|
|
public int TransmitterModeDebounceMilliseconds = 200;
|
|
[AsInitParam(desc = "手动控制夹臂速度系数")] public float ManualArmSpeedFac = 1.0f;
|
|
[AsInitParam(desc = "遥控转弯舵角同步限速宽度，单位为度")]
|
|
public float ManualSteeringAlignmentSigmaDegrees = 8.0f;
|
|
[AsInitParam(desc = "自转最大角速度，单位deg/s")]
|
|
public float MaxSpinAngularSpeedDegreesPerSecond = 30f;
|
|
[AsInitParam(desc = "轮速诊断日志相对目录")]
|
|
public string WheelSpeedDiagnosticDirectory =
|
|
@"logs\wheel-speed";
|
|
[AsInitParam(desc = "左夹臂低限位")][AsLowerIO] public int LeftArmLowerPos = -10000;
|
|
[AsInitParam(desc = "左夹臂高限位")][AsLowerIO] public int LeftArmUpperPos = 5927610;
|
|
[AsInitParam(desc = "右夹臂低限位")][AsLowerIO] public int RightArmLowerPos = -17295;
|
|
[AsInitParam(desc = "右夹臂高限位")][AsLowerIO] public int RightArmUpperPos = 5927610;
|
|
|
|
|
|
#endregion
|
|
|
|
#region 监控参数
|
|
[IOObjectMonitor(desc = "从M上复位")] public bool ResetFromM;
|
|
[IOObjectMonitor(desc = "从M将驱动轮下使能")] public bool DisableFromM;
|
|
[IOObjectMonitor(desc = "左前左轮PID修正后速度")] public float SpeedLFL;
|
|
[IOObjectMonitor(desc = "左前右轮PID修正后速度")] public float SpeedLFR;
|
|
[IOObjectMonitor(desc = "右前左轮PID修正后速度")] public float SpeedRFL;
|
|
[IOObjectMonitor(desc = "右前右轮PID修正后速度")] public float SpeedRFR;
|
|
[IOObjectMonitor(desc = "左后左轮PID修正后速度")] public float SpeedLRL;
|
|
[IOObjectMonitor(desc = "左后右轮PID修正后速度")] public float SpeedLRR;
|
|
[IOObjectMonitor(desc = "右后左轮PID修正后速度")] public float SpeedRRL;
|
|
[IOObjectMonitor(desc = "右后右轮PID修正后速度")] public float SpeedRRR;
|
|
[IOObjectMonitor(desc = "左前舵轮转向PID输出")] public float DiffSteerOutputLeftFront;
|
|
[IOObjectMonitor(desc = "左后舵轮转向PID输出")] public float DiffSteerOutputLeftRear;
|
|
[IOObjectMonitor(desc = "右前舵轮转向PID输出")] public float DiffSteerOutputRightFront;
|
|
[IOObjectMonitor(desc = "右后舵轮转向PID输出")] public float DiffSteerOutputRightRear;
|
|
[IOObjectMonitor(desc = "左前舵轮目标角速度前馈输出")] public float DiffSteerRateFeedforwardLeftFront;
|
|
[IOObjectMonitor(desc = "左后舵轮目标角速度前馈输出")] public float DiffSteerRateFeedforwardLeftRear;
|
|
[IOObjectMonitor(desc = "右前舵轮目标角速度前馈输出")] public float DiffSteerRateFeedforwardRightFront;
|
|
[IOObjectMonitor(desc = "右后舵轮目标角速度前馈输出")] public float DiffSteerRateFeedforwardRightRear;
|
|
[IOObjectMonitor(desc = "左前舵轮转向合成差速输出")] public float DiffSteerTotalOutputLeftFront;
|
|
[IOObjectMonitor(desc = "左后舵轮转向合成差速输出")] public float DiffSteerTotalOutputLeftRear;
|
|
[IOObjectMonitor(desc = "右前舵轮转向合成差速输出")] public float DiffSteerTotalOutputRightFront;
|
|
[IOObjectMonitor(desc = "右后舵轮转向合成差速输出")] public float DiffSteerTotalOutputRightRear;
|
|
[IOObjectMonitor(desc = "灯光模式")] public int LightMode = 0;
|
|
[IOObjectMonitor(desc = "实体遥控器当前速度倍率")] public float TransmitterSpeed = 0.3f;
|
|
[IOObjectMonitor(desc = "轮速诊断记录已启用")]
|
|
public bool WheelSpeedDiagnosticEnabled;
|
|
[IOObjectMonitor(desc = "轮速诊断记录状态")]
|
|
public string WheelSpeedDiagnosticStatus = "未启动";
|
|
[IOObjectMonitor(desc = "左前左驱动器远程帧701")] public byte LFLRemoteCode = 0;
|
|
[IOObjectMonitor(desc = "左前右驱动器远程帧702")] public byte LFRRemoteCode = 0;
|
|
[IOObjectMonitor(desc = "右前左驱动器远程帧703")] public byte RFLRemoteCode = 0;
|
|
[IOObjectMonitor(desc = "右前右驱动器远程帧704")] public byte RFRRemoteCode = 0;
|
|
[IOObjectMonitor(desc = "左后左驱动器远程帧705")] public byte LRLRemoteCode = 0;
|
|
[IOObjectMonitor(desc = "左后右驱动器远程帧706")] public byte LRRRemoteCode = 0;
|
|
[IOObjectMonitor(desc = "右后左驱动器远程帧707")] public byte RRLRemoteCode = 0;
|
|
[IOObjectMonitor(desc = "右后右驱动器远程帧708")] public byte RRRRemoteCode = 0;
|
|
[IOObjectMonitor(desc = "左夹臂驱动器远程帧709")] public byte LArmRemoteCode = 0;
|
|
[IOObjectMonitor(desc = "右夹臂驱动器远程帧70A")] public byte RArmRemoteCode = 0;
|
|
#endregion
|
|
|
|
#region 操作按钮
|
|
// M层单车硬件：向驱动轮发送复位请求。
|
|
[IOObjectUtility]
|
|
public void WheelReset()
|
|
{
|
|
ResetFromM = true;
|
|
}
|
|
// M层单车硬件：向驱动轮发送下使能请求。
|
|
[IOObjectUtility]
|
|
public void WheelDisable()
|
|
{
|
|
DisableFromM = true;
|
|
}
|
|
|
|
// M层诊断：请求开始保存CAN轮速事件和底盘周期快照。
|
|
[IOObjectUtility]
|
|
public void StartWheelSpeedDiagnostic()
|
|
{
|
|
WheelSpeedDiagnosticEnabled = true;
|
|
WheelSpeedDiagnosticStatus = "等待创建日志文件";
|
|
}
|
|
|
|
// M层诊断：请求停止轮速记录并刷新CSV文件。
|
|
[IOObjectUtility]
|
|
public void StopWheelSpeedDiagnostic()
|
|
{
|
|
WheelSpeedDiagnosticEnabled = false;
|
|
WheelSpeedDiagnosticStatus = "等待停止并刷新日志";
|
|
}
|
|
#endregion
|
|
|
|
public override void CommunicationInit()
|
|
{
|
|
if (GhostMode) return;
|
|
State = -1;
|
|
Bridge = new MCUSerialBridge();
|
|
//Step1:打开指定串口连接
|
|
var err = Bridge.Open(MCUPort, 1000000u);
|
|
if (err != MCUSerialBridgeError.OK)
|
|
{
|
|
Console.WriteLine($"MCU Open FAILED: {err.ToDescription()}");
|
|
return;
|
|
}
|
|
else
|
|
{
|
|
Console.WriteLine("MCU Open OK");
|
|
}
|
|
//Step2:远程复位MCU
|
|
err = Bridge.Reset();
|
|
if (err != MCUSerialBridgeError.OK)
|
|
{
|
|
Console.WriteLine($"MCU Reset FAILED: {err.ToDescription()}");
|
|
return;
|
|
}
|
|
else
|
|
{
|
|
Thread.Sleep(500);
|
|
Console.WriteLine("MCU Reset OK");
|
|
}
|
|
//Step3:获取MCU版本号
|
|
err = Bridge.GetVersion(out var version, 100);
|
|
if (err != MCUSerialBridgeError.OK)
|
|
{
|
|
Console.WriteLine($"MCU GetVersion FAILED: {err.ToDescription()}");
|
|
return;
|
|
}
|
|
else
|
|
{
|
|
Console.WriteLine($"MCU GetVersion OK: {version}");
|
|
}
|
|
//Step4:获取MCU状态
|
|
err = Bridge.GetState(out var state, 100);
|
|
if (err != MCUSerialBridgeError.OK)
|
|
{
|
|
Console.WriteLine($"MCU GetState FAILED: {err.ToDescription()}");
|
|
return;
|
|
}
|
|
else
|
|
{
|
|
Console.WriteLine($"MCU GetState OK: {state}");
|
|
}
|
|
//Step5:串口/CAN配置
|
|
try
|
|
{
|
|
var ports = new List<PortConfig>();
|
|
for (int i = 0; i < 1; i++)
|
|
ports.Add(new CANPortConfig(500000, 10));
|
|
for (int i = 0; i < 3; i++)
|
|
ports.Add(new SerialPortConfig(9600, 10));
|
|
Console.WriteLine("=== Port Configuration ===");
|
|
for (int i = 0; i < ports.Count; i++)
|
|
{
|
|
if (ports[i] is SerialPortConfig s)
|
|
Console.WriteLine($"Port {i}: Serial, Baud={s.Baud}, ReceiveFrameMs={s.ReceiveFrameMs}");
|
|
else if (ports[i] is CANPortConfig c)
|
|
Console.WriteLine($"Port {i}: CAN, Baud={c.Baud}, RetryTimeMs={c.RetryTimeMs}");
|
|
}
|
|
|
|
var ret = Bridge.Configure(ports, 200);
|
|
if (ret != MCUSerialBridgeError.OK)
|
|
{
|
|
Console.WriteLine($"MCU Configure FAILED: {ret.ToDescription()}");
|
|
return;
|
|
}
|
|
else
|
|
{
|
|
Console.WriteLine("MCU Configure OK");
|
|
}
|
|
Console.WriteLine("MCU Configure {0}", ret == MCUSerialBridgeError.OK ? "OK" : $"FAILED: 0x{(uint)ret:X8}");
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
Console.WriteLine($"Configure Exception: {ex.Message}");
|
|
return;
|
|
}
|
|
State = 0;
|
|
}
|
|
|
|
internal void ManualControl(
|
|
ManualControlMode mode,
|
|
float x,
|
|
float y,
|
|
float frontDirection,
|
|
float speedThreshold,
|
|
TimeSpan? interval = null)
|
|
{
|
|
if (Chassis == null) return;
|
|
|
|
var adapter = GetChassisAdapter();
|
|
if (adapter == null) return;
|
|
|
|
// 模式变化时先停车并下发舵轮准备角度；
|
|
// 在实际舵角到位之前，不开放驱动速度。
|
|
if (!EnsureManualModeReady(mode, interval))
|
|
{
|
|
adapter.StopImmediately();
|
|
return;
|
|
}
|
|
|
|
var speed = speedThreshold * y;
|
|
var normalizedSteering =
|
|
(float)Math.Pow(
|
|
Math.Abs(x),
|
|
ManualThetaPow) *
|
|
Math.Sign(x);
|
|
var steeringDegrees =
|
|
-normalizedSteering * MaxManualTheta;
|
|
var frontTh = steeringDegrees;
|
|
var rearTh = -steeringDegrees;
|
|
ManualMode = (int)mode;
|
|
|
|
switch (mode)
|
|
{
|
|
case ManualControlMode.Normal:
|
|
// 普通模式统一使用车体速度命令：
|
|
// X向前，行驶中连续改变角速度时舵轮边转、车辆边走。
|
|
// SendBodyCommand(
|
|
// vx: speed,
|
|
// vy: 0.0,
|
|
// omegaRadiansPerSecond: omega,
|
|
// interval);
|
|
Chassis.SendMotion(
|
|
speed,
|
|
frontTh,
|
|
rearTh,
|
|
interval);
|
|
break;
|
|
case ManualControlMode.Crab:
|
|
// 舵轮机械范围为[-120°,120°]。
|
|
// 蟹行后虚拟轴距由原车宽度决定，比正常模式轴距短。
|
|
// 按几何比例缩小转角，使相同摇杆输入获得接近一致的曲率。
|
|
var normalSteeringRadians =
|
|
AngleMath.DegreesToRadians(steeringDegrees);
|
|
var geometryRatio =
|
|
adapter.HalfTrackWidthMeters /
|
|
adapter.HalfWheelBaseMeters;
|
|
|
|
// +90°运动坐标系已经把虚拟左侧映射为车体后方，
|
|
// 此处保持普通模式的转向符号，避免再次取反导致左右颠倒。
|
|
var crabSteeringRadians =
|
|
Math.Atan(
|
|
geometryRatio *
|
|
Math.Tan(
|
|
normalSteeringRadians));
|
|
|
|
// 蟹行转角最终限制为±30°，为±120°机械舵角保留余量。
|
|
var maximumCrabSteeringRadians =
|
|
AngleMath.DegreesToRadians(30.0);
|
|
crabSteeringRadians = Math.Max(
|
|
-maximumCrabSteeringRadians,
|
|
Math.Min(
|
|
maximumCrabSteeringRadians,
|
|
crabSteeringRadians));
|
|
|
|
// 将车体左侧作为虚拟阿克曼车头，并在该运动坐标系中
|
|
// 复用与普通模式相同的SendMotion前后控制点解算。
|
|
if (!adapter.SendVirtualAckermannMotion(
|
|
motionDirectionRadians:
|
|
Math.PI / 2.0,
|
|
speedMetersPerSecond:
|
|
speed,
|
|
steeringRadians:
|
|
crabSteeringRadians,
|
|
interval))
|
|
{
|
|
adapter.StopImmediately();
|
|
|
|
Console.WriteLine(
|
|
"蟹行SendMotion命令分解失败，车辆已经停车：" +
|
|
adapter.LastFailureReason);
|
|
}
|
|
break;
|
|
case ManualControlMode.Spin:
|
|
// 摇杆处于中位时只清零驱动速度，保持已经准备好的
|
|
// 自转舵角；下次推动摇杆时仍会重新检查实际舵角。
|
|
if (Math.Abs(speed) < 1e-6f)
|
|
{
|
|
adapter
|
|
.StopXYThDrivePreserveSteeringState();
|
|
break;
|
|
}
|
|
|
|
// 自转时speed表示最外侧舵轮中心的目标切向速度，
|
|
// 根据v=omega*r换算为SendXYThSpeed需要的角速度。
|
|
var requestedSpinOmegaRadiansPerSecond =
|
|
speed /
|
|
adapter.MaximumWheelRadiusMeters;
|
|
|
|
// 对半径换算结果做正负对称限幅，防止遥控速度参数误设后自转过快。
|
|
var maximumSpinOmegaRadiansPerSecond =
|
|
AngleMath.DegreesToRadians(
|
|
Math.Max(
|
|
0f,
|
|
MaxSpinAngularSpeedDegreesPerSecond));
|
|
|
|
var spinOmegaRadiansPerSecond =
|
|
Math.Max(
|
|
-maximumSpinOmegaRadiansPerSecond,
|
|
Math.Min(
|
|
maximumSpinOmegaRadiansPerSecond,
|
|
requestedSpinOmegaRadiansPerSecond));
|
|
|
|
// 普通安全版SendXYThSpeed只下发角速度，
|
|
// 四轮实际舵角未到位时不会开放驱动速度。
|
|
if (!adapter.Send(
|
|
new ChassisCommand(
|
|
CarNum,
|
|
new Twist2D(
|
|
0.0,
|
|
0.0,
|
|
spinOmegaRadiansPerSecond)),
|
|
interval))
|
|
{
|
|
adapter.StopImmediately();
|
|
|
|
Console.WriteLine(
|
|
"SendXYThSpeed原地自转命令分解失败，车辆已经停车：" +
|
|
adapter.LastFailureReason);
|
|
}
|
|
break;
|
|
default:
|
|
ManualMode = -1;
|
|
adapter.StopImmediately();
|
|
break;
|
|
}
|
|
}
|
|
|
|
// 停车后切换模式：先预转舵轮，实际角度到位后才允许发送运动命令。
|
|
private bool EnsureManualModeReady(
|
|
ManualControlMode mode,
|
|
TimeSpan? interval)
|
|
{
|
|
var adapter = GetChassisAdapter();
|
|
if (adapter == null)
|
|
return false;
|
|
|
|
// 当前模式已经完成准备，可以直接接受运动命令。
|
|
if (_activeManualMode == mode &&
|
|
_pendingManualMode == null)
|
|
{
|
|
return true;
|
|
}
|
|
|
|
// 第一次收到新模式时，停车并下发一次舵轮准备姿态。
|
|
if (_pendingManualMode != mode)
|
|
{
|
|
adapter.StopImmediately();
|
|
// 所有模式的准备角度均按真实机械舵角表达；
|
|
// 先退出上一模式的虚拟运动坐标系，再执行预对齐。
|
|
adapter.ResetToBodyFrame();
|
|
_activeManualMode = null;
|
|
|
|
var preparationAccepted = mode switch
|
|
{
|
|
ManualControlMode.Normal =>
|
|
adapter.PrepareParallelDirection(0.0),
|
|
|
|
ManualControlMode.Crab =>
|
|
adapter.PrepareParallelDirection(
|
|
Math.PI / 2.0),
|
|
|
|
ManualControlMode.Spin =>
|
|
adapter.PrepareSpin(interval),
|
|
|
|
_ => false
|
|
};
|
|
|
|
if (!preparationAccepted)
|
|
{
|
|
_pendingManualMode = null;
|
|
return false;
|
|
}
|
|
|
|
_pendingManualMode = mode;
|
|
return false;
|
|
}
|
|
|
|
// 后续控制周期保持停车，并读取实际舵角判断是否到位。
|
|
adapter.StopImmediately();
|
|
|
|
var toleranceRadians =
|
|
AngleMath.DegreesToRadians(2.0);
|
|
|
|
bool aligned;
|
|
|
|
if (mode == ManualControlMode.Spin)
|
|
{
|
|
// 自转的四个舵轮目标角不同，等待期间持续刷新其目标。
|
|
var preparationAccepted =
|
|
adapter.PrepareSpin(interval);
|
|
|
|
aligned =
|
|
preparationAccepted &&
|
|
adapter.AreSpinWheelsAligned;
|
|
}
|
|
else
|
|
{
|
|
var targetDirection = mode ==
|
|
ManualControlMode.Crab
|
|
? Math.PI / 2.0
|
|
: 0.0;
|
|
|
|
aligned =
|
|
adapter.AreParallelWheelsAligned(
|
|
targetDirection,
|
|
toleranceRadians);
|
|
}
|
|
|
|
if (!aligned)
|
|
return false;
|
|
|
|
if (mode == ManualControlMode.Spin)
|
|
{
|
|
// 四轮实际舵角确认到位后只交接一次，保留PrepareSpin
|
|
// 选定的机械舵角和轮速方向，避免首条XYTh命令重新选角。
|
|
if (!adapter.AdoptPreparedSpinForXYTh(
|
|
toleranceRadians))
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
// 蟹行轮子在真实车体系中到达机械+90°后，
|
|
// 再将车体左侧激活为SendMotion的虚拟X正方向。
|
|
else if (mode == ManualControlMode.Crab)
|
|
{
|
|
adapter.ActivateMotionFrame(
|
|
Math.PI / 2.0);
|
|
}
|
|
else
|
|
{
|
|
adapter.ResetToBodyFrame();
|
|
}
|
|
|
|
_activeManualMode = mode;
|
|
_pendingManualMode = null;
|
|
return true;
|
|
}
|
|
|
|
private MultiWheelChassisAdapter _chassisAdapter;
|
|
|
|
private MultiWheelChassisAdapter GetChassisAdapter()
|
|
{
|
|
if (Chassis == null)
|
|
return null;
|
|
|
|
if (_chassisAdapter == null ||
|
|
_chassisAdapter.VehicleId != CarNum)
|
|
{
|
|
_chassisAdapter =
|
|
new MultiWheelChassisAdapter(Chassis, CarNum);
|
|
}
|
|
|
|
_chassisAdapter.SteeringAlignmentSigmaDegrees =
|
|
Math.Max(
|
|
ManualSteeringAlignmentSigmaDegrees,
|
|
0.1f);
|
|
|
|
return _chassisAdapter;
|
|
}
|
|
|
|
#region MCURoutine兼容参数（暂保留原硬件协议）
|
|
|
|
// 保存MCU读取到的原始输入字节，供M层监控和硬件排查使用。
|
|
[AsLowerIO(desc = "MCU原始输入字节")]
|
|
public float test;
|
|
|
|
// 保留原MCU灯光分支；单车默认值-1表示使用本车LightMode。
|
|
[AsUpperIO(desc = "多车灯光同步兼容值，-1使用本车灯光")]
|
|
public int MultiVehicleLightSync = -1;
|
|
|
|
#endregion
|
|
|
|
}
|
|
}
|