// 实际CAN协议、反馈解析、IO、电池、急停 using CartActivator; using FundamentalLib; using MCUSerialBridgeCLR; using System; using System.Collections.Generic; using System.IO; namespace MedullaAdapter { public class MCURoutine : LadderLogic { private int _lastIteration = 0; private int _count = 0; private int _operationTime = 0; private bool _driversDisabled; private bool _canCallbackRegistered; private bool _serialCallbackRegistered; private byte _resetCode = 0x86; private byte _enableCode1 = 0x06; private byte _enableCode2 = 0x07; private byte _enableCode3 = 0x0F; private bool io_bit0 = false;//继电器 private bool io_bit1 = false;//抱闸 private bool io_bit2 = false;//红灯 private bool io_bit3 = false;//绿灯 private bool io_bit4 = false;//黄灯 private const byte BatteryPortIndex = 3; private static readonly byte[] BatteryRequest = BuildBatteryRequest(); private readonly WheelSpeedDiagnosticLogger _wheelSpeedLogger = new WheelSpeedDiagnosticLogger(); // M层单车底盘：将车轮线速度换算为驱动电机转速。 private static float ConvertMps2Rpm(float mps) { return (float)(mps / (Math.PI * 85f) * 10.5f * 60f * 1000f); } // M层单车底盘：将驱动电机转速换算为车轮线速度。 private static float ConvertRpm2Mps(float rpm) { return (float)(rpm / 10.5f / 60f * Math.PI * 85f / 1000f); } // M层单车底盘：将夹臂或执行器转数换算为毫米位移。 private float ConvertR2MM(float r) { return (float)(r / 10.5f * Math.PI * 85); } // M层CAN解析：从驱动器报文中解码有符号转速。 private static float DecodeRpmFromPayload(byte[] payload, int offset = 4) { return BitConverter.ToInt32(payload, offset) * 1875f / 512f / 10000f; } // M层硬件主循环：交换IO、发送轮组指令并更新车辆反馈状态。 public override void Operation(int iteration) { UpdateWheelSpeedDiagnosticState(); if (_lastIteration != iteration) { _lastIteration = iteration; _count = 0; } else { _count++; } if (_count >= 15) { cart.AlarmLevel = 2; Console.WriteLine("mcu lost connection \n"); } if (_count >= 30) { return; } if (cart?.Bridge == null) { return; } //注册CAN回调 EnsureCanCallbacksRegistered(); //注册串口回调 EnsureSerialCallbacksRegistered(); PollBatterySerial(iteration); #region io部分 var ioReadBuffer = new byte[4]; var readInputErr = cart.Bridge.ReadInput(out ioReadBuffer, 20); if (readInputErr == MCUSerialBridgeError.OK) { cart.test = ioReadBuffer[0]; cart.Start = (ioReadBuffer[0] & (1 << 0)) != 0; cart.ResetPressed = (ioReadBuffer[0] & (1 << 1)) != 0; cart.ChassisMode = ((ioReadBuffer[0] & (1 << 2)) == 0) ? 0 : 1; cart.BrakeEnable = (ioReadBuffer[0] & (1 << 3)) != 0; if ((ioReadBuffer[0] & (1 << 4)) == 0) { cart.EmergencyPressed = 1; } } else { Console.WriteLine("IO Read FAILED"); } var ioWriteBuffer = new byte[4]; io_bit0 = cart.ChargePort ? true : false; io_bit1 = cart.EmergencyPressed == 1 ? false : true; if (cart.MultiVehicleLightSync == 0) { io_bit2 = false; io_bit3 = false; io_bit4 = false; } else if (cart.MultiVehicleLightSync == 1) { io_bit2 = false; io_bit3 = true; io_bit4 = false; } else { if (cart.LightMode == 2) { io_bit2 = true;//红 io_bit3 = false;//绿 io_bit4 = false;//黄 } else if (cart.LightMode == 3) { io_bit2 = false; io_bit3 = false; io_bit4 = true; } else if (cart.LightMode == 1) { io_bit2 = false; io_bit3 = true; io_bit4 = false; } else { io_bit2 = false; io_bit3 = false; io_bit4 = false; } } ioWriteBuffer[0] = (byte)((io_bit0 ? 1 << 0 : 0) | //继电器 (io_bit1 ? 1 << 1 : 0) | //抱闸 (io_bit2 ? 1 << 2 : 0) | //红灯 (io_bit3 ? 1 << 3 : 0) | //绿灯 (io_bit4 ? 1 << 4 : 0)); //黄灯 if (cart.Bridge.WriteOutput(ioWriteBuffer, 20) != MCUSerialBridgeError.OK) { Console.WriteLine("IO Write FAILED"); } #endregion #region 驱动部分 cart.ActualSpeedLeftFront = (cart.ActualSpeedLeftFrontLeft + cart.ActualSpeedLeftFrontRight) / 2; cart.ActualSpeedLeftRear = (cart.ActualSpeedLeftRearLeft + cart.ActualSpeedLeftRearRight) / 2; cart.ActualSpeedRightFront = (cart.ActualSpeedRightFrontLeft + cart.ActualSpeedRightFrontRight) / 2; cart.ActualSpeedRightRear = (cart.ActualSpeedRightRearLeft + cart.ActualSpeedRightRearRight) / 2; _wheelSpeedLogger.RecordSnapshot(cart); // M层CAN辅助：封装本周期驱动器CAN发送参数。 MCUSerialBridgeError SendCan(byte port, ushort standardId, byte[] payload, bool RTR = false, uint timeout = 2) { var canSend = new CANMessage { ID = standardId, RTR = RTR, DLC = !RTR ? (byte)8 : (byte)0, Payload = payload ?? new byte[] { } }; return cart.Bridge.WriteCAN(port, canSend, timeout); } var resetPayload = new byte[8] { 0xFD, _resetCode, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; var enable1Payload = new byte[8] { 0xFD, _enableCode1, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; var enable2Payload = new byte[8] { 0xFD, _enableCode2, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; var enable3Payload = new byte[8] { 0xFD, _enableCode3, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; Hedingben.ToastText($"_operationTime:{_operationTime}", "OperationTime"); if (_operationTime == 0) { SendCan(0, 0x00, new byte[] { 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }); _operationTime++; return; } else if (_operationTime == 1) { if (cart.LeftFrontLeftErrorCode != 0) { SendCan(0, 0x201, enable1Payload); SendCan(0, 0x201, resetPayload); } if (cart.LeftFrontRightErrorCode != 0) { SendCan(0, 0x202, enable1Payload); SendCan(0, 0x202, resetPayload); } if (cart.RightFrontLeftErrorCode != 0) { SendCan(0, 0x203, enable1Payload); SendCan(0, 0x203, resetPayload); } if (cart.RightFrontRightErrorCode != 0) { SendCan(0, 0x204, enable1Payload); SendCan(0, 0x204, resetPayload); } if (cart.LeftRearLeftErrorCode != 0) { SendCan(0, 0x205, enable1Payload); SendCan(0, 0x205, resetPayload); } if (cart.LeftRearRightErrorCode != 0) { SendCan(0, 0x206, enable1Payload); SendCan(0, 0x206, resetPayload); } if (cart.RightRearLeftErrorCode != 0) { SendCan(0, 0x207, enable1Payload); SendCan(0, 0x207, resetPayload); } if (cart.RightRearRightErrorCode != 0) { SendCan(0, 0x208, enable1Payload); SendCan(0, 0x208, resetPayload); } if (cart.LeftArmErrorCode != 0) { SendCan(0, 0x209, enable1Payload); SendCan(0, 0x209, resetPayload); } if (cart.RightArmErrorCode != 0) { SendCan(0, 0x20A, enable1Payload); SendCan(0, 0x20A, resetPayload); } _operationTime++; return; } else if (_operationTime == 2) { //SendNodeGuardRequests(SendCan); //if (AreAllNodesOperational()) //{ // _operationTime++; //} _operationTime++; return; } //检查错误是否被消除 若没被消除就得重新返回上一步发复位 若消除了就继续往下 else if (_operationTime == 3) { if (cart.LeftFrontLeftErrorCode != 0 || cart.LeftFrontRightErrorCode != 0 || cart.LeftRearLeftErrorCode != 0 || cart.LeftRearRightErrorCode != 0 || cart.RightFrontLeftErrorCode != 0 || cart.RightFrontRightErrorCode != 0 || cart.RightRearLeftErrorCode != 0 || cart.RightRearRightErrorCode != 0 || cart.LeftArmErrorCode != 0 || cart.RightArmErrorCode != 0) { _operationTime = 1; return; } else { _operationTime++; return; } } // 没有错误 没有节点保护 就发06 07 0F使能 else if (_operationTime == 4) { SendCan(0, 0x201, enable1Payload); SendCan(0, 0x202, enable1Payload); SendCan(0, 0x203, enable1Payload); SendCan(0, 0x204, enable1Payload); SendCan(0, 0x205, enable1Payload); SendCan(0, 0x206, enable1Payload); SendCan(0, 0x207, enable1Payload); SendCan(0, 0x208, enable1Payload); SendCan(0, 0x209, enable1Payload); SendCan(0, 0x20A, enable1Payload); _operationTime++; return; } else if (_operationTime == 5) { SendCan(0, 0x201, enable2Payload); SendCan(0, 0x202, enable2Payload); SendCan(0, 0x203, enable2Payload); SendCan(0, 0x204, enable2Payload); SendCan(0, 0x205, enable2Payload); SendCan(0, 0x206, enable2Payload); SendCan(0, 0x207, enable2Payload); SendCan(0, 0x208, enable2Payload); SendCan(0, 0x209, enable2Payload); SendCan(0, 0x20A, enable2Payload); _operationTime++; return; } else if (_operationTime == 6) { SendCan(0, 0x201, enable3Payload); SendCan(0, 0x202, enable3Payload); SendCan(0, 0x203, enable3Payload); SendCan(0, 0x204, enable3Payload); SendCan(0, 0x205, enable3Payload); SendCan(0, 0x206, enable3Payload); SendCan(0, 0x207, enable3Payload); SendCan(0, 0x208, enable3Payload); SendCan(0, 0x209, enable3Payload); SendCan(0, 0x20A, enable3Payload); if (cart.LeftFrontLeftErrorCode != 0 || cart.LeftFrontRightErrorCode != 0 || cart.LeftRearLeftErrorCode != 0 || cart.LeftRearRightErrorCode != 0 || cart.RightFrontLeftErrorCode != 0 || cart.RightFrontRightErrorCode != 0 || cart.RightRearLeftErrorCode != 0 || cart.RightRearRightErrorCode != 0 || cart.LeftArmErrorCode != 0 || cart.RightArmErrorCode != 0) { _operationTime = 1; return; } else { _operationTime++; return; } } if (cart.ResetPressed || cart.ResetFromM || cart.ResetFromC) { cart.AlarmLevel = -1; cart.EmergencyPressed = 0; _count = 0; _operationTime = 0; _driversDisabled = false; cart.WheelAbleState = true; cart.ResetFromM = false; } if (cart.DisableFromM || cart.DisableFromC) { _driversDisabled = true; cart.WheelAbleState = false; cart.DisableFromM = false; } var lfl = Math.Sign(cart.SpeedLFL) * Math.Min(Math.Max(0, cart.SendThresSpeed), Math.Abs(cart.SpeedLFL)); var lfr = Math.Sign(cart.SpeedLFR) * Math.Min(Math.Max(0, cart.SendThresSpeed), Math.Abs(cart.SpeedLFR)); var rfl = Math.Sign(cart.SpeedRFL) * Math.Min(Math.Max(0, cart.SendThresSpeed), Math.Abs(cart.SpeedRFL)); var rfr = Math.Sign(cart.SpeedRFR) * Math.Min(Math.Max(0, cart.SendThresSpeed), Math.Abs(cart.SpeedRFR)); var lrl = Math.Sign(cart.SpeedLRL) * Math.Min(Math.Max(0, cart.SendThresSpeed), Math.Abs(cart.SpeedLRL)); var lrr = Math.Sign(cart.SpeedLRR) * Math.Min(Math.Max(0, cart.SendThresSpeed), Math.Abs(cart.SpeedLRR)); var rrl = Math.Sign(cart.SpeedRRL) * Math.Min(Math.Max(0, cart.SendThresSpeed), Math.Abs(cart.SpeedRRL)); var rrr = Math.Sign(cart.SpeedRRR) * Math.Min(Math.Max(0, cart.SendThresSpeed), Math.Abs(cart.SpeedRRR)); var v1 = ConvertMps2Rpm(lfl); var v2 = ConvertMps2Rpm(-lfr); var v3 = ConvertMps2Rpm(rfl); var v4 = ConvertMps2Rpm(-rfr); var v5 = ConvertMps2Rpm(lrl); var v6 = ConvertMps2Rpm(-lrr); var v7 = ConvertMps2Rpm(rrl); var v8 = ConvertMps2Rpm(-rrr); var v9 = ConvertMps2Rpm(cart.SpeedLeftArm); var v10 = ConvertMps2Rpm(cart.SpeedRightArm); if (cart.AlarmLevel == 2 || cart.WaitStart || cart.PreparaStart) { v1 = v2 = v3 = v4 = v5 = v6 = v7 = v8 = v9 = v10 = 0; } if ((cart.ActualPosLeftArm <= cart.LeftArmLowerPos && cart.SpeedLeftArm < 0) || (cart.ActualPosLeftArm >= cart.LeftArmUpperPos && cart.SpeedLeftArm > 0)) { v9 = 0; } if ((cart.ActualPosRightArm <= cart.RightArmLowerPos && cart.SpeedRightArm < 0) || (cart.ActualPosRightArm >= cart.RightArmUpperPos && cart.SpeedRightArm > 0)) { v10 = 0; } var sendLFL = BitConverter.GetBytes((int)Math.Round(v1 * 512f * 10000f / 1875f)); var sendLFR = BitConverter.GetBytes((int)Math.Round(v2 * 512f * 10000f / 1875f)); var sendRFL = BitConverter.GetBytes((int)Math.Round(v3 * 512f * 10000f / 1875f)); var sendRFR = BitConverter.GetBytes((int)Math.Round(v4 * 512f * 10000f / 1875f)); var sendLRL = BitConverter.GetBytes((int)Math.Round(v5 * 512f * 10000f / 1875f)); var sendLRR = BitConverter.GetBytes((int)Math.Round(v6 * 512f * 10000f / 1875f)); var sendRRL = BitConverter.GetBytes((int)Math.Round(v7 * 512f * 10000f / 1875f)); var sendRRR = BitConverter.GetBytes((int)Math.Round(v8 * 512f * 10000f / 1875f)); var sendLArm = BitConverter.GetBytes((int)Math.Round(v9 * 512f * 10000f / 1875f)); var sendRArm = BitConverter.GetBytes((int)Math.Round(v10 * 512f * 10000f / 1875f)); if (iteration % 2 == 0) { //SendNodeGuardRequests(SendCan); } if (_driversDisabled) { SendCan(0, 0x201, enable1Payload); SendCan(0, 0x202, enable1Payload); SendCan(0, 0x203, enable1Payload); SendCan(0, 0x204, enable1Payload); SendCan(0, 0x205, enable1Payload); SendCan(0, 0x206, enable1Payload); SendCan(0, 0x207, enable1Payload); SendCan(0, 0x208, enable1Payload); } else { SendCan(0, 0x201, new byte[] { 0xFD, _enableCode3, 0x00, sendLFL[0], sendLFL[1], sendLFL[2], sendLFL[3], 0x00 }); SendCan(0, 0x202, new byte[] { 0xFD, _enableCode3, 0x00, sendLFR[0], sendLFR[1], sendLFR[2], sendLFR[3], 0x00 }); SendCan(0, 0x203, new byte[] { 0xFD, _enableCode3, 0x00, sendRFL[0], sendRFL[1], sendRFL[2], sendRFL[3], 0x00 }); SendCan(0, 0x204, new byte[] { 0xFD, _enableCode3, 0x00, sendRFR[0], sendRFR[1], sendRFR[2], sendRFR[3], 0x00 }); SendCan(0, 0x205, new byte[] { 0xFD, _enableCode3, 0x00, sendLRL[0], sendLRL[1], sendLRL[2], sendLRL[3], 0x00 }); SendCan(0, 0x206, new byte[] { 0xFD, _enableCode3, 0x00, sendLRR[0], sendLRR[1], sendLRR[2], sendLRR[3], 0x00 }); SendCan(0, 0x207, new byte[] { 0xFD, _enableCode3, 0x00, sendRRL[0], sendRRL[1], sendRRL[2], sendRRL[3], 0x00 }); SendCan(0, 0x208, new byte[] { 0xFD, _enableCode3, 0x00, sendRRR[0], sendRRR[1], sendRRR[2], sendRRR[3], 0x00 }); } SendCan(0, 0x209, new byte[] { 0xFD, _enableCode3, 0x00, sendLArm[0], sendLArm[1], sendLArm[2], sendLArm[3], 0x00 }); SendCan(0, 0x20A, new byte[] { 0xFD, _enableCode3, 0x00, sendRArm[0], sendRArm[1], sendRArm[2], sendRArm[3], 0x00 }); if (cart.LeftFrontLeftErrorCode != 0 || cart.LeftFrontRightErrorCode != 0 || cart.LeftRearLeftErrorCode != 0 || cart.LeftRearRightErrorCode != 0 || cart.RightFrontLeftErrorCode != 0 || cart.RightFrontRightErrorCode != 0 || cart.RightRearLeftErrorCode != 0 || cart.RightRearRightErrorCode != 0) { //_errorCount++; } #endregion } // M层诊断：根据界面开关创建或关闭本次轮速CSV记录。 private void UpdateWheelSpeedDiagnosticState() { if (cart == null) return; if (cart.WheelSpeedDiagnosticEnabled) { if (_wheelSpeedLogger.IsRunning) return; try { var configuredDirectory = string.IsNullOrWhiteSpace( cart.WheelSpeedDiagnosticDirectory) ? @"logs\wheel-speed" : cart.WheelSpeedDiagnosticDirectory; var logDirectory = Path.IsPathRooted(configuredDirectory) ? configuredDirectory : Path.Combine( AppContext.BaseDirectory, configuredDirectory); _wheelSpeedLogger.Start( logDirectory, cart.CarNum); cart.WheelSpeedDiagnosticStatus = "记录中：" + _wheelSpeedLogger.SnapshotLogPath; } catch (Exception ex) { cart.WheelSpeedDiagnosticEnabled = false; cart.WheelSpeedDiagnosticStatus = "启动失败：" + ex.Message; Console.WriteLine( "轮速诊断启动失败：" + ex.Message); } return; } if (!_wheelSpeedLogger.IsRunning) return; try { var snapshotPath = _wheelSpeedLogger.SnapshotLogPath; _wheelSpeedLogger.Stop(); cart.WheelSpeedDiagnosticStatus = "已保存：" + snapshotPath; } catch (Exception ex) { cart.WheelSpeedDiagnosticStatus = "停止失败：" + ex.Message; Console.WriteLine( "轮速诊断停止失败：" + ex.Message); } } // M层CAN安全：判断单个驱动节点是否处于可运行状态。 private static bool IsNodeOperational(byte remoteCode) { return remoteCode == 5 || remoteCode == 133; } // M层CAN安全：检查全部车轮和夹臂节点是否已上线。 private bool AreAllNodesOperational() { return IsNodeOperational(cart.LFLRemoteCode) && IsNodeOperational(cart.LFRRemoteCode) && IsNodeOperational(cart.RFLRemoteCode) && IsNodeOperational(cart.RFRRemoteCode) && IsNodeOperational(cart.LRLRemoteCode) && IsNodeOperational(cart.LRRRemoteCode) && IsNodeOperational(cart.RRLRemoteCode) && IsNodeOperational(cart.RRRRemoteCode) && IsNodeOperational(cart.LArmRemoteCode) && IsNodeOperational(cart.RArmRemoteCode); } // M层CAN维护：轮询所有驱动节点的在线状态。 private static void SendNodeGuardRequests(Func sendCan) { sendCan(0, 0x709, Array.Empty(), true, 2); sendCan(0, 0x70A, Array.Empty(), true, 2); sendCan(0, 0x701, Array.Empty(), true, 2); sendCan(0, 0x702, Array.Empty(), true, 2); sendCan(0, 0x703, Array.Empty(), true, 2); sendCan(0, 0x704, Array.Empty(), true, 2); sendCan(0, 0x705, Array.Empty(), true, 2); sendCan(0, 0x706, Array.Empty(), true, 2); sendCan(0, 0x707, Array.Empty(), true, 2); sendCan(0, 0x708, Array.Empty(), true, 2); } // M层CAN通信：按需注册驱动器反馈报文回调。 private void EnsureCanCallbacksRegistered() { if (_canCallbackRegistered) { return; } if (cart?.Bridge == null) { return; } var dispatch = BuildCanDispatchTable(); var err0 = cart.Bridge.RegisterCANPortCallback(0, msg => { if (msg == null) return; if (dispatch.TryGetValue(msg.ID, out var handler)) { handler(msg); } }); // _canCallbackRegistered = true; _canCallbackRegistered = err0 == MCUSerialBridgeError.OK; } // M层串口通信：按需注册电池等串口设备回调。 private void EnsureSerialCallbacksRegistered() { if (_serialCallbackRegistered) { return; } if (cart?.Bridge == null) { return; } var err1 = cart.Bridge.RegisterSerialPortCallback(1, msg => { if (msg == null) return; Hedingben.ToastText($"Callback Serial 1 Callback Received: {BitConverter.ToString(msg)}", "Serial 1"); }); Hedingben.ToastText($"Register Serial 1: {err1}", "Register Serial"); _serialCallbackRegistered = err1 == MCUSerialBridgeError.OK; } // M层单车电源：周期发送Modbus电池状态查询。 private void PollBatterySerial(int iteration) { if (cart?.Bridge == null) return; if (iteration % 20 != 0) return; var writeErr = cart.Bridge.WriteSerial(BatteryPortIndex, BatteryRequest, 60); if (writeErr != MCUSerialBridgeError.OK) { Hedingben.ToastText($"Battery write err: {writeErr}", "Serial 2"); return; } var readErr = cart.Bridge.ReadSerial(BatteryPortIndex, out var msg, 40); if (readErr == MCUSerialBridgeError.NoData) return; if (readErr != MCUSerialBridgeError.OK) { Hedingben.ToastText($"Battery read err: {readErr}", "Serial 2"); return; } if (msg == null || msg.Length == 0) return; Hedingben.ToastText($"Serial 2 RX: {BitConverter.ToString(msg)}", "Serial 2"); TryUpdateBatteryData(msg); } // M层单车电源：校验并解析电池Modbus响应。 private void TryUpdateBatteryData(byte[] msg) { // Modbus RTU response: [id,03,08,data(8),crc(2)] if (msg.Length < 13 || msg[0] != 0x01 || msg[1] != 0x03 || msg[2] < 8) return; ushort calc = ComputeModbusCrc(msg, msg.Length - 2); ushort recv = (ushort)(msg[msg.Length - 2] | (msg[msg.Length - 1] << 8)); if (calc != recv) return; cart.Voltage = ReadInt16BE(msg, 3) * 0.1f; cart.Soc = ReadUInt16BE(msg, 5); cart.SOH = ReadUInt16BE(msg, 7); cart.ElectricCurrent = -ReadInt16BE(msg, 9) * 0.1f; } // M层单车电源：构造读取电池寄存器的Modbus请求。 private static byte[] BuildBatteryRequest() { // 01 03 00 03 00 04 CRC (读取寄存器 03~06) byte[] req = new byte[] { 0x01, 0x03, 0x00, 0x03, 0x00, 0x04, 0x00, 0x00 }; ushort crc = ComputeModbusCrc(req, 6); req[6] = (byte)(crc & 0xFF); req[7] = (byte)((crc >> 8) & 0xFF); return req; } // M层协议辅助：计算Modbus RTU的CRC16校验值。 private static ushort ComputeModbusCrc(byte[] data, int length) { ushort crc = 0xFFFF; for (int i = 0; i < length; i++) { crc ^= data[i]; for (int j = 0; j < 8; j++) { bool lsb = (crc & 0x0001) != 0; crc >>= 1; if (lsb) crc ^= 0xA001; } } return crc; } // M层协议辅助：按大端序读取有符号16位数值。 private static short ReadInt16BE(byte[] data, int offset) { return (short)((data[offset] << 8) | data[offset + 1]); } // M层协议辅助：按大端序读取无符号16位数值。 private static ushort ReadUInt16BE(byte[] data, int offset) { return (ushort)((data[offset] << 8) | data[offset + 1]); } // M层CAN解析：建立驱动器报文标识符到处理函数的分发表。 private Dictionary> BuildCanDispatchTable() { return new Dictionary> { // 速度/位置反馈 0x281~0x28A [0x281] = (msg) => { //Console.WriteLine("Received 0x281 CAN Message"); var payload = msg.Payload; if (payload == null || payload.Length < 8) return; var rpm = DecodeRpmFromPayload(payload); var speed = ConvertRpm2Mps(rpm); cart.ActualSpeedLeftFrontLeft = speed; _wheelSpeedLogger.RecordCanFeedback( 0x281, "LFL", rpm, speed); cart.LFLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f); }, [0x282] = (msg) => { //Console.WriteLine("Received 0x282 CAN Message"); var payload = msg.Payload; if (payload == null || payload.Length < 8) return; var rpm = DecodeRpmFromPayload(payload); var speed = -ConvertRpm2Mps(rpm); cart.ActualSpeedLeftFrontRight = speed; _wheelSpeedLogger.RecordCanFeedback( 0x282, "LFR", rpm, speed); cart.LFRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f); }, [0x283] = (msg) => { //Console.WriteLine("Received 0x283 CAN Message"); var payload = msg.Payload; if (payload == null || payload.Length < 8) return; var rpm = DecodeRpmFromPayload(payload); var speed = ConvertRpm2Mps(rpm); cart.ActualSpeedRightFrontLeft = speed; _wheelSpeedLogger.RecordCanFeedback( 0x283, "RFL", rpm, speed); cart.RFLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f); }, [0x284] = (msg) => { //Console.WriteLine("Received 0x284 CAN Message"); var payload = msg.Payload; if (payload == null || payload.Length < 8) return; var rpm = DecodeRpmFromPayload(payload); var speed = -ConvertRpm2Mps(rpm); cart.ActualSpeedRightFrontRight = speed; _wheelSpeedLogger.RecordCanFeedback( 0x284, "RFR", rpm, speed); cart.RFRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f); }, [0x285] = (msg) => { //Console.WriteLine("Received 0x285 CAN Message"); var payload = msg.Payload; if (payload == null || payload.Length < 8) return; var rpm = DecodeRpmFromPayload(payload); var speed = ConvertRpm2Mps(rpm); cart.ActualSpeedLeftRearLeft = speed; _wheelSpeedLogger.RecordCanFeedback( 0x285, "LRL", rpm, speed); cart.LRLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f); }, [0x286] = (msg) => { //Console.WriteLine("Received 0x286 CAN Message"); var payload = msg.Payload; if (payload == null || payload.Length < 8) return; var rpm = DecodeRpmFromPayload(payload); var speed = -ConvertRpm2Mps(rpm); cart.ActualSpeedLeftRearRight = speed; _wheelSpeedLogger.RecordCanFeedback( 0x286, "LRR", rpm, speed); cart.LRRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f); }, [0x287] = (msg) => { //Console.WriteLine("Received 0x287 CAN Message"); var payload = msg.Payload; if (payload == null || payload.Length < 8) return; var rpm = DecodeRpmFromPayload(payload); var speed = ConvertRpm2Mps(rpm); cart.ActualSpeedRightRearLeft = speed; _wheelSpeedLogger.RecordCanFeedback( 0x287, "RRL", rpm, speed); cart.RRLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f); }, [0x288] = (msg) => { //Console.WriteLine("Received 0x288 CAN Message"); var payload = msg.Payload; if (payload == null || payload.Length < 8) return; var rpm = DecodeRpmFromPayload(payload); var speed = -ConvertRpm2Mps(rpm); cart.ActualSpeedRightRearRight = speed; _wheelSpeedLogger.RecordCanFeedback( 0x288, "RRR", rpm, speed); cart.RRRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f); }, [0x289] = (msg) => { //Console.WriteLine("Received 0x289 CAN Message"); var payload = msg.Payload; if (payload == null || payload.Length < 8) return; var rpm = DecodeRpmFromPayload(payload); cart.ActualSpeedLeftArm = ConvertRpm2Mps(rpm); cart.ActualPosLeftArm = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f); }, [0x28A] = (msg) => { //Console.WriteLine("Received 0x28A CAN Message"); var payload = msg.Payload; if (payload == null || payload.Length < 8) return; var rpm = DecodeRpmFromPayload(payload); cart.ActualSpeedRightArm = ConvertRpm2Mps(rpm); cart.ActualPosRightArm = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f); }, // 状态/错误码 0x181~0x18A [0x181] = (msg) => { //Console.WriteLine("Received 0x181 CAN Message"); var payload = msg.Payload; if (payload == null || payload.Length < 4) return; cart.LeftFrontLeftStateCode = BitConverter.ToInt16(payload, 0); cart.LeftFrontLeftErrorCode = BitConverter.ToInt16(payload, 2); cart.LeftFrontLeftElectric = BitConverter.ToInt16(payload, 5) * 0.001f; }, [0x182] = (msg) => { //Console.WriteLine("Received 0x182 CAN Message"); var payload = msg.Payload; if (payload == null || payload.Length < 4) return; cart.LeftFrontRightStateCode = BitConverter.ToInt16(payload, 0); cart.LeftFrontRightErrorCode = BitConverter.ToInt16(payload, 2); cart.LeftFrontRightElectric = BitConverter.ToInt16(payload, 5) * 0.001f; }, [0x183] = (msg) => { //Console.WriteLine("Received 0x183 CAN Message"); var payload = msg.Payload; if (payload == null || payload.Length < 4) return; cart.RightFrontLeftStateCode = BitConverter.ToInt16(payload, 0); cart.RightFrontLeftErrorCode = BitConverter.ToInt16(payload, 2); cart.RightFrontLeftElectric = BitConverter.ToInt16(payload, 5) * 0.001f; }, [0x184] = (msg) => { //Console.WriteLine("Received 0x184 CAN Message"); var payload = msg.Payload; if (payload == null || payload.Length < 4) return; cart.RightFrontRightStateCode = BitConverter.ToInt16(payload, 0); cart.RightFrontRightErrorCode = BitConverter.ToInt16(payload, 2); cart.RightFrontRightElectric = BitConverter.ToInt16(payload, 5) * 0.001f; }, [0x185] = (msg) => { //Console.WriteLine("Received 0x185 CAN Message"); var payload = msg.Payload; if (payload == null || payload.Length < 4) return; cart.LeftRearLeftStateCode = BitConverter.ToInt16(payload, 0); cart.LeftRearLeftErrorCode = BitConverter.ToInt16(payload, 2); cart.LeftRearLeftElectric = BitConverter.ToInt16(payload, 5) * 0.001f; }, [0x186] = (msg) => { //Console.WriteLine("Received 0x186 CAN Message"); var payload = msg.Payload; if (payload == null || payload.Length < 4) return; cart.LeftRearRightStateCode = BitConverter.ToInt16(payload, 0); cart.LeftRearRightErrorCode = BitConverter.ToInt16(payload, 2); cart.LeftRearRightElectric = BitConverter.ToInt16(payload, 5) * 0.001f; }, [0x187] = (msg) => { //Console.WriteLine("Received 0x187 CAN Message"); var payload = msg.Payload; if (payload == null || payload.Length < 4) return; cart.RightRearLeftStateCode = BitConverter.ToInt16(payload, 0); cart.RightRearLeftErrorCode = BitConverter.ToInt16(payload, 2); cart.RightRearLeftElectric = BitConverter.ToInt16(payload, 5) * 0.001f; }, [0x188] = (msg) => { //Console.WriteLine("Received 0x188 CAN Message"); var payload = msg.Payload; if (payload == null || payload.Length < 4) return; cart.RightRearRightStateCode = BitConverter.ToInt16(payload, 0); cart.RightRearRightErrorCode = BitConverter.ToInt16(payload, 2); cart.RightRearRightElectric = BitConverter.ToInt16(payload, 5) * 0.001f; }, [0x189] = (msg) => { //Console.WriteLine("Received 0x189 CAN Message"); var payload = msg.Payload; if (payload == null || payload.Length < 4) return; cart.LeftArmStateCode = BitConverter.ToInt16(payload, 0); cart.LeftArmErrorCode = BitConverter.ToInt16(payload, 2); cart.LeftArmElectric = BitConverter.ToInt16(payload, 5) * 0.001f; }, [0x18A] = (msg) => { //Console.WriteLine("Received 0x18A CAN Message"); var payload = msg.Payload; if (payload == null || payload.Length < 4) return; cart.RightArmStateCode = BitConverter.ToInt16(payload, 0); cart.RightArmErrorCode = BitConverter.ToInt16(payload, 2); cart.RightArmElectric = BitConverter.ToInt16(payload, 5) * 0.001f; }, // 舵角 0x18B~0x18E [0x18B] = (msg) => { //Console.WriteLine("Received 0x18B CAN Message"); var payload = msg.Payload; if (payload == null || payload.Length < 4) return; cart.ActualThLeftFront = BitConverter.ToInt32(payload, 0); cart.ActualThLeftFront = cart.ActualThLeftFront >= 16384 ? (cart.ActualThLeftFront - 98303) / 4096f / 5f * 360 - cart.ThBiasLeftFront : cart.ActualThLeftFront / 4096f / 5f * 360 - cart.ThBiasLeftFront; }, [0x18C] = (msg) => { var payload = msg.Payload; if (payload == null || payload.Length < 4) return; var raw = BitConverter.ToInt32(payload, 0); cart.ActualThRightFront = raw >= 16384 ? (raw - 98303) / 4096f / 5f * 360 - cart.ThBiasRightFront : raw / 4096f / 5f * 360 - cart.ThBiasRightFront; //DLog.Log($"RF raw=0x{raw:X8}({raw}) angle={cart.ActualThRightFront:F2}", "0x18C"); }, [0x18D] = (msg) => { //Console.WriteLine($"Received 0x18D CAN Message {DateTime.Now:yyyy-MM-dd HH:mm:ss.ffffff}"); var payload = msg.Payload; if (payload == null || payload.Length < 4) return; cart.ActualThLeftRear = BitConverter.ToInt32(payload, 0); cart.ActualThLeftRear = cart.ActualThLeftRear >= 16384 ? (cart.ActualThLeftRear - 98303) / 4096f / 5f * 360 - cart.ThBiasLeftRear : cart.ActualThLeftRear / 4096f / 5f * 360 - cart.ThBiasLeftRear; }, [0x18E] = (msg) => { //Console.WriteLine("Received 0x18E CAN Message"); var payload = msg.Payload; if (payload == null || payload.Length < 4) return; cart.ActualThRightRear = BitConverter.ToInt32(payload, 0); cart.ActualThRightRear = cart.ActualThRightRear >= 16384 ? (cart.ActualThRightRear - 98303) / 4096f / 5f * 360 - cart.ThBiasRightRear : cart.ActualThRightRear / 4096f / 5f * 360 - cart.ThBiasRightRear; }, // 远程帧 [0x701] = (msg) => { //Console.WriteLine("Received 0x701 CAN Message"); var payload = msg.Payload; if (payload == null || payload.Length < 1) return; cart.LFLRemoteCode = payload[0]; }, [0x702] = (msg) => { //Console.WriteLine("Received 0x702 CAN Message"); var payload = msg.Payload; if (payload == null || payload.Length < 1) return; cart.LFRRemoteCode = payload[0]; }, [0x703] = (msg) => { //Console.WriteLine("Received 0x703 CAN Message"); var payload = msg.Payload; if (payload == null || payload.Length < 1) return; cart.RFLRemoteCode = payload[0]; }, [0x704] = (msg) => { //Console.WriteLine("Received 0x704 CAN Message"); var payload = msg.Payload; if (payload == null || payload.Length < 1) return; cart.RFRRemoteCode = payload[0]; }, [0x705] = (msg) => { //Console.WriteLine("Received 0x705 CAN Message"); var payload = msg.Payload; if (payload == null || payload.Length < 1) return; cart.LRLRemoteCode = payload[0]; }, [0x706] = (msg) => { //Console.WriteLine("Received 0x706 CAN Message"); var payload = msg.Payload; if (payload == null || payload.Length < 1) return; cart.LRRRemoteCode = payload[0]; }, [0x707] = (msg) => { //Console.WriteLine("Received 0x707 CAN Message"); var payload = msg.Payload; if (payload == null || payload.Length < 1) return; cart.RRLRemoteCode = payload[0]; }, [0x708] = (msg) => { //Console.WriteLine("Received 0x708 CAN Message"); var payload = msg.Payload; if (payload == null || payload.Length < 1) return; cart.RRRRemoteCode = payload[0]; }, [0x709] = (msg) => { //Console.WriteLine("Received 0x709 CAN Message"); var payload = msg.Payload; if (payload == null || payload.Length < 1) return; cart.LArmRemoteCode = payload[0]; }, [0x70A] = (msg) => { //Console.WriteLine("Received 0x70A CAN Message"); var payload = msg.Payload; if (payload == null || payload.Length < 1) return; cart.RArmRemoteCode = payload[0]; }, }; } } }