using CanTest; using System; using System.Collections.Generic; using System.Linq; using System.Text; using System.Threading; using System.Threading.Tasks; namespace CCDCount.DLL.CanBus { /// /// CANopen从站设备 - 基于Nameless.eds文件实现 /// 支持4个RPDO和4个TPDO,符合CiA DS301标准 /// public class CanOpenSlaveDevice : IDisposable { #region 私有字段 private CanOpenManager m_canManager; private byte m_nodeId = 1; // 默认节点ID private NmtState m_currentState = NmtState.Initializing; // PDO配置 - 根据EDS文件定义 private PdoConfig[] m_rpdoConfigs = new PdoConfig[4]; // 接收PDO private PdoConfig[] m_tpdoConfigs = new PdoConfig[4]; // 发送PDO // PDO映射对象 private PdoMapping[,] m_rpdoMappings = new PdoMapping[4, 4]; // RPDO映射表 private PdoMapping[,] m_tpdoMappings = new PdoMapping[4, 4]; // TPDO映射表 // 应用数据缓冲区 (用于PDO数据交换) private Dictionary m_objectDictionary = new Dictionary(); // 心跳参数 private ushort m_producerHeartbeatTime = 0; // 心跳时间(ms),0表示禁用 private Timer m_heartbeatTimer; // SYNC参数 private bool m_syncEnabled = false; private byte m_syncCounter = 0; // 线程控制 private CancellationTokenSource m_cancellationTokenSource; private Task m_receiveTask; private bool m_isRunning = false; #endregion #region 事件定义 /// /// RPDO接收事件 /// public event Action OnRpdoReceived; /// /// NMT状态改变事件 /// public event Action OnNmtStateChanged; /// /// SDO读取请求事件 /// public event Action OnSdoReadRequest; /// /// SDO写入请求事件 /// public event Action OnSdoWriteRequest; #endregion #region 构造函数 /// /// 构造函数 /// /// 节点ID(1-127) /// 设备类型 /// 设备索引 /// CAN通道索引 public CanOpenSlaveDevice(byte nodeId = 1, UInt32 deviceType = 4, UInt32 deviceIndex = 0, UInt32 canIndex = 0) { if (nodeId < 1 || nodeId > 127) throw new ArgumentException("节点ID必须在1-127之间"); m_nodeId = nodeId; m_canManager = new CanOpenManager(deviceType, deviceIndex, canIndex); InitializePdoConfigs(); InitializeObjectDictionary(); } #endregion #region 初始化方法 /// /// 初始化PDO配置 - 根据EDS文件 /// private void InitializePdoConfigs() { // 初始化RPDO配置 (1400-1403) for (int i = 0; i < 4; i++) { m_rpdoConfigs[i] = new PdoConfig { CobId = GetRpdoCobId((byte)(i + 1), m_nodeId), TransmissionType = 0xFF, // 事件驱动 Enabled = true }; // 初始化映射表为空 for (int j = 0; j < 4; j++) { m_rpdoMappings[i, j] = new PdoMapping { Index = 0, SubIndex = 0, BitLength = 0 }; } } // 初始化TPDO配置 (1800-1803) for (int i = 0; i < 4; i++) { m_tpdoConfigs[i] = new PdoConfig { CobId = GetTpdoCobId((byte)(i + 1), m_nodeId), TransmissionType = 0xFF, // 事件驱动 Enabled = true }; // 初始化映射表为空 for (int j = 0; j < 4; j++) { m_tpdoMappings[i, j] = new PdoMapping { Index = 0, SubIndex = 0, BitLength = 0 }; } } } /// /// 初始化对象字典 - 根据EDS文件定义 /// private void InitializeObjectDictionary() { // 1000h - Device type m_objectDictionary[(ushort)0x1000] = (uint)0x00000000; // 默认值 // 1001h - Error register m_objectDictionary[(ushort)0x1001] = (byte)0x00; // 1018h - Identity Object m_objectDictionary[(ushort)0x1018] = new IdentityObject { VendorId = 0x0000000D, // HanLin ProductCode = 0x00000001, // CCDCountCanOpenSlave RevisionNumber = 0x00010001 }; // 1800-1803h - TPDO communication parameters for (int i = 0; i < 4; i++) { m_objectDictionary[(ushort)(0x1800 + i)] = new TpdoCommParam { CobId = GetTpdoCobId((byte)(i + 1), m_nodeId), TransmissionType = 0xFF }; } // 1A00-1A03h - TPDO mapping parameters for (int i = 0; i < 4; i++) { m_objectDictionary[(ushort)(0x1A00 + i)] = new PdoMappingParam { NumberOfEntries = 4, Mappings = new uint[4] }; } // 1400-1403h - RPDO communication parameters for (int i = 0; i < 4; i++) { m_objectDictionary[(ushort)(0x1400 + i)] = new RpdoCommParam { CobId = GetRpdoCobId((byte)(i + 1), m_nodeId), TransmissionType = 0xFF }; } // 1600-1603h - RPDO mapping parameters for (int i = 0; i < 4; i++) { m_objectDictionary[(ushort)(0x1600 + i)] = new PdoMappingParam { NumberOfEntries = 4, Mappings = new uint[4] }; } } #endregion #region 启动和停止 /// /// 启动从站设备 /// /// 波特率 /// 是否成功 public bool Start(CanBaudRate baudRate = CanBaudRate.BaudRate_1M) { if (m_isRunning) { Console.WriteLine("从站设备已在运行"); return true; } // 初始化CAN if (!m_canManager.Initialize(baudRate)) { Console.WriteLine("CAN初始化失败"); return false; } m_isRunning = true; m_currentState = NmtState.PreOperational; // 启动接收线程 m_cancellationTokenSource = new CancellationTokenSource(); m_receiveTask = Task.Run(() => ReceiveLoop(m_cancellationTokenSource.Token)); Console.WriteLine($"CANopen从站启动成功 - 节点ID: {m_nodeId}"); return true; } /// /// 停止从站设备 /// public void Stop() { if (!m_isRunning) return; m_isRunning = false; // 停止接收线程 m_cancellationTokenSource?.Cancel(); m_receiveTask?.Wait(1000); m_receiveTask?.Dispose(); // 停止心跳定时器 m_heartbeatTimer?.Dispose(); // 关闭CAN m_canManager.Close(); m_currentState = NmtState.Stopped; Console.WriteLine("CANopen从站已停止"); } #endregion #region 接收处理循环 /// /// CAN帧接收循环 /// private void ReceiveLoop(CancellationToken cancellationToken) { while (!cancellationToken.IsCancellationRequested) { try { var frames = m_canManager.ReceiveCanFrames(50); foreach (var frame in frames) { ProcessCanFrame(frame); } } catch (Exception ex) { Console.WriteLine($"接收错误: {ex.Message}"); } Thread.Sleep(1); // 避免CPU占用过高 } } /// /// 处理接收到的CAN帧 /// private void ProcessCanFrame(CanOpenFrame frame) { // 检查是否是NMT命令 (COB-ID = 0x000) if (frame.CobId == (uint)0x000 && frame.DataLength >= 2) { ProcessNmtCommand(frame.Data); return; } // 检查是否是SYNC (COB-ID = 0x080) if (frame.CobId == (uint)0x080) { ProcessSync(frame.Data); return; } // 检查是否是SDO请求 (COB-ID = 0x600 + nodeId) uint sdoRequestCobId = (uint)0x600 + m_nodeId; if (frame.CobId == sdoRequestCobId) { ProcessSdoRequest(frame.Data); return; } // 检查是否是RPDO for (int i = 0; i < 4; i++) { if (frame.CobId == m_rpdoConfigs[i].CobId && m_rpdoConfigs[i].Enabled) { ProcessRpdo((byte)(i + 1), frame.Data, frame.DataLength); return; } } } #endregion #region NMT处理 /// /// 处理NMT命令 /// private void ProcessNmtCommand(byte[] data) { byte commandSpecifier = data[0]; byte nodeId = data[1]; // 检查是否是广播命令或针对本节点的命令 if (nodeId != 0 && nodeId != m_nodeId) return; NmtState oldState = m_currentState; switch (commandSpecifier) { case 0x01: // 启动节点 m_currentState = NmtState.Operational; Console.WriteLine($"NMT: 节点{m_nodeId}进入运行状态"); break; case 0x02: // 停止节点 m_currentState = NmtState.Stopped; Console.WriteLine($"NMT: 节点{m_nodeId}进入停止状态"); break; case 0x80: // 进入预操作状态 m_currentState = NmtState.PreOperational; Console.WriteLine($"NMT: 节点{m_nodeId}进入预操作状态"); break; case 0x81: // 重置节点 m_currentState = NmtState.Initializing; Console.WriteLine($"NMT: 节点{m_nodeId}重置"); // TODO: 执行节点重置逻辑 m_currentState = NmtState.PreOperational; break; case 0x82: // 重置通信 m_currentState = NmtState.PreOperational; Console.WriteLine($"NMT: 节点{m_nodeId}通信重置"); // TODO: 执行通信重置逻辑 break; } // 触发状态改变事件 if (oldState != m_currentState) { OnNmtStateChanged?.Invoke(oldState, m_currentState); } } #endregion #region SYNC处理 /// /// 处理SYNC帧 /// private void ProcessSync(byte[] data) { if (data.Length > 0) { m_syncCounter = data[0]; } // 如果TPDO配置为同步触发,则发送 for (int i = 0; i < 4; i++) { if (m_tpdoConfigs[i].Enabled && m_tpdoConfigs[i].TransmissionType >= 1 && m_tpdoConfigs[i].TransmissionType <= 240) { SendTpdo((byte)(i + 1)); } } } #endregion #region SDO处理 /// /// 处理SDO请求 /// private void ProcessSdoRequest(byte[] data) { if (data.Length < 8) return; byte cs = (byte)(data[0] >> 5); // 命令 specifier ushort index = (ushort)(data[1] | (data[2] << 8)); byte subIndex = data[3]; switch (cs) { case 2: // SDO上传请求 (读) HandleSdoUpload(index, subIndex); break; case 1: // SDO下载请求 (写) HandleSdoDownload(index, subIndex, data); break; default: Console.WriteLine($"未支持的SDO命令: CS={cs}"); break; } } /// /// 处理SDO上传 (读对象字典) /// private void HandleSdoUpload(ushort index, byte subIndex) { byte[] response = new byte[8]; // 触发自定义事件 OnSdoReadRequest?.Invoke(m_nodeId, index, subIndex); // 根据索引返回数据 if (m_objectDictionary.TryGetValue(index, out object value)) { if (subIndex == 0) { // 返回子索引数量 response[0] = 0x4F; // 快速上传响应 response[1] = (byte)(index & 0xFF); response[2] = (byte)(index >> 8); response[3] = subIndex; if (value is IdentityObject identity) { response[4] = 4; // 4个子条目 } else if (value is PdoMappingParam mappingParam) { response[4] = mappingParam.NumberOfEntries; } else { response[4] = 0; } } else { // 返回具体子索引的值 response[0] = 0x4B; // 4字节快速上传 response[1] = (byte)(index & 0xFF); response[2] = (byte)(index >> 8); response[3] = subIndex; uint dataValue = 0; if (index == (ushort)0x1000 && value is uint deviceType) { dataValue = deviceType; } else if (index == (ushort)0x1001 && value is byte errorReg) { response[0] = 0x4F; // 1字节 dataValue = errorReg; } else if (index == (ushort)0x1018 && value is IdentityObject identity) { response[0] = 0x4B; switch (subIndex) { case 1: dataValue = identity.VendorId; break; case 2: dataValue = identity.ProductCode; break; case 3: dataValue = identity.RevisionNumber; break; default: dataValue = 0; break; } } else if ((index >= (ushort)0x1800 && index <= (ushort)0x1803) && value is TpdoCommParam tpdoParam) { switch (subIndex) { case 1: dataValue = tpdoParam.CobId; break; case 2: dataValue = tpdoParam.TransmissionType; break; default: dataValue = 0; break; } } else if ((index >= (ushort)0x1400 && index <= (ushort)0x1403) && value is RpdoCommParam rpdoParam) { switch (subIndex) { case 1: dataValue = rpdoParam.CobId; break; case 2: dataValue = rpdoParam.TransmissionType; break; default: dataValue = 0; break; } } response[4] = (byte)(dataValue & 0xFF); response[5] = (byte)((dataValue >> 8) & 0xFF); response[6] = (byte)((dataValue >> 16) & 0xFF); response[7] = (byte)((dataValue >> 24) & 0xFF); } // 发送SDO响应 uint responseCobId = (uint)0x580 + m_nodeId; m_canManager.SendCanFrame(responseCobId, response); } else { // 对象不存在,发送错误响应 response[0] = 0x80; // 错误响应 response[1] = (byte)(index & 0xFF); response[2] = (byte)(index >> 8); response[3] = subIndex; response[4] = 0x06; // 错误代码: 对象不存在 response[5] = 0x02; response[6] = 0x00; response[7] = 0x00; uint responseCobId = (uint)0x580 + m_nodeId; m_canManager.SendCanFrame(responseCobId, response); } } /// /// 处理SDO下载 (写对象字典) /// private void HandleSdoDownload(ushort index, byte subIndex, byte[] data) { uint value = (uint)(data[4] | (data[5] << 8) | (data[6] << 16) | (data[7] << 24)); // 触发自定义事件 OnSdoWriteRequest?.Invoke(m_nodeId, index, subIndex, value); // 更新对象字典 if (m_objectDictionary.ContainsKey(index)) { if (index >= (ushort)0x1800 && index <= (ushort)0x1803 && m_objectDictionary[index] is TpdoCommParam tpdoParam) { int pdoIndex = index - (ushort)0x1800; if (subIndex == 1) { tpdoParam.CobId = value; m_tpdoConfigs[pdoIndex].CobId = value; } else if (subIndex == 2) { tpdoParam.TransmissionType = (byte)value; m_tpdoConfigs[pdoIndex].TransmissionType = (byte)value; } } else if (index >= (ushort)0x1400 && index <= (ushort)0x1403 && m_objectDictionary[index] is RpdoCommParam rpdoParam) { int pdoIndex = index - (ushort)0x1400; if (subIndex == 1) { rpdoParam.CobId = value; m_rpdoConfigs[pdoIndex].CobId = value; } else if (subIndex == 2) { rpdoParam.TransmissionType = (byte)value; m_rpdoConfigs[pdoIndex].TransmissionType = (byte)value; } } } // 发送SDO确认响应 byte[] response = new byte[8]; response[0] = 0x60; // SDO下载确认 response[1] = (byte)(index & 0xFF); response[2] = (byte)(index >> 8); response[3] = subIndex; uint responseCobId = (uint)0x580 + m_nodeId; m_canManager.SendCanFrame(responseCobId, response); } #endregion #region RPDO处理 /// /// 处理接收到的RPDO /// private void ProcessRpdo(byte pdoNumber, byte[] data, byte length) { Console.WriteLine($"收到RPDO{pdoNumber}: 长度{length}字节"); // 复制有效数据 byte[] validData = new byte[length]; Array.Copy(data, validData, length); // 触发事件 OnRpdoReceived?.Invoke(m_nodeId, pdoNumber, validData); } #endregion #region TPDO发送 /// /// 发送TPDO /// public void SendTpdo(byte pdoNumber) { if (pdoNumber < 1 || pdoNumber > 4) throw new ArgumentException("PDO编号必须在1-4之间"); if (!m_tpdoConfigs[pdoNumber - 1].Enabled) { Console.WriteLine($"TPDO{pdoNumber}未启用"); return; } // TODO: 根据映射表构建PDO数据 // 这里使用示例数据 byte[] pdoData = new byte[8]; // 示例: 填充一些测试数据 pdoData[0] = (byte)(m_syncCounter & 0xFF); pdoData[1] = (byte)((m_syncCounter >> 8) & 0xFF); uint cobId = m_tpdoConfigs[pdoNumber - 1].CobId; m_canManager.SendCanFrame(cobId, pdoData); Console.WriteLine($"发送TPDO{pdoNumber}: COB-ID 0x{cobId:X3}"); } /// /// 发送所有启用的TPDO /// public void SendAllTpdos() { for (byte i = 1; i <= 4; i++) { if (m_tpdoConfigs[i - 1].Enabled) { SendTpdo(i); } } } #endregion #region 心跳机制 /// /// 配置心跳 /// /// 心跳时间(毫秒),0表示禁用 public void ConfigureHeartbeat(ushort heartbeatTimeMs) { m_producerHeartbeatTime = heartbeatTimeMs; // 停止旧的定时器 m_heartbeatTimer?.Dispose(); if (heartbeatTimeMs > 0) { // 创建新的定时器 m_heartbeatTimer = new Timer(state => { if (m_currentState == NmtState.Operational || m_currentState == NmtState.PreOperational) { SendHeartbeat(); } }, null, heartbeatTimeMs, heartbeatTimeMs); Console.WriteLine($"心跳已配置: {heartbeatTimeMs}ms"); } else { Console.WriteLine("心跳已禁用"); } } /// /// 发送心跳 /// private void SendHeartbeat() { byte[] heartbeatData = new byte[1]; // 根据当前状态设置心跳字节 switch (m_currentState) { case NmtState.Initializing: heartbeatData[0] = 0x00; break; case NmtState.PreOperational: heartbeatData[0] = 0x7F; break; case NmtState.Operational: heartbeatData[0] = 0x05; break; case NmtState.Stopped: heartbeatData[0] = 0x04; break; default: heartbeatData[0] = 0x00; break; } uint heartbeatCobId = (uint)0x700 + (uint)m_nodeId; m_canManager.SendCanFrame(heartbeatCobId, heartbeatData); } #endregion #region 辅助方法 /// /// 获取RPDO的COB-ID /// private uint GetRpdoCobId(byte pdoNumber, byte nodeId) { switch (pdoNumber) { case 1: return (uint)0x200 + nodeId; case 2: return (uint)0x300 + nodeId; case 3: return (uint)0x400 + nodeId; case 4: return (uint)0x500 + nodeId; default: throw new ArgumentException("无效的PDO编号"); } } /// /// 获取TPDO的COB-ID /// private uint GetTpdoCobId(byte pdoNumber, byte nodeId) { switch (pdoNumber) { case 1: return (uint)0x180 + nodeId; case 2: return (uint)0x280 + nodeId; case 3: return (uint)0x380 + nodeId; case 4: return (uint)0x480 + nodeId; default: throw new ArgumentException("无效的PDO编号"); } } #endregion #region 资源管理 /// /// 释放资源 /// public void Dispose() { Stop(); m_canManager?.Dispose(); m_cancellationTokenSource?.Dispose(); m_heartbeatTimer?.Dispose(); } #endregion #region 公共属性 /// /// 获取当前节点ID /// public byte NodeId => m_nodeId; /// /// 获取当前NMT状态 /// public NmtState CurrentState => m_currentState; /// /// 获取是否正在运行 /// public bool IsRunning => m_isRunning; #endregion } #region 数据结构定义 /// /// NMT状态枚举 /// public enum NmtState { Initializing = 0x00, PreOperational = 0x7F, Operational = 0x05, Stopped = 0x04 } /// /// PDO配置 /// public class PdoConfig { public uint CobId { get; set; } public byte TransmissionType { get; set; } // 0xFF=事件驱动, 1-240=同步周期 public bool Enabled { get; set; } } /// /// PDO映射 /// public class PdoMapping { public ushort Index { get; set; } public byte SubIndex { get; set; } public byte BitLength { get; set; } } /// /// PDO映射参数 /// public class PdoMappingParam { public byte NumberOfEntries { get; set; } public uint[] Mappings { get; set; } } /// /// 身份对象 (1018h) /// public class IdentityObject { public uint VendorId { get; set; } public uint ProductCode { get; set; } public uint RevisionNumber { get; set; } } /// /// TPDO通信参数 (1800-1803h) /// public class TpdoCommParam { public uint CobId { get; set; } public byte TransmissionType { get; set; } } /// /// RPDO通信参数 (1400-1403h) /// public class RpdoCommParam { public uint CobId { get; set; } public byte TransmissionType { get; set; } } #endregion }