WindowsFormsTest/CanOpenSlaveTest/CanOpenSlaveDevice.cs

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
{
    /// <summary>
    /// CANopen从站设备 - 基于Nameless.eds文件实现
    /// 支持4个RPDO和4个TPDO，符合CiA DS301标准
    /// 直接基于创芯科技CAN卡底层库实现，不依赖CanOpenManager
    /// </summary>
    public class CanOpenSlaveDevice : IDisposable
    {
        #region 私有字段
        
        // CAN设备参数
        private UInt32 m_deviceType = 4;  // USBCAN2
        private UInt32 m_deviceIndex = 0;
        private UInt32 m_canIndex = 0;
        private bool m_isOpened = false;
        private bool m_isStarted = false;
        
        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<ushort, object> m_objectDictionary = new Dictionary<ushort, object>();
        
        // 心跳参数
        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 事件定义
        
        /// <summary>
        /// RPDO接收事件
        /// </summary>
        public event Action<byte, byte, byte[]> OnRpdoReceived;
        
        /// <summary>
        /// NMT状态改变事件
        /// </summary>
        public event Action<NmtState, NmtState> OnNmtStateChanged;
        
        /// <summary>
        /// SDO读取请求事件
        /// </summary>
        public event Action<byte, ushort, byte> OnSdoReadRequest;
        
        /// <summary>
        /// SDO写入请求事件
        /// </summary>
        public event Action<byte, ushort, byte, uint> OnSdoWriteRequest;
        
        #endregion
        
        #region 构造函数
        
        /// <summary>
        /// 构造函数
        /// </summary>
        /// <param name="nodeId">节点ID(1-127)</param>
        /// <param name="deviceType">设备类型</param>
        /// <param name="deviceIndex">设备索引</param>
        /// <param name="canIndex">CAN通道索引</param>
        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_deviceType = deviceType;
            m_deviceIndex = deviceIndex;
            m_canIndex = canIndex;
            
            InitializePdoConfigs();
            InitializeObjectDictionary();
        }
        
        #endregion
        
        #region 初始化方法
        
        /// <summary>
        /// 初始化PDO配置 - 根据EDS文件
        /// </summary>
        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
                    };
                }
            }
        }
        
        /// <summary>
        /// 初始化对象字典 - 根据EDS文件定义
        /// </summary>
        private void InitializeObjectDictionary()
        {
            // 1000h - Device type
            m_objectDictionary[(ushort)0x1000] = (uint)0x00000000;  // 默认值
            
            // 1001h - Error register
            m_objectDictionary[(ushort)0x1001] = (byte)0x00;
            
            // 1017h - Producer Heartbeat Time (心跳时间)
            // 根据CANopen标准，此对象用于配置心跳发送间隔
            m_objectDictionary[(ushort)0x1017] = (ushort)1000;  // 默认1000ms
            
            // 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 CAN底层操作
        
        /// <summary>
        /// 设置波特率参数
        /// </summary>
        private void SetBaudRate(ref VCI_INIT_CONFIG config, CanBaudRate baudRate)
        {
            switch (baudRate)
            {
                case CanBaudRate.BaudRate_1M:
                    config.Timing0 = 0x00;
                    config.Timing1 = 0x14;
                    break;
                case CanBaudRate.BaudRate_500K:
                    config.Timing0 = 0x00;
                    config.Timing1 = 0x1C;
                    break;
                case CanBaudRate.BaudRate_250K:
                    config.Timing0 = 0x01;
                    config.Timing1 = 0x1C;
                    break;
                case CanBaudRate.BaudRate_125K:
                    config.Timing0 = 0x03;
                    config.Timing1 = 0x1C;
                    break;
                default:
                    config.Timing0 = 0x00;
                    config.Timing1 = 0x1C;
                    break;
            }
        }
        
        /// <summary>
        /// 发送原始CAN帧
        /// </summary>
        private unsafe bool SendCanFrame(uint cobId, byte[] data, bool isExtended = false, bool isRemote = false)
        {
            if (!m_isStarted)
            {
                Console.WriteLine("CAN通道未启动");
                return false;
            }

            if (data == null || data.Length > 8)
            {
                Console.WriteLine("SendCanFrame: 数据长度无效(必须0-8字节)");
                return false;
            }

            VCI_CAN_OBJ sendObj = new VCI_CAN_OBJ();
            sendObj.ID = cobId;
            sendObj.ExternFlag = isExtended ? (byte)1 : (byte)0;
            sendObj.RemoteFlag = isRemote ? (byte)1 : (byte)0;
            sendObj.SendType = 0;
            sendObj.TimeFlag = 0;
            sendObj.DataLen = (byte)data.Length;

            // 复制数据
            for (int i = 0; i < data.Length; i++)
            {
                sendObj.Data[i] = data[i];
            }

            // 填充剩余字节
            for (int i = data.Length; i < 8; i++)
            {
                sendObj.Data[i] = 0;
            }

            UInt32 result = CanLibraryClass.VCI_Transmit(m_deviceType, m_deviceIndex, m_canIndex, ref sendObj, 1);

            if (result == 0)
            {
                Console.WriteLine($"发送CAN帧失败 - COB-ID: 0x{cobId:X3}");
                return false;
            }
            if (cobId != 1793)
            {
                string logStr = $"发送CAN帧 - COB-ID: 0x{cobId:X3}, Len: {data.Length}, Data: ";
                for (int i = 0; i < data.Length; i++)
                {
                    logStr += $"{data[i]:X2} ";
                }
                Console.WriteLine(logStr);
            }
            return true;
        }
        
        /// <summary>
        /// 接收CAN帧
        /// </summary>
        private unsafe List<CanOpenFrame> ReceiveCanFrames(int timeoutMs = 100)
        {
            if (!m_isStarted)
            {
                Console.WriteLine("CAN通道未启动");
                return new List<CanOpenFrame>();
            }

            List<CanOpenFrame> receivedFrames = new List<CanOpenFrame>();
            VCI_CAN_OBJ[] receiveBuffer = new VCI_CAN_OBJ[2500];

            UInt32 count = CanLibraryClass.VCI_Receive(
                m_deviceType,
                m_deviceIndex,
                m_canIndex,
                ref receiveBuffer[0],
                2500,
                timeoutMs);

            if (count == 0xFFFFFFFF) count = 0;

            for (UInt32 i = 0; i < count; i++)
            {
                CanOpenFrame frame = new CanOpenFrame();
                frame.CobId = receiveBuffer[i].ID;
                frame.IsExtended = receiveBuffer[i].ExternFlag == 1;
                frame.IsRemote = receiveBuffer[i].RemoteFlag == 1;
                frame.DataLength = receiveBuffer[i].DataLen;
                frame.TimeStamp = receiveBuffer[i].TimeStamp;
                frame.Data = new byte[8];

                for (int j = 0; j < 8; j++)
                {
                    frame.Data[j] = receiveBuffer[i].Data[j];
                }

                receivedFrames.Add(frame);

                // 输出每个CAN帧的详细信息，统一格式
                string dataHex = BitConverter.ToString(frame.Data, 0, frame.DataLength).Replace("-", " ");
                Console.WriteLine($"接收CAN帧 - COB-ID: 0x{frame.CobId:X3}, Len: {frame.DataLength}, Data: {dataHex}");
            }

            return receivedFrames;
        }
        
        #endregion
        
        #region 启动和停止
        
        /// <summary>
        /// 启动从站设备
        /// </summary>
        /// <param name="baudRate">波特率</param>
        /// <returns>是否成功</returns>
        public bool Start(CanBaudRate baudRate = CanBaudRate.BaudRate_1M)
        {
            if (m_isRunning)
            {
                Console.WriteLine("从站设备已在运行");
                return true;
            }
            
            // 1. 打开设备
            UInt32 result = CanLibraryClass.VCI_OpenDevice(m_deviceType, m_deviceIndex, 0);
            if (result == 0)
            {
                Console.WriteLine("打开CAN设备失败");
                return false;
            }
            m_isOpened = true;
            Console.WriteLine("CAN设备打开成功");

            // 2. 初始化CAN通道
            VCI_INIT_CONFIG config = new VCI_INIT_CONFIG();
            config.AccCode = 0x00000000;
            config.AccMask = 0xFFFFFFFF;
            config.Filter = 1;  // 接收所有帧
            config.Mode = 0;    // 正常模式
            SetBaudRate(ref config, baudRate);

            result = CanLibraryClass.VCI_InitCAN(m_deviceType, m_deviceIndex, m_canIndex, ref config);
            if (result == 0)
            {
                Console.WriteLine("初始化CAN通道失败");
                Close();
                return false;
            }
            Console.WriteLine($"CAN通道初始化成功,波特率:{baudRate}");

            // 3. 启动CAN通道
            result = CanLibraryClass.VCI_StartCAN(m_deviceType, m_deviceIndex, m_canIndex);
            if (result == 0)
            {
                Console.WriteLine("启动CAN通道失败");
                Close();
                return false;
            }
            m_isStarted = true;
            Console.WriteLine("CAN通道启动成功");
            
            m_isRunning = true;
            m_currentState = NmtState.Initializing;
            
            // 默认配置心跳为1000ms
            ConfigureHeartbeat(1000);
            
            // 发送无错误紧急报文，表明设备正常
            SendNoErrorEmcy();
            
            // 等待主站发送NMT命令来控制状态转换
            Console.WriteLine($"NMT: 节点{m_nodeId}已初始化，等待主站命令...");
            
            // 启动接收线程
            m_cancellationTokenSource = new CancellationTokenSource();
            m_receiveTask = Task.Run(() => ReceiveLoop(m_cancellationTokenSource.Token));
            
            Console.WriteLine($"CANopen从站启动成功 - 节点ID: {m_nodeId}");
            return true;
        }
        
        /// <summary>
        /// 停止从站设备
        /// </summary>
        public void Stop()
        {
            if (!m_isRunning)
                return;
            
            m_isRunning = false;
            
            // 停止接收线程
            m_cancellationTokenSource?.Cancel();
            m_receiveTask?.Wait(1000);
            m_receiveTask?.Dispose();
            
            // 停止心跳定时器
            m_heartbeatTimer?.Dispose();
            
            // 关闭CAN
            Close();
            
            m_currentState = NmtState.Stopped;
            Console.WriteLine("CANopen从站已停止");
        }
        
        /// <summary>
        /// 关闭CAN设备
        /// </summary>
        private void Close()
        {
            if (m_isStarted)
            {
                CanLibraryClass.VCI_ResetCAN(m_deviceType, m_deviceIndex, m_canIndex);
                m_isStarted = false;
            }

            if (m_isOpened)
            {
                CanLibraryClass.VCI_CloseDevice(m_deviceType, m_deviceIndex);
                m_isOpened = false;
                Console.WriteLine("CAN设备已关闭");
            }
        }
        
        #endregion
        
        #region 接收处理循环
        
        /// <summary>
        /// CAN帧接收循环
        /// </summary>
        private void ReceiveLoop(CancellationToken cancellationToken)
        {
            while (!cancellationToken.IsCancellationRequested)
            {
                try
                {
                    var frames = ReceiveCanFrames(1);
                    
                    foreach (var frame in frames)
                    {
                        ProcessCanFrame(frame);
                    }
                }
                catch (Exception ex)
                {
                    Console.WriteLine($"接收错误: {ex.Message}");
                }
                //Thread.Sleep(1);  // 避免CPU占用过高
            }
        }
        
        /// <summary>
        /// 处理接收到的CAN帧
        /// </summary>
        private void ProcessCanFrame(CanOpenFrame frame)
        {
            // 记录所有接收到的帧(用于调试)
            Console.WriteLine($"[接收详细] COB-ID: 0x{frame.CobId:X3}, Len: {frame.DataLength}, Data: {BitConverter.ToString(frame.Data, 0, frame.DataLength).Replace("-", " ")}");
            
            // 检查是否是NMT命令 (COB-ID = 0x000)
            if (frame.CobId == (uint)0x000 && frame.DataLength >= 2)
            {
                Console.WriteLine($"[NMT命令] COB-ID: 0x{frame.CobId:X3}, Data: {BitConverter.ToString(frame.Data, 0, frame.DataLength).Replace("-", " ")}");
                ProcessNmtCommand(frame.Data);
                return;
            }
            
            // 检查是否是SYNC (COB-ID = 0x080)
            if (frame.CobId == (uint)0x080)
            {
                Console.WriteLine($"[SYNC] COB-ID: 0x{frame.CobId:X3}");
                ProcessSync(frame.Data);
                return;
            }
            
            // 检查是否是SDO请求 (COB-ID = 0x600 + nodeId)
            uint sdoRequestCobId = (uint)0x600 + m_nodeId;
            if (frame.CobId == sdoRequestCobId)
            {
                Console.WriteLine($"[SDO请求] COB-ID: 0x{frame.CobId:X3}, Data: {BitConverter.ToString(frame.Data, 0, frame.DataLength).Replace("-", " ")}");
                ProcessSdoRequest(frame.Data);
                return;
            }
            
            // 检查是否是RPDO
            for (int i = 0; i < 4; i++)
            {
                if (frame.CobId == m_rpdoConfigs[i].CobId && m_rpdoConfigs[i].Enabled)
                {
                    Console.WriteLine($"[RPDO{i+1}] COB-ID: 0x{frame.CobId:X3}, Data: {BitConverter.ToString(frame.Data, 0, frame.DataLength).Replace("-", " ")}");
                    ProcessRpdo((byte)(i + 1), frame.Data, frame.DataLength);
                    return;
                }
            }
            
            // 其他未知帧
            Console.WriteLine($"[未知帧] COB-ID: 0x{frame.CobId:X3}, Len: {frame.DataLength}, Data: {BitConverter.ToString(frame.Data, 0, frame.DataLength).Replace("-", " ")}");
        }
        
        #endregion
        
        #region NMT处理
        
        /// <summary>
        /// 处理NMT命令
        /// </summary>
        private void ProcessNmtCommand(byte[] data)
        {
            byte commandSpecifier = data[0];
            byte nodeId = data[1];
            
            // 检查是否是广播命令或针对本节点的命令
            if (nodeId != 0 && nodeId != m_nodeId)
                return;
            
            NmtState oldState = m_currentState;
            
            Console.WriteLine($"[NMT详细] 收到命令: 0x{commandSpecifier:X2}, 目标节点: {nodeId}, 当前状态: {m_currentState}");
            
            switch (commandSpecifier)
            {
                case 0x01:  // 启动节点
                    Console.WriteLine($"[NMT详细] 执行启动节点命令");
                    m_currentState = NmtState.Operational;
                    Console.WriteLine($"NMT: 节点{m_nodeId}进入运行状态");
                    break;
                    
                case 0x02:  // 停止节点
                    Console.WriteLine($"[NMT详细] 执行停止节点命令");
                    m_currentState = NmtState.Stopped;
                    Console.WriteLine($"NMT: 节点{m_nodeId}进入停止状态");
                    break;
                    
                case 0x80:  // 进入预操作状态
                    Console.WriteLine($"[NMT详细] 执行进入预操作状态命令");
                    m_currentState = NmtState.PreOperational;
                    Console.WriteLine($"NMT: 节点{m_nodeId}进入预操作状态");
                    break;
                    
                case 0x81:  // 重置节点
                    Console.WriteLine($"[NMT详细] 执行重置节点命令 - 开始初始化流程");
                    // 执行节点重置逻辑
                    m_currentState = NmtState.Initializing;
                    
                    // 打印当前对象字典状态(用于调试)
                    Console.WriteLine($"[NMT调试] 当前对象字典关键参数:");
                    if (m_objectDictionary.ContainsKey((ushort)0x1000))
                        Console.WriteLine($"  - 设备类型(0x1000): {m_objectDictionary[(ushort)0x1000]}");
                    if (m_objectDictionary.ContainsKey((ushort)0x1001))
                        Console.WriteLine($"  - 错误寄存器(0x1001): {m_objectDictionary[(ushort)0x1001]}");
                    if (m_objectDictionary.ContainsKey((ushort)0x1017))
                        Console.WriteLine($"  - 心跳时间(0x1017): {m_objectDictionary[(ushort)0x1017]}ms");
                    if (m_objectDictionary.ContainsKey((ushort)0x1018))
                    {
                        var identity = m_objectDictionary[(ushort)0x1018] as IdentityObject;
                        if (identity != null)
                            Console.WriteLine($"  - 厂商ID(0x1018.1): 0x{identity.VendorId:X4}");
                    }
                    
                    // 短暂延迟后自动进入预操作状态（模拟初始化过程）
                    Task.Delay(50).ContinueWith(_ => 
                    {
                        // 立即发送一次心跳，确认状态转换
                        SendHeartbeat();

                        m_currentState = NmtState.PreOperational;
                        Console.WriteLine($"NMT: 节点{m_nodeId}初始化完成，进入预操作状态");
                        

                        
                        OnNmtStateChanged?.Invoke(oldState, m_currentState);
                    });
                    return;  // 提前返回，等待异步完成
                    
                case 0x82:  // 重置通信
                    Console.WriteLine($"[NMT详细] 执行重置通信命令");
                    m_currentState = NmtState.PreOperational;
                    Console.WriteLine($"NMT: 节点{m_nodeId}通信重置");
                    // TODO: 执行通信重置逻辑
                    break;
                    
                default:
                    Console.WriteLine($"[NMT详细] 未知命令: 0x{commandSpecifier:X2}");
                    break;
            }
            
            // 触发状态改变事件
            if (oldState != m_currentState)
            {
                Console.WriteLine($"[NMT详细] 状态改变: {oldState} → {m_currentState}");
                OnNmtStateChanged?.Invoke(oldState, m_currentState);
            }
            else
            {
                Console.WriteLine($"[NMT详细] 状态未改变: {oldState}");
            }
        }
        
        #endregion
        
        #region SYNC处理
        
        /// <summary>
        /// 处理SYNC帧
        /// </summary>
        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处理
        
        /// <summary>
        /// 处理SDO请求
        /// </summary>
        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;
            }
        }
        
        /// <summary>
        /// 处理SDO上传 (读对象字典)
        /// </summary>
        private void HandleSdoUpload(ushort index, byte subIndex)
        {
            byte[] response = new byte[8];
            
            Console.WriteLine($"[SDO详细] 收到上传请求: 索引0x{index:X4}, 子索引{subIndex}");
            
            // 触发自定义事件
            OnSdoReadRequest?.Invoke(m_nodeId, index, subIndex);
            
            // 根据索引返回数据
            if (m_objectDictionary.TryGetValue(index, out object value))
            {
                Console.WriteLine($"[SDO详细] 找到对象 0x{index:X4}");
                
                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个子条目
                        Console.WriteLine($"[SDO详细] 返回Identity对象，子条目数: 4");
                    }
                    else if (value is PdoMappingParam mappingParam)
                    {
                        response[4] = mappingParam.NumberOfEntries;
                        Console.WriteLine($"[SDO详细] 返回PDO映射参数，子条目数: {mappingParam.NumberOfEntries}");
                    }
                    else
                    {
                        response[4] = 0;
                        Console.WriteLine($"[SDO详细] 返回简单对象，子条目数: 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;
                        Console.WriteLine($"[SDO详细] 返回设备类型: 0x{dataValue:X8}");
                    }
                    else if (index == (ushort)0x1001 && value is byte errorReg)
                    {
                        response[0] = 0x4F;  // 1字节
                        dataValue = errorReg;
                        Console.WriteLine($"[SDO详细] 返回错误寄存器: 0x{dataValue:X2}");
                    }
                    else if (index == (ushort)0x1017 && value is ushort heartbeatTime)
                    {
                        response[0] = 0x4B;  // 2字节
                        dataValue = heartbeatTime;
                        Console.WriteLine($"[SDO详细] 返回心跳时间: {heartbeatTime}ms");
                    }
                    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;
                        }
                        Console.WriteLine($"[SDO详细] 返回Identity子索引{subIndex}: 0x{dataValue:X8}");
                    }
                    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;
                        }
                        Console.WriteLine($"[SDO详细] 返回TPDO{index - 0x1800 + 1}通信参数子索引{subIndex}: 0x{dataValue:X8}");
                    }
                    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;
                        }
                        Console.WriteLine($"[SDO详细] 返回RPDO{index - 0x1400 + 1}通信参数子索引{subIndex}: 0x{dataValue:X8}");
                    }
                    
                    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;
                Console.WriteLine($"[SDO响应] COB-ID: 0x{responseCobId:X3}, Index: 0x{index:X4}, SubIndex: {subIndex}, Data: {BitConverter.ToString(response).Replace("-", " ")}");
                SendCanFrame(responseCobId, response);
            }
            else
            {
                // 对象不存在，发送错误响应
                Console.WriteLine($"[SDO详细] 对象 0x{index:X4} 不存在，返回错误");
                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;
                Console.WriteLine($"[SDO错误响应] COB-ID: 0x{responseCobId:X3}, Index: 0x{index:X4}, SubIndex: {subIndex}");
                SendCanFrame(responseCobId, response);
            }
        }
        
        /// <summary>
        /// 处理SDO下载 (写对象字典)
        /// </summary>
        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))
            {
                // 特殊处理：心跳时间对象 (0x1017)
                if (index == (ushort)0x1017 && subIndex == 0)
                {
                    ushort heartbeatTime = (ushort)value;
                    m_objectDictionary[index] = heartbeatTime;
                    ConfigureHeartbeat(heartbeatTime);  // 重新配置心跳
                    Console.WriteLine($"[SDO写入] 心跳时间已更新为: {heartbeatTime}ms");
                }
                else 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;
            SendCanFrame(responseCobId, response);
        }
        
        #endregion
        
        #region RPDO处理
        
        /// <summary>
        /// 处理接收到的RPDO
        /// </summary>
        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, data);
        }

        #endregion

        #region TPDO发送

        /// <summary>
        /// 发送TPDO
        /// 长度不可以超过PLC中PDO映射的长度
        /// </summary>
        public void SendTpdo(byte pdoNumber, byte[] pdoData)
        {
            if (pdoNumber < 1 || pdoNumber > 4)
                throw new ArgumentException("PDO编号必须在1-4之间");
            
            if (!m_tpdoConfigs[pdoNumber - 1].Enabled)
            {
                Console.WriteLine($"TPDO{pdoNumber}未启用");
                return;
            }
            
            uint cobId = m_tpdoConfigs[pdoNumber - 1].CobId;
            SendCanFrame(cobId, pdoData);
            
            Console.WriteLine($"发送TPDO{pdoNumber}: COB-ID 0x{cobId:X3}");
        }
       
        
        #endregion
        
        #region 心跳机制
        
        /// <summary>
        /// 配置心跳
        /// </summary>
        /// <param name="heartbeatTimeMs">心跳时间(毫秒)，0表示禁用</param>
        public void ConfigureHeartbeat(ushort heartbeatTimeMs)
        {
            m_producerHeartbeatTime = heartbeatTimeMs;
            
            // 同步更新对象字典中的 1017h 对象
            if (m_objectDictionary.ContainsKey((ushort)0x1017))
            {
                m_objectDictionary[(ushort)0x1017] = heartbeatTimeMs;
            }
            
            // 停止旧的定时器
            m_heartbeatTimer?.Dispose();
            
            if (heartbeatTimeMs > 0)
            {
                // 创建新的定时器
                m_heartbeatTimer = new Timer(state =>
                {
                    // 只在PreOperational和Operational状态下发送心跳
                    // Initializing和Stopped状态下不发送心跳
                    if (m_currentState == NmtState.Operational || 
                        m_currentState == NmtState.PreOperational)
                    {
                        SendHeartbeat();
                    }
                    else
                    {
                        // 在其他状态下跳过心跳发送
                        //Console.WriteLine($"[心跳跳过] 当前状态: {m_currentState}, 不发送心跳");
                    }
                }, null, heartbeatTimeMs, heartbeatTimeMs);
                
                Console.WriteLine($"心跳已配置: {heartbeatTimeMs}ms");
            }
            else
            {
                Console.WriteLine("心跳已禁用");
            }
        }
        
        /// <summary>
        /// 发送心跳
        /// </summary>
        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;
            //Console.WriteLine($"[心跳发送] COB-ID: 0x{heartbeatCobId:X3}, Data: {heartbeatData[0]:X2} (状态: {m_currentState})");
            SendCanFrame(heartbeatCobId, heartbeatData);
        }
        
        /// <summary>
        /// 发送无错误紧急报文 (EMCY)
        /// 用于在启动时表明设备无错误状态
        /// </summary>
        private void SendNoErrorEmcy()
        {
            byte[] emcyData = new byte[8];
            emcyData[0] = 0x00;  // 错误代码低字节 - 无错误
            emcyData[1] = 0x00;  // 错误代码高字节
            emcyData[2] = 0x00;  // 错误寄存器
            
            uint emcyCobId = (uint)0x080 + (uint)m_nodeId;
            Console.WriteLine($"[EMCY] 发送无错误报文 - COB-ID: 0x{emcyCobId:X3}");
            SendCanFrame(emcyCobId, emcyData);
        }
        #endregion
        
        #region 辅助方法
        
        /// <summary>
        /// 获取RPDO的COB-ID
        /// </summary>
        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编号");
            }
        }
        
        /// <summary>
        /// 获取TPDO的COB-ID
        /// </summary>
        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 资源管理
        
        /// <summary>
        /// 释放资源
        /// </summary>
        public void Dispose()
        {
            Stop();
            m_cancellationTokenSource?.Dispose();
            m_heartbeatTimer?.Dispose();
        }
        
        #endregion
        
        #region 公共属性
        
        /// <summary>
        /// 获取当前节点ID
        /// </summary>
        public byte NodeId => m_nodeId;
        
        /// <summary>
        /// 获取当前NMT状态
        /// </summary>
        public NmtState CurrentState => m_currentState;
        
        /// <summary>
        /// 获取是否正在运行
        /// </summary>
        public bool IsRunning => m_isRunning;
        
        #endregion
    }
    
    #region 数据结构定义
    
    /// <summary>
    /// NMT状态枚举
    /// </summary>
    public enum NmtState
    {
        Initializing = 0x00,
        PreOperational = 0x7F,
        Operational = 0x05,
        Stopped = 0x04
    }
    
    /// <summary>
    /// PDO配置
    /// </summary>
    public class PdoConfig
    {
        public uint CobId { get; set; }
        public byte TransmissionType { get; set; }  // 0xFF=事件驱动, 1-240=同步周期
        public bool Enabled { get; set; }
    }
    
    /// <summary>
    /// PDO映射
    /// </summary>
    public class PdoMapping
    {
        public ushort Index { get; set; }
        public byte SubIndex { get; set; }
        public byte BitLength { get; set; }
    }
    
    /// <summary>
    /// PDO映射参数
    /// </summary>
    public class PdoMappingParam
    {
        public byte NumberOfEntries { get; set; }
        public uint[] Mappings { get; set; }
    }
    
    /// <summary>
    /// 身份对象 (1018h)
    /// </summary>
    public class IdentityObject
    {
        public uint VendorId { get; set; }
        public uint ProductCode { get; set; }
        public uint RevisionNumber { get; set; }
    }
    
    /// <summary>
    /// TPDO通信参数 (1800-1803h)
    /// </summary>
    public class TpdoCommParam
    {
        public uint CobId { get; set; }
        public byte TransmissionType { get; set; }
    }
    
    /// <summary>
    /// RPDO通信参数 (1400-1403h)
    /// </summary>
    public class RpdoCommParam
    {
        public uint CobId { get; set; }
        public byte TransmissionType { get; set; }
    }

    /// <summary>
    /// CANopen帧数据结构
    /// </summary>
    public class CanOpenFrame
    {
        public uint CobId { get; set; }
        public bool IsExtended { get; set; }
        public bool IsRemote { get; set; }
        public byte DataLength { get; set; }
        public byte[] Data { get; set; }
        public uint TimeStamp { get; set; }

        public override string ToString()
        {
            string dataStr = "";
            for (int i = 0; i < DataLength; i++)
            {
                dataStr += $"{Data[i]:X2} ";
            }
            return $"COB-ID: 0x{CobId:X3}, Len: {DataLength}, Data: [{dataStr}]";
        }
    }

    #endregion
}