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
}