QUYVN/I150
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Initial commit

Browse Source
This commit is contained in:
QUYVN
2026-07-03 16:37:12 +07:00
commit 63b8c1ea8b
1931 changed files with 640587 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

204
srcs/RobotNet10/RobotApp/Communication/RobotNet10.CANOpen/Models/SdoMessage.cs Normal file
View File

@@ -0,0 +1,204 @@
using RobotNet10.CANOpen.Enums;
namespace RobotNet10.CANOpen.Models;
public readonly struct SdoRequest
{
public byte CommandSpecifier { get; init; }
public ushort Index { get; init; }
public byte SubIndex { get; init; }
public uint Data { get; init; }
public SdoRequest(SdoCommand command, ushort index, byte subIndex, uint data = 0)
{
CommandSpecifier = (byte)command;
Index = index;
SubIndex = subIndex;
Data = data;
}
public SdoRequest(SdoCommand command, ObjectDictionaryAddress address, uint data = 0)
: this(command, address.Index, address.SubIndex, data)
{
}
public byte[] ToBytes()
{
var bytes = new byte[8];
bytes[0] = CommandSpecifier;
bytes[1] = (byte)(Index & 0xFF);
bytes[2] = (byte)((Index >> 8) & 0xFF);
bytes[3] = SubIndex;
bytes[4] = (byte)(Data & 0xFF);
bytes[5] = (byte)((Data >> 8) & 0xFF);
bytes[6] = (byte)((Data >> 16) & 0xFF);
bytes[7] = (byte)((Data >> 24) & 0xFF);
return bytes;
}
public static SdoRequest CreateUpload(ushort index, byte subIndex)
=> new(SdoCommand.UploadInitiate, index, subIndex);
public static SdoRequest CreateDownload(ushort index, byte subIndex, byte[] data)
{
if (data.Length > 4)
throw new ArgumentException("Use CreateDownloadInitiate for data > 4 bytes");
byte cs = (byte)((byte)SdoCommand.DownloadInitiate | 0x03);
int n = 4 - data.Length;
cs |= (byte)((n << 2) & 0x0C);
uint dataValue = 0;
for (int i = 0; i < data.Length; i++)
dataValue |= (uint)(data[i] << (i * 8));
return new SdoRequest { CommandSpecifier = cs, Index = index, SubIndex = subIndex, Data = dataValue };
}
/// <summary>
/// Create download initiate request for segmented transfer (data > 4 bytes)
/// </summary>
public static SdoRequest CreateDownloadInitiate(ushort index, byte subIndex, int dataLength)
{
if (dataLength <= 4)
throw new ArgumentException("Use CreateDownload for data <= 4 bytes");
// Download Initiate: bit 0 = 0 (not expedited), bit 1 = 1 (size indicated), bit 2 = 0 (not complete)
byte cs = (byte)SdoCommand.DownloadInitiate;
cs |= 0x02; // Size indicated
// Data contains size (lower 3 bytes) and number of segments not used (upper byte = 0)
uint sizeData = (uint)dataLength;
return new SdoRequest { CommandSpecifier = cs, Index = index, SubIndex = subIndex, Data = sizeData };
}
/// <summary>
/// Create download segment request
/// </summary>
/// <param name="toggle">Toggle bit (0 or 1), alternates for each segment</param>
/// <param name="isLastSegment">True if this is the last segment</param>
/// <param name="segmentData">Data for this segment (up to 7 bytes)</param>
public static SdoRequest CreateDownloadSegment(byte toggle, bool isLastSegment, byte[] segmentData)
{
if (segmentData.Length > 7)
throw new ArgumentException("Segment data cannot exceed 7 bytes");
byte cs = (byte)SdoCommand.DownloadSegment;
if (toggle != 0)
cs |= 0x10; // Toggle bit
if (isLastSegment)
cs |= 0x01; // Last segment bit
int n = 7 - segmentData.Length;
cs |= (byte)((n << 1) & 0x0E);
uint dataValue = 0;
for (int i = 0; i < segmentData.Length; i++)
dataValue |= (uint)(segmentData[i] << (i * 8));
return new SdoRequest { CommandSpecifier = cs, Index = 0, SubIndex = 0, Data = dataValue };
}
/// <summary>
/// Create upload segment request
/// </summary>
/// <param name="toggle">Toggle bit (0 or 1), alternates for each segment</param>
public static SdoRequest CreateUploadSegment(byte toggle)
{
byte cs = (byte)SdoCommand.UploadSegment;
if (toggle != 0)
cs |= 0x10; // Toggle bit
return new SdoRequest { CommandSpecifier = cs, Index = 0, SubIndex = 0, Data = 0 };
}
}
public readonly struct SdoResponse
{
public byte CommandSpecifier { get; init; }
public ushort Index { get; init; }
public byte SubIndex { get; init; }
public uint Data { get; init; }
public bool IsAbort => (CommandSpecifier & 0xE0) == (byte)SdoCommand.Abort;
public SdoAbortCode AbortCode => (SdoAbortCode)Data;
public static SdoResponse FromBytes(byte[] data)
{
if (data.Length < 8)
throw new ArgumentException("SDO response must be 8 bytes");
return new SdoResponse
{
CommandSpecifier = data[0],
Index = (ushort)(data[1] | (data[2] << 8)),
SubIndex = data[3],
Data = (uint)(data[4] | (data[5] << 8) | (data[6] << 16) | (data[7] << 24))
};
}
public byte[] GetDataBytes()
{
if ((CommandSpecifier & 0x02) != 0)
{
int n = (CommandSpecifier >> 2) & 0x03;
int dataLength = 4 - n;
var bytes = new byte[dataLength];
for (int i = 0; i < dataLength; i++)
bytes[i] = (byte)((Data >> (i * 8)) & 0xFF);
return bytes;
}
return BitConverter.GetBytes(Data);
}
/// <summary>
/// Check if response is expedited transfer
/// </summary>
public bool IsExpedited => (CommandSpecifier & 0x02) != 0;
/// <summary>
/// Check if response indicates size (for segmented transfer)
/// </summary>
public bool IsSizeIndicated => (CommandSpecifier & 0x02) != 0;
/// <summary>
/// Get data size from upload initiate response (for segmented transfer)
/// </summary>
public int GetDataSize()
{
if (!IsSizeIndicated)
return 4; // Expedited transfer
return (int)(Data & 0xFFFFFF); // Lower 3 bytes contain size
}
/// <summary>
/// Get segment data from upload segment response
/// </summary>
public byte[] GetSegmentData()
{
int n = (CommandSpecifier >> 1) & 0x07;
int dataLength = 7 - n;
var bytes = new byte[dataLength];
for (int i = 0; i < dataLength; i++)
bytes[i] = (byte)((Data >> (i * 8)) & 0xFF);
return bytes;
}
/// <summary>
/// Check if this is the last segment
/// </summary>
public bool IsLastSegment => (CommandSpecifier & 0x01) != 0;
/// <summary>
/// Get toggle bit from segment response
/// </summary>
public byte GetToggle() => (byte)((CommandSpecifier >> 4) & 0x01);
}