First_Dev_Dept/ParkingRobot
10
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Initial commit from MyParking project

Browse Source
This commit is contained in:
yuxiang.shen
2026-08-04 10:29:21 +08:00
commit d9432bd529
138 changed files with 16957 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MedullaAdapter/AlarmRoutine.cs
+15
View File
@@ -0,0 +1,15 @@
// 驱动器、急停、夹臂等安全报警
using MDCSToolBox.Medulla.Chassis.MultiWheel;
namespace MedullaAdapter
{
public class AlarmRoutine : MultiWheelAlarmRoutine<DiverCartDefinition>
{
// M层单车安全：汇总停车机器人夹臂等自定义报警状态。
public override void SetOtherAlarms()
{
AddAlarm("左夹臂驱动报警", 2, () => cart.LeftArmErrorCode != 0);
AddAlarm("右夹臂驱动报警", 2, () => cart.RightArmErrorCode != 0);
}
}
}
MedullaAdapter/DiverCartDefinition.cs
+532
View File
@@ -0,0 +1,532 @@
// 定义车型、上下层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 = "MCU端口号")] public string MCUPort = "COM4";
[AsInitParam(desc = "遥控器速度上限")] public float TransmitterSpeedUpperLimit = 1.0f;
[AsInitParam(desc = "遥控器速度下限")] public float TransmitterSpeedLowerLimit = 0.0f;
[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 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
}
}
MedullaAdapter/MCURoutine.cs
+1028
View File
File diff suppressed because it is too large Load Diff
MedullaAdapter/MCUSerialBridgeCLR.cs
+928
View File
@@ -0,0 +1,928 @@
// C#调用MCU通信桥
using System;
using System.Collections.Generic;
using System.Linq;
using System.Runtime.InteropServices;
namespace MCUSerialBridgeCLR
{
/// <summary>
/// 辅助方法与内部 C 结构体封装，用于 P/Invoke 交互
/// </summary>
internal static class PortStructHelper
{
/// <summary>
/// 将任意结构体序列化为字节数组
/// </summary>
/// <typeparam name="T">结构体类型</typeparam>
/// <param name="str">要序列化的结构体</param>
/// <returns>返回结构体对应的字节数组</returns>
/// <remarks>使用 Marshal 分配内存并复制内容</remarks>
public static byte[] StructToBytes<T>(T str)
where T : struct
{
int size = Marshal.SizeOf<T>();
byte[] arr = new byte[size];
IntPtr ptr = Marshal.AllocHGlobal(size);
try
{
Marshal.StructureToPtr(str, ptr, false);
Marshal.Copy(ptr, arr, 0, size);
}
finally
{
Marshal.FreeHGlobal(ptr);
}
return arr;
}
/// <summary>
/// 串口端口配置的原生结构体（与 MCU C 层对应）
/// </summary>
[StructLayout(LayoutKind.Sequential, Pack = 1)]
public struct SerialPortConfigC
{
/// <summary>端口类型（0x01 = Serial）</summary>
public byte port_type;
/// <summary>波特率</summary>
public uint baud;
/// <summary>接收帧时间间隔</summary>
public uint receive_frame_ms;
/// <summary>保留字节，填 0</summary>
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 7)]
public byte[] reserved;
}
/// <summary>
/// CAN 端口配置的原生结构体（与 MCU C 层对应）
/// </summary>
[StructLayout(LayoutKind.Sequential, Pack = 1)]
public struct CANPortConfigC
{
/// <summary>端口类型（0x02 = CAN）</summary>
public byte port_type;
/// <summary>波特率</summary>
public uint baud;
/// <summary>最大重发时间</summary>
public uint retry_time_ms;
/// <summary>保留字节，填 0</summary>
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 7)]
public byte[] reserved;
}
}
/// <summary>
/// MCU 固件版本信息结构体
/// </summary>
[StructLayout(LayoutKind.Sequential, CharSet = CharSet.Ansi)]
public struct VersionInfo
{
/// <summary>产品型号</summary>
[MarshalAs(UnmanagedType.ByValTStr, SizeConst = 16)]
public string ProductionName;
/// <summary>Git 标签</summary>
[MarshalAs(UnmanagedType.ByValTStr, SizeConst = 8)]
public string GitTag;
/// <summary>Git commit 哈希值</summary>
[MarshalAs(UnmanagedType.ByValTStr, SizeConst = 8)]
public string GitCommit;
/// <summary>编译时间（字符串）</summary>
[MarshalAs(UnmanagedType.ByValTStr, SizeConst = 24)]
public string BuildTime;
/// <summary>
/// 转换为可读字符串
/// </summary>
/// <returns>返回包含产品、Tag、Commit、BuildTime 的字符串</returns>
// M层MCU适配：格式化固件版本信息便于日志显示。
public override string ToString()
{
return $"Product: {ProductionName}, Tag: {GitTag}, Commit: {GitCommit}, Built: {BuildTime}";
}
}
/// <summary>
/// MCU 当前运行状态
/// </summary>
[StructLayout(LayoutKind.Explicit)]
public struct MCUState
{
/// <summary>原始 32 位状态值</summary>
[FieldOffset(0)]
public uint RawValue;
/// <summary>状态子字段 0</summary>
[FieldOffset(0)]
public byte Substate0;
/// <summary>状态子字段 1</summary>
[FieldOffset(1)]
public byte Substate1;
/// <summary>状态子字段 2</summary>
[FieldOffset(2)]
public byte Substate2;
/// <summary>高字节模式标志</summary>
[FieldOffset(3)]
public byte Mode;
/// <summary>是否处于 Bridge 模式</summary>
public bool IsBridge => (Mode & 0x80) == 0;
/// <summary>是否处于 DIVER 模式</summary>
public bool IsDIVER => (Mode & 0x80) != 0;
/// <summary>
/// 返回可读的状态字符串
/// </summary>
/// <returns>例如 "Bridge: Running" 或 "DIVER: Error"</returns>
// M层MCU适配：格式化MCU运行状态便于日志显示。
public override string ToString()
{
string modeStr = IsBridge ? "Bridge" : "DIVER";
uint substate = (uint)(Substate0 | (Substate1 << 8) | (Substate2 << 16));
string subStr = substate switch
{
0x00000000 => "Idle",
0x0000000F => "Running",
0x000000FF => "Error",
0x00000001 => "Configured", // DIVER specific
0x8000000F => "Running",
0x800000FF => "Error",
_ => $"Unknown (0x{substate:X6})",
};
return $"{modeStr}: {subStr}";
}
}
/// <summary>
/// 抽象端口配置基类
/// </summary>
public abstract class PortConfig
{
/// <summary>端口类型（由子类实现）</summary>
public abstract byte PortType { get; }
/// <summary>序列化端口配置为字节数组（供 P/Invoke 使用）</summary>
/// <returns>返回固定长度字节数组（16 bytes）</returns>
// M层MCU适配：将端口配置序列化为原生接口字节。
public abstract byte[] ToBytes();
}
/// <summary>
/// 串口配置
/// </summary>
/// <remarks>
/// 构造函数
/// </remarks>
/// <param name="baud">波特率</param>
/// <param name="receiveFrameMs">接收帧间隔</param>
// M层MCU适配：创建串口通信参数配置。
public class SerialPortConfig(uint baud, uint receiveFrameMs) : PortConfig
{
/// <summary>Serial 类型</summary>
public override byte PortType => 0x01;
/// <summary>波特率</summary>
public uint Baud { get; set; } = baud;
/// <summary>接收帧间隔</summary>
public uint ReceiveFrameMs { get; set; } = receiveFrameMs;
/// <summary>
/// 转换为字节数组
/// </summary>
/// <returns>16 字节数组</returns>
// M层MCU适配：序列化串口波特率和组帧时间。
public override byte[] ToBytes()
{
var c = new PortStructHelper.SerialPortConfigC
{
port_type = PortType,
baud = Baud,
receive_frame_ms = ReceiveFrameMs,
reserved = new byte[7],
};
return PortStructHelper.StructToBytes(c);
}
}
/// <summary>
/// CAN 端口配置
/// </summary>
/// <remarks>
/// 构造函数
/// </remarks>
/// <param name="baud">波特率</param>
/// <param name="retryTimeMs">重发间隔</param>
// M层MCU适配：创建CAN通信参数配置。
public class CANPortConfig(uint baud, uint retryTimeMs) : PortConfig
{
/// <summary>CAN 类型</summary>
public override byte PortType => 0x02;
/// <summary>波特率</summary>
public uint Baud { get; set; } = baud;
/// <summary>重发间隔</summary>
public uint RetryTimeMs { get; set; } = retryTimeMs;
/// <summary>
/// 转换为字节数组
/// </summary>
/// <returns>16 字节数组</returns>
// M层MCU适配：序列化CAN波特率和重试时间。
public override byte[] ToBytes()
{
var c = new PortStructHelper.CANPortConfigC
{
port_type = PortType,
baud = Baud,
retry_time_ms = RetryTimeMs,
reserved = new byte[7],
};
return PortStructHelper.StructToBytes(c);
}
}
/// <summary>
/// CAN 帧结构（标准帧 11-bit ID + 1-bit RTR + 4-bit DLC + Payload）
/// </summary>
public class CANMessage
{
/// <summary>标准帧 ID（0~0x7FF，11 位）</summary>
public ushort ID { get; set; }
/// <summary>远程帧标志：false = 数据帧，true = 远程帧</summary>
public bool RTR { get; set; }
/// <summary>数据长度码：0~8</summary>
public byte DLC { get; set; }
/// <summary>数据负载，长度必须严格等于 DLC（DLC=0 时可为 null）</summary>
public byte[] Payload { get; set; }
/// <summary>
/// 序列化为 MCU 协议字节流
/// </summary>
/// <returns>返回字节数组：2 bytes header + Payload</returns>
/// <exception cref="ArgumentOutOfRangeException">如果 DLC > 8</exception>
/// <exception cref="ArgumentException">如果 Payload 长度 != DLC</exception>
// M层CAN适配：将标准或扩展CAN帧序列化为原生布局。
public byte[] ToBytes()
{
if (DLC > 8)
throw new ArgumentOutOfRangeException(nameof(DLC), "DLC must be 0-8");
if (DLC > 0 && (Payload == null || Payload.Length != DLC))
throw new ArgumentException("Payload length must equal DLC");
// 构造 2 字节 header
ushort header = 0;
header |= (ushort)(ID & 0x7FF); // bits 0-10
if (RTR)
header |= (1 << 11); // bit 11
header |= (ushort)((DLC & 0xF) << 12); // bits 12-15
byte[] result = new byte[2 + DLC];
byte[] headerBytes = BitConverter.GetBytes(header); // 小端序
result[0] = headerBytes[0];
result[1] = headerBytes[1];
if (DLC > 0)
Buffer.BlockCopy(Payload, 0, result, 2, DLC);
return result;
}
/// <summary>
/// 反序列化 MCU 协议字节流为 CANMessage
/// </summary>
/// <param name="data">原始字节数组</param>
/// <param name="length">实际有效长度</param>
/// <returns>CANMessage 实例</returns>
/// <exception cref="ArgumentException">数据长度错误</exception>
// M层CAN适配：从原生缓冲区还原CAN消息。
public static CANMessage FromBytes(byte[] data, uint length)
{
if (data == null || length > data.Length || length < 2)
throw new ArgumentException("Data must be at least 2 bytes");
ushort header = BitConverter.ToUInt16(data, 0);
var msg = new CANMessage
{
ID = (ushort)(header & 0x7FF),
RTR = (header & (1 << 11)) != 0,
DLC = (byte)((header >> 12) & 0xF),
};
if (msg.DLC > 0)
{
if (length < 2 + msg.DLC)
throw new ArgumentException("Data length less than DLC");
msg.Payload = new byte[msg.DLC];
Buffer.BlockCopy(data, 2, msg.Payload, 0, msg.DLC);
}
else
{
msg.Payload = Array.Empty<byte>();
}
return msg;
}
// M层CAN诊断：格式化CAN标识符和数据内容。
public override string ToString()
{
string payloadStr =
(Payload == null || Payload.Length == 0)
? "[]"
: "0x[" + string.Join(" ", Payload.Select(b => $"{b:X2}")) + "]";
return $"CANMessage(ID=0x{ID:X3}, RTR={RTR}, DLC={DLC}, Payload={payloadStr})";
}
}
/// <summary>
/// 内部 P/Invoke 层，直接映射 C DLL 函数
/// </summary>
internal static class MCUSerialBridgeCoreAPI
{
private const string DLL = @"mcu_serial_bridge.dll";
[DllImport(DLL, CallingConvention = CallingConvention.Cdecl)]
// M层原生接口：打开MCU串口桥设备。
internal static extern MCUSerialBridgeError msb_open(
out IntPtr handle,
[MarshalAs(UnmanagedType.LPStr)] string port,
uint baud
);
[DllImport(DLL, CallingConvention = CallingConvention.Cdecl)]
// M层原生接口：关闭MCU串口桥设备。
internal static extern MCUSerialBridgeError msb_close(IntPtr handle);
[DllImport(DLL, CallingConvention = CallingConvention.Cdecl)]
// M层原生接口：复位MCU串口桥。
internal static extern MCUSerialBridgeError msb_reset(IntPtr handle, uint timeout_ms);
[DllImport(DLL, CallingConvention = CallingConvention.Cdecl)]
// M层原生接口：读取MCU固件版本。
public static extern MCUSerialBridgeError msb_version(
IntPtr handle,
out VersionInfo version,
uint timeout_ms
);
[DllImport(DLL, CallingConvention = CallingConvention.Cdecl)]
// M层原生接口：读取MCU当前运行状态。
public static extern MCUSerialBridgeError mcu_state(
IntPtr handle,
out MCUState state,
uint timeout
);
[DllImport(DLL, CallingConvention = CallingConvention.Cdecl)]
// M层原生接口：下发串口桥端口配置。
internal static extern MCUSerialBridgeError msb_configure(
IntPtr handle,
uint num_ports,
IntPtr ports,
uint timeout
);
[DllImport(DLL, CallingConvention = CallingConvention.Cdecl)]
// M层原生接口：读取MCU数字输入。
internal static extern MCUSerialBridgeError msb_read_input(
IntPtr handle,
[Out] byte[] inputs,
uint timeout_ms
);
[DllImport(DLL, CallingConvention = CallingConvention.Cdecl)]
// M层原生接口：写入MCU数字输出。
internal static extern MCUSerialBridgeError msb_write_output(
IntPtr handle,
[In] byte[] outputs,
uint timeout_ms
);
[DllImport(DLL, CallingConvention = CallingConvention.Cdecl)]
// M层原生接口：从指定串口或CAN端口读取数据。
internal static extern MCUSerialBridgeError msb_read_port(
IntPtr handle,
byte port_index,
[Out] byte[] dst_data,
uint dst_capacity,
out uint out_length,
uint timeout_ms
);
[UnmanagedFunctionPointer(CallingConvention.Cdecl)]
// M层硬件桥接：定义串口或CAN端口收到原生数据时的回调签名。
internal delegate void msb_on_port_data_callback_function_t(
IntPtr dst_data,
uint dst_data_size,
IntPtr user_ctx
);
[DllImport(DLL, CallingConvention = CallingConvention.Cdecl)]
// M层原生接口：注册端口数据到达回调。
internal static extern MCUSerialBridgeError msb_register_port_data_callback(
IntPtr handle,
byte port_index,
msb_on_port_data_callback_function_t callback,
IntPtr user_ctx
);
[DllImport(DLL, CallingConvention = CallingConvention.Cdecl)]
// M层原生接口：向指定串口或CAN端口写入数据。
internal static extern MCUSerialBridgeError msb_write_port(
IntPtr handle,
byte port_index,
[In] byte[] src_data,
uint src_data_len,
uint timeout_ms
);
}
/// <summary>
/// MCU 串口/端口操作托管封装类
/// 实现 IDisposable 管理底层句柄生命周期
/// </summary>
public class MCUSerialBridge : IDisposable
{
public static uint MaxPortNumber = 16;
private IntPtr nativeHandle = IntPtr.Zero;
/// <summary>判断是否已打开</summary>
public bool IsOpen => nativeHandle != IntPtr.Zero;
/// <summary>构造函数，初始化对象</summary>
// M层MCU适配：创建串口桥包装器并固定原生回调委托。
public MCUSerialBridge()
{
nativeHandle = IntPtr.Zero;
}
/// <summary>析构函数</summary>
// M层MCU适配：对象回收时兜底释放原生串口桥句柄。
~MCUSerialBridge()
{
Dispose(false);
}
/// <summary>显式释放资源</summary>
// M层MCU适配：释放串口桥句柄和非托管资源。
public void Dispose()
{
Dispose(true);
GC.SuppressFinalize(this);
}
/// <summary>内部释放资源方法</summary>
/// <param name="disposing">true 表示手动释放，false 表示析构释放</param>
// M层MCU适配：按托管或终结路径关闭原生句柄。
private void Dispose(bool disposing)
{
if (nativeHandle != IntPtr.Zero)
{
MCUSerialBridgeCoreAPI.msb_close(nativeHandle);
nativeHandle = IntPtr.Zero;
}
}
/// <summary>打开串口</summary>
/// <param name="portName">串口名，如 "COM3"</param>
/// <param name="baud">波特率</param>
/// <returns>错误码</returns>
// M层单车通信：按端口名和波特率连接MCU串口桥。
public MCUSerialBridgeError Open(string portName, uint baud)
{
return MCUSerialBridgeCoreAPI.msb_open(out nativeHandle, portName, baud);
}
/// <summary>关闭串口</summary>
/// <returns>错误码</returns>
// M层单车通信：关闭当前MCU串口桥连接。
public MCUSerialBridgeError Close()
{
if (nativeHandle == IntPtr.Zero)
return MCUSerialBridgeError.Win_HandleNotFound;
MCUSerialBridgeError error = MCUSerialBridgeCoreAPI.msb_close(nativeHandle);
nativeHandle = IntPtr.Zero;
return error;
}
/// <summary>MCU 复位</summary>
/// <returns>错误码</returns>
// M层单车通信：请求MCU复位并等待结果。
public MCUSerialBridgeError Reset(uint timeout = 200)
{
if (nativeHandle == IntPtr.Zero)
return MCUSerialBridgeError.Win_HandleNotFound;
return MCUSerialBridgeCoreAPI.msb_reset(nativeHandle, timeout);
}
/// <summary>获取固件版本</summary>
/// <param name="version">输出版本信息</param>
/// <param name="timeout">超时时间（ms）</param>
/// <returns>错误码</returns>
// M层MCU诊断：读取串口桥固件版本。
public MCUSerialBridgeError GetVersion(out VersionInfo version, uint timeout = 200)
{
version = new VersionInfo();
if (nativeHandle == IntPtr.Zero)
return MCUSerialBridgeError.Win_HandleNotFound;
return MCUSerialBridgeCoreAPI.msb_version(nativeHandle, out version, timeout);
}
/// <summary>获取 MCU 当前状态</summary>
/// <param name="state">输出状态</param>
/// <param name="timeout">超时时间（ms）</param>
/// <returns>错误码</returns>
// M层MCU诊断：读取串口桥运行状态。
public MCUSerialBridgeError GetState(out MCUState state, uint timeout = 200)
{
state = new MCUState();
if (nativeHandle == IntPtr.Zero)
return MCUSerialBridgeError.Win_HandleNotFound;
return MCUSerialBridgeCoreAPI.mcu_state(nativeHandle, out state, timeout);
}
/// <summary>配置端口</summary>
/// <param name="ports">端口集合</param>
/// <param name="timeout">超时时间（ms）</param>
/// <returns>错误码</returns>
// M层MCU适配：批量配置CAN和串口通道参数。
public MCUSerialBridgeError Configure(IEnumerable<PortConfig> ports, uint timeout = 200)
{
if (nativeHandle == IntPtr.Zero)
return MCUSerialBridgeError.Win_HandleNotFound;
if (ports == null)
return MCUSerialBridgeError.Win_InvalidParam;
// 转换成数组
PortConfig[] portArray = ports as PortConfig[] ?? ports.ToArray();
int count = portArray.Length;
// 分配连续原生内存
int structSize = 16; // 每个 PortConfig 固定 16 字节
IntPtr nativePorts = Marshal.AllocHGlobal(structSize * count);
try
{
for (int i = 0; i < count; i++)
{
byte[] bytes = portArray[i].ToBytes();
if (bytes.Length != structSize)
return MCUSerialBridgeError.Win_InvalidParam;
Marshal.Copy(bytes, 0, nativePorts + i * structSize, structSize);
}
// 调用底层 API
return MCUSerialBridgeCoreAPI.msb_configure(
nativeHandle,
(uint)count,
nativePorts,
timeout
);
}
catch
{
return MCUSerialBridgeError.Win_InvalidParam;
}
finally
{
Marshal.FreeHGlobal(nativePorts);
}
}
/// <summary>读取输入（4 字节）</summary>
/// <param name="inputs">输出数组</param>
/// <param name="timeout">超时（ms）</param>
/// <returns>错误码</returns>
// M层单车IO：读取MCU数字输入状态。
public MCUSerialBridgeError ReadInput(out byte[] inputs, uint timeout = 100)
{
inputs = new byte[4];
if (nativeHandle == IntPtr.Zero)
return MCUSerialBridgeError.Win_HandleNotFound;
return MCUSerialBridgeCoreAPI.msb_read_input(nativeHandle, inputs, timeout);
}
/// <summary>写输出（4 字节）</summary>
/// <param name="outputs">数据数组</param>
/// <param name="timeout">超时（ms）</param>
/// <returns>错误码</returns>
// M层单车IO：写入继电器、灯光等数字输出状态。
public MCUSerialBridgeError WriteOutput(byte[] outputs, uint timeout = 100)
{
if (nativeHandle == IntPtr.Zero)
return MCUSerialBridgeError.Win_HandleNotFound;
return MCUSerialBridgeCoreAPI.msb_write_output(nativeHandle, outputs, timeout);
}
/// <summary>
/// 读取 Serial 端口的一帧数据。
/// Serial 上报的数据按帧入队；本接口每次调用最多读取一帧。
/// 注意，如果不及时调用该接口，数据可能会丢失。
/// </summary>
/// <param name="portIndex">Serial 端口索引</param>
/// <param name="buffer">接收到的数据</param>
/// <param name="timeout">
/// 超时时间（毫秒）
/// - 0：不等待，有数据立即返回，没有数据立即返回 MSB_Error_NoData
/// - >0：若当前无数据，最多等待 timeout，期间有新帧到达则立即返回
/// </param>
/// <remarks>
/// 如果已经注册回调，本函数将始终返回 MSB_Error_NoData
/// </remarks>
/// <returns>
/// 错误码 MCUSerialBridgeError
/// - OK 成功读取一帧
/// - NoData 当前无可读数据（仅在 timeout == 0 或等待超时）
/// - Win_InvalidParam 参数错误
/// </returns>
// M层串口通信：同步读取指定MCU串口的数据。
public MCUSerialBridgeError ReadSerial(byte portIndex, out byte[] buffer, uint timeout)
{
buffer = Array.Empty<byte>();
if (nativeHandle == IntPtr.Zero)
return MCUSerialBridgeError.Win_HandleNotFound;
const int MAX_PORT_FRAME = 2048;
byte[] tmp = new byte[MAX_PORT_FRAME];
var err = MCUSerialBridgeCoreAPI.msb_read_port(
nativeHandle,
portIndex,
tmp,
(uint)tmp.Length,
out uint outLen,
timeout
);
if (err != MCUSerialBridgeError.OK)
return err;
buffer = new byte[outLen];
Buffer.BlockCopy(tmp, 0, buffer, 0, (int)outLen);
return MCUSerialBridgeError.OK;
}
/// <summary>
/// 写 Serial 端口数据。
/// 注意：对于单个Serial端口，不支持多线程并行发送，不要在上一条数据没有发送完成之前调用该函数，否则有可能导致数据错误。
/// 注意：超时时间一定要大于波特率和数据长度综合得出的帧时间
/// </summary>
/// <param name="portIndex">Serial 端口索引</param>
/// <param name="data">待发送数据</param>
/// <param name="timeout">超时时间（毫秒）</param>
/// <returns>
/// 错误码 MCUSerialBridgeError
/// - OK 成功发送
/// - 其他错误请查看 MCUSerialBridgeError
/// </returns>
// M层串口通信：向指定MCU串口发送数据。
public MCUSerialBridgeError WriteSerial(byte portIndex, byte[] data, uint timeout)
{
if (nativeHandle == IntPtr.Zero)
return MCUSerialBridgeError.Win_HandleNotFound;
if (data == null || data.Length == 0)
return MCUSerialBridgeError.Win_InvalidParam;
return MCUSerialBridgeCoreAPI.msb_write_port(
nativeHandle,
portIndex,
data,
(uint)data.Length,
timeout
);
}
/// <summary>
/// 读取 CAN 端口的一帧数据。
/// CAN 上报的数据按帧入队；本接口每次调用最多读取一帧。
/// 注意，如果不及时调用该接口，数据可能会丢失。
/// </summary>
/// <param name="portIndex">CAN 端口索引</param>
/// <param name="message">输出 CAN 消息对象</param>
/// <param name="timeout">超时时间（毫秒）</param>
/// <remarks>
/// 如果已经注册回调，本函数将始终返回 MSB_Error_NoData
/// </remarks>
/// <returns>
/// 错误码 MCUSerialBridgeError
/// - OK 成功读取一帧
/// - NoData 当前无可读数据（仅在 timeout == 0 或等待超时）
/// - Win_InvalidParam 参数错误
/// - CAN_DataError CAN数据错误
/// - Win_HandleNotFound 句柄无效
/// </returns>
// M层CAN通信：同步读取指定CAN通道的一帧消息。
public MCUSerialBridgeError ReadCAN(byte portIndex, out CANMessage message, uint timeout)
{
message = null;
if (nativeHandle == IntPtr.Zero)
return MCUSerialBridgeError.Win_HandleNotFound;
const int MAX_FRAME = 16;
byte[] tmp = new byte[MAX_FRAME];
var err = MCUSerialBridgeCoreAPI.msb_read_port(
nativeHandle,
portIndex,
tmp,
(uint)tmp.Length,
out uint outLen,
timeout
);
if (err != MCUSerialBridgeError.OK)
return err;
try
{
message = CANMessage.FromBytes(tmp, outLen);
}
catch
{
return MCUSerialBridgeError.CAN_DataError;
}
return MCUSerialBridgeError.OK;
}
/// <summary>
/// 写 CAN 端口数据。
/// 注意：CAN 支持多线程发送，最多可同时发送 16 个消息。
/// 但是，多个CAN消息会进入排队队列等待发送，如果消息太多，可能引起超时。
/// </summary>
/// <param name="portIndex">CAN 端口索引</param>
/// <param name="message">待发送 CAN 消息对象</param>
/// <param name="timeout">超时时间（毫秒），默认 500ms</param>
/// <returns>
/// 错误码 MCUSerialBridgeError
/// - OK 成功发送
/// - Win_InvalidParam 参数错误
/// - CAN_DataError CAN 数据错误
/// - Win_HandleNotFound 句柄无效
/// </returns>
// M层CAN通信：向指定CAN通道发送一帧消息。
public MCUSerialBridgeError WriteCAN(byte portIndex, CANMessage message, uint timeout)
{
if (nativeHandle == IntPtr.Zero)
return MCUSerialBridgeError.Win_HandleNotFound;
if (message == null)
return MCUSerialBridgeError.Win_InvalidParam;
try
{
byte[] buffer = message.ToBytes();
return MCUSerialBridgeCoreAPI.msb_write_port(
nativeHandle,
portIndex,
buffer,
(uint)buffer.Length,
timeout
);
}
catch
{
return MCUSerialBridgeError.CAN_DataError;
}
}
private readonly Dictionary<
byte,
MCUSerialBridgeCoreAPI.msb_on_port_data_callback_function_t
> _portCallbacks = [];
/// <summary>
/// 注册指定端口(Serial)的回调函数
/// </summary>
/// <param name="portIndex">端口索引</param>
/// <param name="callback">接收数据回调，byte[] 为接收到的原始数据</param>
/// <returns>错误码</returns>
/// <remarks>
/// 注意事项：
/// 1. 回调会在底层 C 层线程中直接调用，请**不要在回调内阻塞**，例如等待 I/O 或 Sleep。
/// 2. 回调内**不能调用 WriteSerial/WriteCAN 等发送函数**，否则可能导致死锁或丢帧。
/// 3. 回调内只能做轻量级操作，例如简单解析、统计或打标记。
/// 4. 若需要复杂处理（例如长时间解析、解码、存储数据库等），请**将数据入队到另一个线程**，再在后台处理。
/// 5. 数据可能随时到来，请保证回调尽快返回，避免影响后续帧接收。
/// 6. 不要把其他类型的端口注册到这个接口，接口不对 portIndex 做类型检查。
/// </remarks>
// M层串口通信：注册指定串口的异步接收回调。
public MCUSerialBridgeError RegisterSerialPortCallback(
byte portIndex,
Action<byte[]> callback
)
{
if (callback == null)
return MCUSerialBridgeError.Win_InvalidParam;
if (portIndex > MaxPortNumber)
return MCUSerialBridgeError.Config_PortNumOver;
// 包装 C# 回调为 P/Invoke 委托
// M层串口回调：复制原生缓存并转交托管回调处理。
void del(IntPtr dst_data, uint dst_data_size, IntPtr user_ctx)
{
byte[] data = new byte[dst_data_size];
Marshal.Copy(dst_data, data, 0, (int)dst_data_size);
callback(data);
}
// 保存引用，防止 GC 回收
_portCallbacks[portIndex] = del;
// 调用 C 层注册
return MCUSerialBridgeCoreAPI.msb_register_port_data_callback(
nativeHandle,
portIndex,
_portCallbacks[portIndex],
IntPtr.Zero
);
}
/// <summary>
/// 注册指定端口(CAN)的回调函数
/// </summary>
/// <param name="portIndex">端口索引</param>
/// <param name="callback">接收数据回调，CANMessage 为接收到的原始数据</param>
/// <returns>错误码</returns>
/// <remarks>
/// 注意事项：
/// 1. 回调会在底层 C 层线程中直接调用，请**不要在回调内阻塞**，例如等待 I/O 或 Sleep。
/// 2. 回调内**不能调用 WriteSerial/WriteCAN 等发送函数**，否则可能导致死锁或丢帧。
/// 3. 回调内只能做轻量级操作，例如简单解析、统计或打标记。
/// 4. 若需要复杂处理（例如长时间解析、解码、存储数据库等），请**将数据入队到另一个线程**，再在后台处理。
/// 5. 数据可能随时到来，请保证回调尽快返回，避免影响后续帧接收。
/// 6. 不要把其他类型的端口注册到这个接口，接口不对 portIndex 做类型检查。
/// </remarks>
// M层CAN通信：注册指定CAN通道的异步接收回调。
public MCUSerialBridgeError RegisterCANPortCallback(
byte portIndex,
Action<CANMessage> callback
)
{
if (callback == null)
return MCUSerialBridgeError.Win_InvalidParam;
if (portIndex > MaxPortNumber)
return MCUSerialBridgeError.Config_PortNumOver;
// 包装 C# 回调为 P/Invoke 委托
// M层CAN回调：还原原生CAN帧并转交托管回调处理。
void del(IntPtr dst_data, uint dst_data_size, IntPtr user_ctx)
{
try
{
byte[] data = new byte[dst_data_size];
Marshal.Copy(dst_data, data, 0, (int)dst_data_size);
CANMessage msg = CANMessage.FromBytes(data, dst_data_size);
callback(msg);
}
catch
{
// 解析失败直接忽略，保证回调不会抛异常阻塞 C 层线程
}
}
// 保存引用，防止 GC 回收
_portCallbacks[portIndex] = del;
// 调用 C 层注册
return MCUSerialBridgeCoreAPI.msb_register_port_data_callback(
nativeHandle,
portIndex,
_portCallbacks[portIndex],
IntPtr.Zero
);
}
}
}
MedullaAdapter/MCUSerialBridgeError.cs
+98
View File
@@ -0,0 +1,98 @@
// MCU通信错误码
namespace MCUSerialBridgeCLR
{
public enum MCUSerialBridgeError : uint
{
OK = 0x00000000, // Success
NoData = 0x00000001, // Port no new data
Win_Unknown = 0x80000001, // Unknown Windows error
Win_InvalidParam = 0x80000002, // Invalid parameter
Win_AllocFail = 0x80000003, // Memory allocation failed
Win_HandleNotFound = 0x80000004, // Handle not found
Win_ResourceBusy = 0x80000005, // Resource busy
Win_BufferFull = 0x80000006, // Buffer is full
Win_UserBufferTooSmall = 0x80000007, // Buffer is too small
Win_CannotOpenPort = 0x80000010, // Cannot open port
Win_CannotGetCommState = 0x80000011, // Cannot get comm state
Win_CannotSetCommState = 0x80000012, // Cannot set comm state
Win_CannotCreateThread = 0x80000013, // Cannot create thread
Proto_Invalid = 0xE0000001, // Protocol invalid
Proto_Checksum = 0xE0000002, // CRC check failed
Proto_Timeout = 0xE0000003, // Protocol timeout
Proto_FrameTooLong = 0xE0000004, // Frame too long
Proto_UnknownCommand = 0xE0000005, // Unknown command
Proto_InvalidPayload = 0xE0000006, // Invalid payload
State_NotRunning = 0xF0000000, // Not running, configure
State_Running = 0xF0000001, // Can not configure, already running
Config_PortNumOver = 0xC0000000, // Port number over
Config_SerialNumOver = 0xC0000001, // Serial port number over
Config_CANNumOver = 0xC0000002, // CAN number over
Config_UnknownPortType = 0xC0000010, // Unknown Port Type
Serial_OpenFail = 0x01000001, // Serial open failed
Serial_NotOpen = 0x01000002, // Serial not open
Serial_ReadFail = 0x01000003, // Serial read failed
Serial_WriteFail = 0x01000004, // Serial write failed
Serial_Busy = 0x01000005, // Serial is busy
CAN_DataError = 0x02000000, // CAN data error
CAN_SendFail = 0x02000001, // CAN send failed
CAN_RecvFail = 0x02000002, // CAN receive failed
CAN_BufferFull = 0x02000003, // CAN buffer full
CAN_NotInit = 0x02000004, // CAN not initialized
Port_WriteBusy = 0x10000001, // Port is busy now
MCU_Unknown = 0x00010001, // MCU unknown error
MCU_IOSizeError = 0x00010002, // IO Should be 4 bytes
MCU_OverTemperature = 0x00010010, // MCU over temperature
}
public static class MCUSerialBridgeErrorExtensions
{
// M层MCU适配：把串口桥错误码转换为便于诊断的说明。
public static string ToDescription(this MCUSerialBridgeError err)
{
return err switch
{
MCUSerialBridgeError.OK => "OK|Success",
MCUSerialBridgeError.NoData => "NoData|Port no new data",
MCUSerialBridgeError.Win_Unknown => "Win_Unknown|Unknown Windows error",
MCUSerialBridgeError.Win_InvalidParam => "Win_InvalidParam|Invalid parameter",
MCUSerialBridgeError.Win_AllocFail => "Win_AllocFail|Memory allocation failed",
MCUSerialBridgeError.Win_HandleNotFound => "Win_HandleNotFound|Handle not found",
MCUSerialBridgeError.Win_ResourceBusy => "Win_ResourceBusy|Resource busy",
MCUSerialBridgeError.Win_BufferFull => "Win_BufferFull|Buffer is full",
MCUSerialBridgeError.Win_UserBufferTooSmall => "Win_UserBufferTooSmall|Buffer is too small",
MCUSerialBridgeError.Win_CannotOpenPort => "Win_CannotOpenPort|Cannot open port",
MCUSerialBridgeError.Win_CannotGetCommState => "Win_CannotGetCommState|Cannot get comm state",
MCUSerialBridgeError.Win_CannotSetCommState => "Win_CannotSetCommState|Cannot set comm state",
MCUSerialBridgeError.Win_CannotCreateThread => "Win_CannotCreateThread|Cannot create thread",
MCUSerialBridgeError.Proto_Invalid => "Proto_Invalid|Protocol invalid",
MCUSerialBridgeError.Proto_Checksum => "Proto_Checksum|CRC check failed",
MCUSerialBridgeError.Proto_Timeout => "Proto_Timeout|Protocol timeout",
MCUSerialBridgeError.Proto_FrameTooLong => "Proto_FrameTooLong|Frame too long",
MCUSerialBridgeError.Proto_UnknownCommand => "Proto_UnknownCommand|Unknown command",
MCUSerialBridgeError.Proto_InvalidPayload => "Proto_InvalidPayload|Invalid payload",
MCUSerialBridgeError.State_NotRunning => "State_NotRunning|Not running, configure",
MCUSerialBridgeError.State_Running => "State_Running|Can not configure, already running",
MCUSerialBridgeError.Config_PortNumOver => "Config_PortNumOver|Port number over",
MCUSerialBridgeError.Config_SerialNumOver => "Config_SerialNumOver|Serial port number over",
MCUSerialBridgeError.Config_CANNumOver => "Config_CANNumOver|CAN number over",
MCUSerialBridgeError.Config_UnknownPortType => "Config_UnknownPortType|Unknown Port Type",
MCUSerialBridgeError.Serial_OpenFail => "Serial_OpenFail|Serial open failed",
MCUSerialBridgeError.Serial_NotOpen => "Serial_NotOpen|Serial not open",
MCUSerialBridgeError.Serial_ReadFail => "Serial_ReadFail|Serial read failed",
MCUSerialBridgeError.Serial_WriteFail => "Serial_WriteFail|Serial write failed",
MCUSerialBridgeError.Serial_Busy => "Serial_Busy|Serial is busy",
MCUSerialBridgeError.CAN_DataError => "CAN_DataError|CAN data error",
MCUSerialBridgeError.CAN_SendFail => "CAN_SendFail|CAN send failed",
MCUSerialBridgeError.CAN_RecvFail => "CAN_RecvFail|CAN receive failed",
MCUSerialBridgeError.CAN_BufferFull => "CAN_BufferFull|CAN buffer full",
MCUSerialBridgeError.CAN_NotInit => "CAN_NotInit|CAN not initialized",
MCUSerialBridgeError.Port_WriteBusy => "Port_WriteBusy|Port is busy now",
MCUSerialBridgeError.MCU_Unknown => "MCU_Unknown|MCU unknown error",
MCUSerialBridgeError.MCU_IOSizeError => "MCU_IOSizeError|IO Should be 4 bytes",
MCUSerialBridgeError.MCU_OverTemperature => "MCU_OverTemperature|MCU over temperature",
_ => "Unknown Error",
};
}
}
}
MedullaAdapter/MedullaAdapter.csproj
+58
View File
@@ -0,0 +1,58 @@
<Project Sdk="Microsoft.NET.Sdk">
<PropertyGroup>
<TargetFramework>net8.0</TargetFramework>
<ImplicitUsings>enable</ImplicitUsings>
<Nullable>disable</Nullable>
<Platforms>AnyCPU</Platforms>
<AssemblyName>MedullaAdapter</AssemblyName>
<AppendTargetFrameworkToOutputPath>false</AppendTargetFrameworkToOutputPath>
<OutputPath>build\Medulla\plugins\</OutputPath>
</PropertyGroup>
<ItemGroup>
<Reference Include="CartActivator">
<HintPath>ref\RefCartActivator.dll</HintPath>
<Private>false</Private>
</Reference>
<Reference Include="MedullaCore">
<HintPath>ref\RefMedullaCore.dll</HintPath>
<Private>false</Private>
</Reference>
<Reference Include="FundamentalLib">
<HintPath>ref\RefFundamentalLib.dll</HintPath>
<Private>false</Private>
</Reference>
<Reference Include="MDCSToolBox">
<HintPath>ref\MDCSToolBox.dll</HintPath>
<Private>false</Private>
</Reference>
<!-- <Reference Include="CycleGUI">
<HintPath>ref\CycleGUI.dll</HintPath>
<Private>false</Private>
</Reference> -->
<Reference Include="CommonUsage">
<HintPath>..\ref\CommonUsage.dll</HintPath>
</Reference>
</ItemGroup>
<ItemGroup>
<Compile Include="..\Shared\Models\ChassisCommand.cs"
Link="Shared\Models\ChassisCommand.cs" />
<Compile Include="..\Shared\Mathematics\FrameTransform2D.cs"
Link="Shared\Mathematics\FrameTransform2D.cs" />
<Compile Include="..\Shared\Mathematics\AngleMath.cs"
Link="Shared\Mathematics\AngleMath.cs" />
<Compile Include="..\Shared\Chassis\MultiWheelChassisAdapter.cs"
Link="Shared\Chassis\MultiWheelChassisAdapter.cs" />
</ItemGroup>
</Project>
MedullaAdapter/MotorRoutine.cs
+364
View File
@@ -0,0 +1,364 @@
// 计算8个驱动电机的目标速度和舵角PID
using CartActivator;
using FundamentalLib;
using MDCSToolBox.Commons;
using System;
using static MDCSToolBox.Medulla.Chassis.BasicCartDefinition;
namespace MedullaAdapter
{
public class MotorRoutine : LadderLogic<DiverCartDefinition>
{
private bool _wasTransmitterControlling;
private DateTime _lastMoveTime = DateTime.Now;
public override void Operation(int iteration)
{
if (!cart.GhostMode && cart.State == -1) return;
// SA稳定打开后，使能实体遥控器。
TriggerOnce(
cart.TransmitterConnected && cart.Transmitter_SA,
300,
() =>
{
cart.TransmitterControlEnable = true;
cart.TransmitterLastTime = DateTime.Now;
});
// 遥控器断连或SA关闭时，立即撤销遥控使能。
if (!cart.TransmitterConnected || !cart.Transmitter_SA)
cart.TransmitterControlEnable = false;
var transmitterSelected =
cart.TransmitterConnected &&
cart.TransmitterControlEnable &&
cart.Transmitter_SA &&
cart.Transmitter_SC == cart.CarNum;
var transmitterControlling =
transmitterSelected &&
CartDefinition.testPriority(5, "TransmitterMode");
if (transmitterControlling)
{
TransmitterChassisControl();
cart.TransmitterLastTime = DateTime.Now;
cart.CarStatu = "实体遥控器控制";
}
else
{
// 只在遥控器刚刚退出时发送一次停车，
// 不能每周期停车，否则会覆盖C层轨迹控制。
if (_wasTransmitterControlling)
{
cart.ManualControl(
cart.TransmitterControlMode,
0, 0, 0,
cart.TransmitterSpeed,
DateTime.Now - cart.TransmitterLastTime);
StopClampArms();
cart.TransmitterLastTime = DateTime.Now;
}
cart.CarStatu = "正常运行";
}
_wasTransmitterControlling = transmitterControlling;
// 当前是否由C层控制。
cart.ClumsyControl = CartDefinition.currentPriority == 0;
// 计算四个舵轮PID和8个驱动电机最终速度。
UpdateDiffSteerWheelSpeeds();
// 平滑更新硬件速度限制。
UpdateSendSpeedLimit();
// 更新红黄绿灯状态。
UpdateLightMode();
}
// 物理遥控器设置
public void TransmitterChassisControl()
{
var interval = DateTime.Now - cart.TransmitterLastTime;
switch (cart.Transmitter_SB)
{
case TransmitterState.Mode0:
cart.TransmitterControlMode =
DiverCartDefinition.ManualControlMode.Normal;
break;
case TransmitterState.Mode1:
cart.TransmitterControlMode =
DiverCartDefinition.ManualControlMode.Crab;
break;
case TransmitterState.Mode2:
cart.TransmitterControlMode =
DiverCartDefinition.ManualControlMode.Spin;
break;
default:
cart.ManualControl(
cart.TransmitterControlMode,
0, 0, 0,
cart.TransmitterSpeed,
interval);
StopClampArms();
return;
}
// SA关闭后立即停车。
if (!cart.Transmitter_SA)
{
cart.ManualControl(
cart.TransmitterControlMode,
0, 0, 0,
cart.TransmitterSpeed,
interval);
StopClampArms();
return;
}
// 限制实体遥控器的最大速度。
cart.TransmitterSpeed = Math.Max(
cart.TransmitterSpeedLowerLimit,
Math.Min(
cart.TransmitterSpeed,
cart.TransmitterSpeedUpperLimit));
// SD的Mode0作为底盘驾驶档。
if (cart.Transmitter_SD == TransmitterState.Mode0)
{
// 底盘驾驶档不允许保留上一周期的夹臂速度。
StopClampArms();
cart.ManualControl(
cart.TransmitterControlMode,
cart.TransmitterLeftJoystickValX,
cart.TransmitterRightJoystickValY,
0,
cart.TransmitterSpeed,
interval);
return;
}
if (cart.Transmitter_SD == TransmitterState.Mode1)
{
// 切换到夹臂档时，先确保底盘停止。
cart.ManualControl(
cart.TransmitterControlMode,
0, 0, 0,
cart.TransmitterSpeed,
interval);
var armSpeed =
cart.TransmitterRightJoystickValX *
cart.ManualArmSpeedFac;
cart.SpeedLeftArm = armSpeed;
cart.SpeedRightArm = armSpeed;
return;
}
// 非驾驶档必须主动停车，防止上一条运动指令残留。
cart.ManualControl(
cart.TransmitterControlMode,
0, 0, 0,
cart.TransmitterSpeed,
interval);
StopClampArms();
}
// M层单车夹臂安全：清除物理遥控器留下的左右夹臂速度命令。
private void StopClampArms()
{
cart.SpeedLeftArm = 0;
cart.SpeedRightArm = 0;
}
// M层单车底盘：根据四个舵轮的目标角度和实际角度修正8个驱动电机速度。
private void UpdateDiffSteerWheelSpeeds()
{
if (cart.LeftFrontPid == null ||
cart.LeftRearPid == null ||
cart.RightFrontPid == null ||
cart.RightRearPid == null)
{
cart.SpeedLFL = 0;
cart.SpeedLFR = 0;
cart.SpeedRFL = 0;
cart.SpeedRFR = 0;
cart.SpeedLRL = 0;
cart.SpeedLRR = 0;
cart.SpeedRRL = 0;
cart.SpeedRRR = 0;
return;
}
// 更新左前舵轮PID参数。
cart.LeftFrontPid.ChangeParameters(
cart.DiffSteerKp,
cart.DiffSteerKi,
cart.DiffSteerKd,
cart.DiffSteerMaxI,
cart.DiffSteerDeadZone,
cart.DiffSteerThresh,
cart.DiffSteerSpeedAcc);
// 更新左后舵轮PID参数。
cart.LeftRearPid.ChangeParameters(
cart.DiffSteerKp,
cart.DiffSteerKi,
cart.DiffSteerKd,
cart.DiffSteerMaxI,
cart.DiffSteerDeadZone,
cart.DiffSteerThresh,
cart.DiffSteerSpeedAcc);
// 更新右前舵轮PID参数。
cart.RightFrontPid.ChangeParameters(
cart.DiffSteerKp,
cart.DiffSteerKi,
cart.DiffSteerKd,
cart.DiffSteerMaxI,
cart.DiffSteerDeadZone,
cart.DiffSteerThresh,
cart.DiffSteerSpeedAcc);
// 更新右后舵轮PID参数。
cart.RightRearPid.ChangeParameters(
cart.DiffSteerKp,
cart.DiffSteerKi,
cart.DiffSteerKd,
cart.DiffSteerMaxI,
cart.DiffSteerDeadZone,
cart.DiffSteerThresh,
cart.DiffSteerSpeedAcc);
// 根据实际舵角计算四条腿的差速修正量。
var diffLf = cart.LeftFrontPid.GetResponse(
cart.ThLeftFront, false, false, "LF");
var diffLr = cart.LeftRearPid.GetResponse(
cart.ThLeftRear, false, false, "LR");
var diffRf = cart.RightFrontPid.GetResponse(
cart.ThRightFront, false, false, "RF");
var diffRr = cart.RightRearPid.GetResponse(
cart.ThRightRear, false, false, "RR");
// 保存四个转向PID的本周期修正量，供M层监控和舵轮响应CSV记录使用。
cart.DiffSteerOutputLeftFront = diffLf;
cart.DiffSteerOutputLeftRear = diffLr;
cart.DiffSteerOutputRightFront = diffRf;
cart.DiffSteerOutputRightRear = diffRr;
// 左前腿：左右电机施加方向相反的PID修正量。
cart.SpeedLFL = cart.SpeedLeftFrontLeft - diffLf;
cart.SpeedLFR = cart.SpeedLeftFrontRight + diffLf;
// 左后腿。
cart.SpeedLRL = cart.SpeedLeftRearLeft - diffLr;
cart.SpeedLRR = cart.SpeedLeftRearRight + diffLr;
// 右前腿。
cart.SpeedRFL = cart.SpeedRightFrontLeft - diffRf;
cart.SpeedRFR = cart.SpeedRightFrontRight + diffRf;
// 右后腿。
cart.SpeedRRL = cart.SpeedRightRearLeft - diffRr;
cart.SpeedRRR = cart.SpeedRightRearRight + diffRr;
}
// M层单车限速：按照加速度和减速度平滑更新实际下发速度上限。
private void UpdateSendSpeedLimit()
{
if (cart.Chassis == null)
{
cart.SendThresSpeed = 0;
return;
}
var now = DateTime.Now;
var elapsedSeconds = (float)(now - _lastMoveTime).TotalSeconds;
_lastMoveTime = now;
// 防止调试暂停或线程卡顿后，一次产生过大的速度跳变。
elapsedSeconds = Math.Clamp(elapsedSeconds, 0f, 0.2f);
// 限速值不允许小于零。
var targetLimit = Math.Max(0f, cart.ThresSpeed);
var currentLimit = Math.Max(0f, cart.SendThresSpeed);
// 增大速度上限时用加速度，减小时用减速度。
var speedChangingRate =
targetLimit > currentLimit
? cart.Chassis.AccPerSecond
: cart.Chassis.DeAccPerSecond;
speedChangingRate = Math.Max(0f, speedChangingRate);
var maxChange = speedChangingRate * elapsedSeconds;
var speedDifference = targetLimit - currentLimit;
if (Math.Abs(speedDifference) <= maxChange)
{
currentLimit = targetLimit;
}
else
{
currentLimit += Math.Sign(speedDifference) * maxChange;
}
// 计算当前8个电机目标速度中的最大绝对值。
var wheelMaxSpeed = 0f;
wheelMaxSpeed = Math.Max(wheelMaxSpeed, Math.Abs(cart.SpeedLFL));
wheelMaxSpeed = Math.Max(wheelMaxSpeed, Math.Abs(cart.SpeedLFR));
wheelMaxSpeed = Math.Max(wheelMaxSpeed, Math.Abs(cart.SpeedRFL));
wheelMaxSpeed = Math.Max(wheelMaxSpeed, Math.Abs(cart.SpeedRFR));
wheelMaxSpeed = Math.Max(wheelMaxSpeed, Math.Abs(cart.SpeedLRL));
wheelMaxSpeed = Math.Max(wheelMaxSpeed, Math.Abs(cart.SpeedLRR));
wheelMaxSpeed = Math.Max(wheelMaxSpeed, Math.Abs(cart.SpeedRRL));
wheelMaxSpeed = Math.Max(wheelMaxSpeed, Math.Abs(cart.SpeedRRR));
// 没有必要让平滑限速值高于当前所有车轮需要的速度。
if (targetLimit < wheelMaxSpeed &&
currentLimit > wheelMaxSpeed)
{
currentLimit = wheelMaxSpeed;
}
cart.SendThresSpeed = currentLimit;
}
// M层单车灯光：根据报警、驱动器、限速和电量状态生成红黄绿灯模式。
private void UpdateLightMode()
{
// 二级报警：红灯常亮。
if (cart.AlarmLevel == 2)
{
cart.LightMode = 2;
return;
}
// 一级报警：黄灯常亮。
if (cart.AlarmLevel == 1)
{
cart.LightMode = 3;
return;
}
// 驱动轮未使能：黄灯闪烁。
if (!cart.WheelAbleState)
{
FlipFlop(ref cart.LightMode, 500, 0, 3);
return;
}
// C层正在进行限速：黄灯常亮。
if (Math.Abs(cart.ThresSpeed - 1f) > 0.001f)
{
cart.LightMode = 3;
return;
}
// 电量低于预警值：黄灯常亮。
if (cart.Soc <= cart.LowBatteryAlarmThreshold)
{
cart.LightMode = 3;
return;
}
// 正常运行：绿灯闪烁。
FlipFlop(ref cart.LightMode, 500, 0, 1);
}
}
}
MedullaAdapter/Remote.cs
+86
View File
@@ -0,0 +1,86 @@
// Medulla虚拟遥控器和夹臂控制
using MDCSToolBox.Medulla.Chassis.MultiWheel;
using CartActivator;
namespace MedullaAdapter
{
// M层单车虚拟遥控器：使用父类提供的底盘控制界面。
public class Remote : MultiWheelRemote<DiverCartDefinition>
{
// 将父类虚拟遥控器界面输入统一转发到本车的ManualControl。
public override void ChassisOperation()
{
if (MultiVehicleMode.on)
{
MultiVehicleModeChassisLogic();
statusText = "单车版本不支持多车联动遥控";
return;
}
// 当前单车适配层只定义Normal、Crab和Spin三种模式。
// 禁止这些旧按钮绕过适配层直接修改底盘坐标偏置。
if (AckermannMode.on ||
SwayMode.on ||
XYThMode.on)
{
cart.Chassis?.PredefinedDriveStop();
statusText = "当前单车版本暂不支持阿克曼、斜行或全向模式";
return;
}
var mode = SpinMode.on
? DiverCartDefinition.ManualControlMode.Spin
: CrabMode.on
? DiverCartDefinition.ManualControlMode.Crab
: DiverCartDefinition.ManualControlMode.Normal;
cart.ManualControl(
mode,
SpeedPad.x,
SpeedPad.y,
FrontDirection.dval * 180,
SpeedThreshold.val);
statusText =
$"{mode}, x={SpeedPad.x:0.00}, " +
$"y={SpeedPad.y:0.00}, " +
$"speed={SpeedThreshold.val:0.00}";
}
[AsControlItem(name = "夹抱速度", LayoutRow = 0, LayoutCol = 4)]
public Throttle ArmSpeed;
[AsControlItem(name = "夹抱打开", LayoutRow = 1, LayoutCol = 0)]
public Button Open;
[AsControlItem(name = "夹抱关闭", LayoutRow = 1, LayoutCol = 2)]
public Button Close;
// M层虚拟遥控器：控制左右夹臂同步打开或关闭。
public override void CustomOperation()
{
if (Open.pressed)
{
cart.SpeedLeftArm =
-ArmSpeed.val * cart.ManualArmSpeedFac;
cart.SpeedRightArm =
-ArmSpeed.val * cart.ManualArmSpeedFac;
}
else if (Close.pressed)
{
cart.SpeedLeftArm =
ArmSpeed.val * cart.ManualArmSpeedFac;
cart.SpeedRightArm =
ArmSpeed.val * cart.ManualArmSpeedFac;
}
else
{
cart.SpeedLeftArm = 0;
cart.SpeedRightArm = 0;
}
}
// 单车不支持多车联动，误打开多车开关时主动停车。
public override void MultiVehicleModeChassisLogic()
{
cart.Chassis?.PredefinedDriveStop();
}
}
}
MedullaAdapter/WheelSpeedDiagnosticLogger.cs
+381
View File
@@ -0,0 +1,381 @@
using System;
using System.Collections.Concurrent;
using System.Diagnostics;
using System.Globalization;
using System.IO;
using System.Text;
using System.Threading;
namespace MedullaAdapter
{
/// <summary>
/// 在后台保存驱动器CAN速度事件和底盘周期快照，避免文件IO阻塞CAN回调。
/// </summary>
internal sealed class WheelSpeedDiagnosticLogger : IDisposable
{
private readonly struct LogRecord
{
public LogRecord(bool isCanEvent, string line)
{
IsCanEvent = isCanEvent;
Line = line;
}
public bool IsCanEvent { get; }
public string Line { get; }
}
private const int MaximumQueuedRecords = 100000;
private const double SnapshotIntervalMilliseconds = 20.0;
private readonly ConcurrentQueue<LogRecord> _records = new();
private readonly AutoResetEvent _recordsAvailable = new(false);
private readonly object _lifecycleLock = new();
private Stopwatch _stopwatch;
private Thread _writerThread;
private StreamWriter _canWriter;
private StreamWriter _snapshotWriter;
private volatile bool _isRunning;
private int _queuedRecordCount;
private long _receiveSequence;
private long _droppedRecordCount;
private double _lastSnapshotMilliseconds = double.NegativeInfinity;
public bool IsRunning => _isRunning;
public string CanLogPath { get; private set; } = "";
public string SnapshotLogPath { get; private set; } = "";
/// <summary>
/// 创建本次诊断的两个CSV文件并启动后台写入线程。
/// </summary>
public void Start(string directory, int carNumber)
{
lock (_lifecycleLock)
{
if (_isRunning)
return;
if (string.IsNullOrWhiteSpace(directory))
throw new ArgumentException(
"轮速诊断目录不能为空。",
nameof(directory));
Directory.CreateDirectory(directory);
var filePrefix =
$"{DateTime.Now:yyyyMMdd_HHmmss_fff}_Car{carNumber}";
CanLogPath = Path.Combine(
directory,
$"{filePrefix}_can.csv");
SnapshotLogPath = Path.Combine(
directory,
$"{filePrefix}_snapshot.csv");
_canWriter = CreateWriter(CanLogPath);
_snapshotWriter = CreateWriter(SnapshotLogPath);
_canWriter.WriteLine(
"ElapsedMs,ReceiveSequence,CanId,MotorName,RawRpm,SpeedMps");
_snapshotWriter.WriteLine(
"ElapsedMs,CarNum,ManualControlMode,ManualMode,SendThresSpeed," +
"DiffSteerKp,DiffSteerKi,DiffSteerKd,DiffSteerMaxI,DiffSteerDeadZone,DiffSteerThresh,DiffSteerSpeedAcc," +
"PidOutLeftFront,PidOutLeftRear,PidOutRightFront,PidOutRightRear," +
"CmdLFL,CmdLFR,CmdLRL,CmdLRR,CmdRFL,CmdRFR,CmdRRL,CmdRRR," +
"PidLFL,PidLFR,PidLRL,PidLRR,PidRFL,PidRFR,PidRRL,PidRRR," +
"ActualLFL,ActualLFR,ActualLRL,ActualLRR,ActualRFL,ActualRFR,ActualRRL,ActualRRR," +
"ActualLeftFront,ActualLeftRear,ActualRightFront,ActualRightRear," +
"TargetThLeftFront,TargetThLeftRear,TargetThRightFront,TargetThRightRear," +
"ActualThLeftFront,ActualThLeftRear,ActualThRightFront,ActualThRightRear," +
"ErrorThLeftFront,ErrorThLeftRear,ErrorThRightFront,ErrorThRightRear");
while (_records.TryDequeue(out _))
{
}
_queuedRecordCount = 0;
_receiveSequence = 0;
_droppedRecordCount = 0;
_lastSnapshotMilliseconds =
double.NegativeInfinity;
_stopwatch = Stopwatch.StartNew();
_isRunning = true;
_writerThread = new Thread(WriterLoop)
{
IsBackground = true,
Name = "WheelSpeedDiagnosticWriter"
};
_writerThread.Start();
}
}
/// <summary>
/// 停止记录并等待队列中的诊断数据写入磁盘。
/// </summary>
public void Stop()
{
Thread writerThread;
lock (_lifecycleLock)
{
if (!_isRunning &&
_writerThread == null)
return;
_isRunning = false;
writerThread = _writerThread;
_recordsAvailable.Set();
}
writerThread?.Join(3000);
lock (_lifecycleLock)
{
_canWriter?.Flush();
_snapshotWriter?.Flush();
_canWriter?.Dispose();
_snapshotWriter?.Dispose();
_canWriter = null;
_snapshotWriter = null;
_writerThread = null;
_stopwatch?.Stop();
}
}
/// <summary>
/// 将一帧驱动器速度反馈加入内存队列，不在CAN回调中执行文件写入。
/// </summary>
public void RecordCanFeedback(
ushort canId,
string motorName,
float rawRpm,
float speedMetersPerSecond)
{
if (!_isRunning)
return;
var elapsedMilliseconds =
_stopwatch.Elapsed.TotalMilliseconds;
var receiveSequence =
Interlocked.Increment(
ref _receiveSequence);
var line = string.Join(
",",
Format(elapsedMilliseconds),
receiveSequence.ToString(
CultureInfo.InvariantCulture),
$"0x{canId:X3}",
motorName,
Format(rawRpm),
Format(speedMetersPerSecond));
Enqueue(new LogRecord(
isCanEvent: true,
line));
}
/// <summary>
/// 按最多50Hz记录一帧控制命令、PID输出、CAN反馈和舵角快照。
/// </summary>
public void RecordSnapshot(
DiverCartDefinition cart)
{
if (!_isRunning || cart == null)
return;
var elapsedMilliseconds =
_stopwatch.Elapsed.TotalMilliseconds;
if (elapsedMilliseconds -
_lastSnapshotMilliseconds <
SnapshotIntervalMilliseconds)
{
return;
}
_lastSnapshotMilliseconds =
elapsedMilliseconds;
var line = string.Join(
",",
Format(elapsedMilliseconds),
cart.CarNum.ToString(
CultureInfo.InvariantCulture),
Format((int)cart.TransmitterControlMode),
Format(cart.ManualMode),
Format(cart.SendThresSpeed),
Format(cart.DiffSteerKp),
Format(cart.DiffSteerKi),
Format(cart.DiffSteerKd),
Format(cart.DiffSteerMaxI),
Format(cart.DiffSteerDeadZone),
Format(cart.DiffSteerThresh),
Format(cart.DiffSteerSpeedAcc),
Format(cart.DiffSteerOutputLeftFront),
Format(cart.DiffSteerOutputLeftRear),
Format(cart.DiffSteerOutputRightFront),
Format(cart.DiffSteerOutputRightRear),
Format(cart.SpeedLeftFrontLeft),
Format(cart.SpeedLeftFrontRight),
Format(cart.SpeedLeftRearLeft),
Format(cart.SpeedLeftRearRight),
Format(cart.SpeedRightFrontLeft),
Format(cart.SpeedRightFrontRight),
Format(cart.SpeedRightRearLeft),
Format(cart.SpeedRightRearRight),
Format(cart.SpeedLFL),
Format(cart.SpeedLFR),
Format(cart.SpeedLRL),
Format(cart.SpeedLRR),
Format(cart.SpeedRFL),
Format(cart.SpeedRFR),
Format(cart.SpeedRRL),
Format(cart.SpeedRRR),
Format(cart.ActualSpeedLeftFrontLeft),
Format(cart.ActualSpeedLeftFrontRight),
Format(cart.ActualSpeedLeftRearLeft),
Format(cart.ActualSpeedLeftRearRight),
Format(cart.ActualSpeedRightFrontLeft),
Format(cart.ActualSpeedRightFrontRight),
Format(cart.ActualSpeedRightRearLeft),
Format(cart.ActualSpeedRightRearRight),
Format(cart.ActualSpeedLeftFront),
Format(cart.ActualSpeedLeftRear),
Format(cart.ActualSpeedRightFront),
Format(cart.ActualSpeedRightRear),
Format(cart.ThLeftFront),
Format(cart.ThLeftRear),
Format(cart.ThRightFront),
Format(cart.ThRightRear),
Format(cart.ActualThLeftFront),
Format(cart.ActualThLeftRear),
Format(cart.ActualThRightFront),
Format(cart.ActualThRightRear),
Format(cart.ThLeftFront - cart.ActualThLeftFront),
Format(cart.ThLeftRear - cart.ActualThLeftRear),
Format(cart.ThRightFront - cart.ActualThRightFront),
Format(cart.ThRightRear - cart.ActualThRightRear));
Enqueue(new LogRecord(
isCanEvent: false,
line));
}
private static StreamWriter CreateWriter(
string path)
{
return new StreamWriter(
path,
append: false,
new UTF8Encoding(
encoderShouldEmitUTF8Identifier: true),
bufferSize: 64 * 1024);
}
private void Enqueue(LogRecord record)
{
var queuedCount =
Interlocked.Increment(
ref _queuedRecordCount);
if (queuedCount >
MaximumQueuedRecords)
{
Interlocked.Decrement(
ref _queuedRecordCount);
Interlocked.Increment(
ref _droppedRecordCount);
return;
}
_records.Enqueue(record);
_recordsAvailable.Set();
}
private void WriterLoop()
{
var lastFlushTime = DateTime.UtcNow;
try
{
while (_isRunning ||
!_records.IsEmpty)
{
var wroteAnyRecord = false;
while (_records.TryDequeue(
out var record))
{
Interlocked.Decrement(
ref _queuedRecordCount);
if (record.IsCanEvent)
_canWriter.WriteLine(record.Line);
else
_snapshotWriter.WriteLine(record.Line);
wroteAnyRecord = true;
}
var shouldFlush =
wroteAnyRecord &&
(DateTime.UtcNow -
lastFlushTime)
.TotalMilliseconds >= 500.0;
if (shouldFlush)
{
_canWriter.Flush();
_snapshotWriter.Flush();
lastFlushTime = DateTime.UtcNow;
}
if (!wroteAnyRecord)
_recordsAvailable.WaitOne(100);
}
var dropped =
Interlocked.Read(
ref _droppedRecordCount);
if (dropped > 0)
{
_canWriter.WriteLine(
$"# DroppedRecords={dropped}");
_snapshotWriter.WriteLine(
$"# DroppedRecords={dropped}");
}
_canWriter.Flush();
_snapshotWriter.Flush();
}
catch (Exception ex)
{
// 后台日志失败不能终止车辆控制线程。
Console.WriteLine(
"轮速诊断后台写入失败：" +
ex.Message);
}
}
private static string Format(
double value)
{
return value.ToString(
"0.######",
CultureInfo.InvariantCulture);
}
public void Dispose()
{
Stop();
_recordsAvailable.Dispose();
}
}
}
MedullaAdapter/build/Medulla/plugins/CommonUsage.dll
BIN
View File
Binary file not shown.
MedullaAdapter/build/Medulla/plugins/MedullaAdapter.deps.json
+39
View File
@@ -0,0 +1,39 @@
{
"runtimeTarget": {
"name": ".NETCoreApp,Version=v8.0",
"signature": ""
},
"compilationOptions": {},
"targets": {
".NETCoreApp,Version=v8.0": {
"MedullaAdapter/1.0.0": {
"dependencies": {
"CommonUsage": "1.0.0.0"
},
"runtime": {
"MedullaAdapter.dll": {}
}
},
"CommonUsage/1.0.0.0": {
"runtime": {
"CommonUsage.dll": {
"assemblyVersion": "1.0.0.0",
"fileVersion": "1.0.0.0"
}
}
}
}
},
"libraries": {
"MedullaAdapter/1.0.0": {
"type": "project",
"serviceable": false,
"sha512": ""
},
"CommonUsage/1.0.0.0": {
"type": "reference",
"serviceable": false,
"sha512": ""
}
}
}
MedullaAdapter/build/Medulla/plugins/MedullaAdapter.dll
BIN
View File
Binary file not shown.
MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb
BIN
View File
Binary file not shown.
MedullaAdapter/ref/CycleGUI.dll
BIN
View File
Binary file not shown.
MedullaAdapter/ref/MDCSToolBox.dll
BIN
View File
Binary file not shown.
MedullaAdapter/ref/RefCartActivator.dll
BIN
View File
Binary file not shown.
MedullaAdapter/ref/RefFundamentalLib.dll
BIN
View File
Binary file not shown.
MedullaAdapter/ref/RefMedullaCore.dll
BIN
View File
Binary file not shown.