Initial commit
This commit is contained in:
@@ -0,0 +1,167 @@
|
||||
namespace RobotNet10.CANOpen.Models;
|
||||
|
||||
/// <summary>
|
||||
/// Emergency (EMCY) Error Codes theo CANopen standard
|
||||
/// </summary>
|
||||
public enum EmergencyErrorCode : ushort
|
||||
{
|
||||
// 0x00xx: Error Reset / No Error
|
||||
ErrorReset = 0x0000,
|
||||
|
||||
// 0x10xx: Generic Error
|
||||
GenericError = 0x1000,
|
||||
|
||||
// 0x20xx: Current
|
||||
CurrentGeneric = 0x2000,
|
||||
CurrentInputSide = 0x2100,
|
||||
CurrentInsideDevice = 0x2200,
|
||||
CurrentOutputSide = 0x2300,
|
||||
|
||||
// 0x21xx: Current, device input side
|
||||
OverCurrent = 0x2110,
|
||||
|
||||
// 0x22xx: Current inside the device
|
||||
ShortCircuit = 0x2220,
|
||||
|
||||
// 0x23xx: Current, device output side
|
||||
LoadDump = 0x2330,
|
||||
|
||||
// 0x30xx: Voltage
|
||||
VoltageGeneric = 0x3000,
|
||||
MainsVoltage = 0x3100,
|
||||
VoltageInsideDevice = 0x3200,
|
||||
OutputVoltage = 0x3300,
|
||||
|
||||
// 0x31xx: Mains Voltage
|
||||
UnderVoltage = 0x3110,
|
||||
OverVoltage = 0x3120,
|
||||
|
||||
// 0x40xx: Temperature
|
||||
TemperatureGeneric = 0x4000,
|
||||
AmbientTemperature = 0x4100,
|
||||
DeviceTemperature = 0x4200,
|
||||
|
||||
// 0x41xx: Ambient Temperature
|
||||
TooHigh = 0x4110,
|
||||
TooLow = 0x4120,
|
||||
|
||||
// 0x50xx: Device Hardware
|
||||
DeviceHardware = 0x5000,
|
||||
|
||||
// 0x60xx: Device Software
|
||||
DeviceSoftware = 0x6000,
|
||||
InternalSoftware = 0x6100,
|
||||
UserSoftware = 0x6200,
|
||||
DataSet = 0x6300,
|
||||
|
||||
// 0x70xx: Additional Modules
|
||||
AdditionalModules = 0x7000,
|
||||
|
||||
// 0x80xx: Monitoring
|
||||
Monitoring = 0x8000,
|
||||
Communication = 0x8100,
|
||||
ProtocolError = 0x8200,
|
||||
|
||||
// 0x81xx: Communication
|
||||
CanOverrun = 0x8110,
|
||||
ErrorPassive = 0x8120,
|
||||
HeartbeatError = 0x8130,
|
||||
BusOffRecovered = 0x8140,
|
||||
|
||||
// 0x82xx: Protocol Error
|
||||
PdoNotProcessed = 0x8210,
|
||||
PdoLengthExceeded = 0x8220,
|
||||
DamMpdo = 0x8230,
|
||||
SyncDataLength = 0x8240,
|
||||
|
||||
// 0x90xx: External Error
|
||||
ExternalError = 0x9000,
|
||||
|
||||
// 0xF0xx: Additional Functions
|
||||
AdditionalFunctions = 0xF000,
|
||||
|
||||
// 0xFFxx: Device specific
|
||||
DeviceSpecific = 0xFF00
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Emergency Message theo CANopen
|
||||
/// </summary>
|
||||
public readonly struct EmergencyMessage(byte nodeId, EmergencyErrorCode errorCode, byte errorRegister,
|
||||
byte[] manufacturerError, DateTime timestamp)
|
||||
{
|
||||
/// <summary>
|
||||
/// Node ID của device gửi emergency
|
||||
/// </summary>
|
||||
public byte NodeId { get; init; } = nodeId;
|
||||
|
||||
/// <summary>
|
||||
/// Emergency Error Code (16-bit)
|
||||
/// </summary>
|
||||
public EmergencyErrorCode ErrorCode { get; init; } = errorCode;
|
||||
|
||||
/// <summary>
|
||||
/// Error Register (8-bit)
|
||||
/// </summary>
|
||||
public byte ErrorRegister { get; init; } = errorRegister;
|
||||
|
||||
/// <summary>
|
||||
/// Manufacturer Specific Error Code (40-bit / 5 bytes)
|
||||
/// </summary>
|
||||
public byte[] ManufacturerError { get; init; } = manufacturerError;
|
||||
|
||||
/// <summary>
|
||||
/// Timestamp khi nhận emergency
|
||||
/// </summary>
|
||||
public DateTime Timestamp { get; init; } = timestamp;
|
||||
|
||||
/// <summary>
|
||||
/// Parse Emergency message từ CAN frame
|
||||
/// </summary>
|
||||
public static EmergencyMessage FromCanFrame(uint canId, byte[] data, DateTime timestamp)
|
||||
{
|
||||
if (data.Length < 8)
|
||||
throw new ArgumentException($"Emergency message must be 8 bytes: {BitConverter.ToString(data)}");
|
||||
|
||||
byte nodeId = (byte)(canId & 0x7F);
|
||||
ushort errorCode = (ushort)(data[0] | (data[1] << 8));
|
||||
byte errorRegister = data[2];
|
||||
byte[] manufacturerError = new byte[5];
|
||||
Array.Copy(data, 3, manufacturerError, 0, 5);
|
||||
|
||||
return new EmergencyMessage(nodeId, (EmergencyErrorCode)errorCode, errorRegister,
|
||||
manufacturerError, timestamp);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Convert sang CAN frame data
|
||||
/// </summary>
|
||||
public byte[] ToBytes()
|
||||
{
|
||||
byte[] data = new byte[8];
|
||||
data[0] = (byte)((ushort)ErrorCode & 0xFF);
|
||||
data[1] = (byte)(((ushort)ErrorCode >> 8) & 0xFF);
|
||||
data[2] = ErrorRegister;
|
||||
|
||||
if (ManufacturerError != null && ManufacturerError.Length >= 5)
|
||||
{
|
||||
Array.Copy(ManufacturerError, 0, data, 3, 5);
|
||||
}
|
||||
|
||||
return data;
|
||||
}
|
||||
|
||||
// Error Register bit definitions
|
||||
public bool GenericErrorBit => (ErrorRegister & 0x01) != 0;
|
||||
public bool CurrentErrorBit => (ErrorRegister & 0x02) != 0;
|
||||
public bool VoltageErrorBit => (ErrorRegister & 0x04) != 0;
|
||||
public bool TemperatureErrorBit => (ErrorRegister & 0x08) != 0;
|
||||
public bool CommunicationErrorBit => (ErrorRegister & 0x10) != 0;
|
||||
public bool DeviceProfileErrorBit => (ErrorRegister & 0x20) != 0;
|
||||
public bool ManufacturerErrorBit => (ErrorRegister & 0x80) != 0;
|
||||
|
||||
public override string ToString()
|
||||
{
|
||||
return $"EMCY[Node {NodeId}]: Code=0x{(ushort)ErrorCode:X4}, Register=0x{ErrorRegister:X2}";
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,51 @@
|
||||
using RobotNet10.CANOpen.Enums;
|
||||
|
||||
namespace RobotNet10.CANOpen.Models;
|
||||
|
||||
public readonly struct NmtMessage(NmtCommand command, byte nodeId)
|
||||
{
|
||||
public NmtCommand Command { get; init; } = command;
|
||||
public byte NodeId { get; init; } = nodeId;
|
||||
|
||||
public byte[] ToBytes()
|
||||
{
|
||||
return [(byte)Command, NodeId];
|
||||
}
|
||||
|
||||
public static NmtMessage FromBytes(byte[] data)
|
||||
{
|
||||
if (data.Length < 2)
|
||||
throw new ArgumentException("NMT message must be at least 2 bytes");
|
||||
|
||||
return new NmtMessage
|
||||
{
|
||||
Command = (NmtCommand)data[0],
|
||||
NodeId = data[1]
|
||||
};
|
||||
}
|
||||
}
|
||||
|
||||
public readonly struct HeartbeatMessage(byte nodeId, NmtState state)
|
||||
{
|
||||
public byte NodeId { get; init; } = nodeId;
|
||||
public NmtState State { get; init; } = state;
|
||||
|
||||
public byte[] ToBytes()
|
||||
{
|
||||
return [(byte)State];
|
||||
}
|
||||
|
||||
public static HeartbeatMessage FromBytes(uint canId, byte[] data)
|
||||
{
|
||||
if (data.Length < 1)
|
||||
throw new ArgumentException("Heartbeat message must be at least 1 byte");
|
||||
|
||||
byte nodeId = (byte)(canId & 0x7F);
|
||||
|
||||
return new HeartbeatMessage
|
||||
{
|
||||
NodeId = nodeId,
|
||||
State = (NmtState)data[0]
|
||||
};
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,21 @@
|
||||
namespace RobotNet10.CANOpen.Models;
|
||||
|
||||
public readonly struct ObjectDictionaryAddress(ushort index, byte subIndex = 0)
|
||||
{
|
||||
public ushort Index { get; init; } = index;
|
||||
public byte SubIndex { get; init; } = subIndex;
|
||||
|
||||
public override string ToString() => $"0x{Index:X4}:{SubIndex:X2}";
|
||||
|
||||
public static bool operator ==(ObjectDictionaryAddress left, ObjectDictionaryAddress right)
|
||||
=> left.Index == right.Index && left.SubIndex == right.SubIndex;
|
||||
|
||||
public static bool operator !=(ObjectDictionaryAddress left, ObjectDictionaryAddress right)
|
||||
=> !(left == right);
|
||||
|
||||
public override bool Equals(object? obj)
|
||||
=> obj is ObjectDictionaryAddress address && this == address;
|
||||
|
||||
public override int GetHashCode()
|
||||
=> HashCode.Combine(Index, SubIndex);
|
||||
}
|
||||
@@ -0,0 +1,276 @@
|
||||
using RobotNet10.CANOpen.Enums;
|
||||
|
||||
namespace RobotNet10.CANOpen.Models;
|
||||
|
||||
/// <summary>
|
||||
/// PDO Mapping entry (đại diện cho 1 object trong PDO)
|
||||
/// </summary>
|
||||
public readonly struct PdoMapping(ushort index, byte subIndex, byte bitLength)
|
||||
{
|
||||
public ushort Index { get; init; } = index;
|
||||
public byte SubIndex { get; init; } = subIndex;
|
||||
public byte BitLength { get; init; } = bitLength;
|
||||
|
||||
/// <summary>
|
||||
/// Convert sang mapping value format (theo DS301)
|
||||
/// Format: [Index:16][SubIndex:8][BitLength:8]
|
||||
/// </summary>
|
||||
public uint ToMappingValue()
|
||||
{
|
||||
return ((uint)Index << 16) | ((uint)SubIndex << 8) | BitLength;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Parse từ mapping value
|
||||
/// </summary>
|
||||
public static PdoMapping FromMappingValue(uint mappingValue)
|
||||
{
|
||||
ushort index = (ushort)((mappingValue >> 16) & 0xFFFF);
|
||||
byte subIndex = (byte)((mappingValue >> 8) & 0xFF);
|
||||
byte bitLength = (byte)(mappingValue & 0xFF);
|
||||
|
||||
return new PdoMapping(index, subIndex, bitLength);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// PDO Configuration (TPDO hoặc RPDO)
|
||||
/// </summary>
|
||||
public class PdoConfiguration
|
||||
{
|
||||
private readonly List<PdoMapping> _mappings = [];
|
||||
|
||||
public PdoConfiguration()
|
||||
{
|
||||
}
|
||||
|
||||
public PdoConfiguration(byte pdoNumber, uint cobId, PdoTransmissionType transmissionType = PdoTransmissionType.Asynchronous)
|
||||
{
|
||||
if (pdoNumber < 1 || pdoNumber > 4)
|
||||
throw new ArgumentException("PDO number must be between 1 and 4", nameof(pdoNumber));
|
||||
|
||||
PdoNumber = pdoNumber;
|
||||
CobId = cobId;
|
||||
TransmissionType = transmissionType;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// PDO number (1-4 cho TPDO, 1-4 cho RPDO)
|
||||
/// </summary>
|
||||
public byte PdoNumber { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// COB-ID của PDO
|
||||
/// </summary>
|
||||
public uint CobId { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// Transmission type
|
||||
/// </summary>
|
||||
public PdoTransmissionType TransmissionType { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// Inhibit time (x100 microseconds)
|
||||
/// </summary>
|
||||
public ushort InhibitTime { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// Event timer (milliseconds)
|
||||
/// </summary>
|
||||
public ushort EventTimer { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// Danh sách các mapped objects (read-only)
|
||||
/// </summary>
|
||||
public IReadOnlyList<PdoMapping> Mappings => _mappings.AsReadOnly();
|
||||
|
||||
/// <summary>
|
||||
/// Check if PDO is valid (bit 31 của COB-ID = 0)
|
||||
/// </summary>
|
||||
public bool IsValid => (CobId & 0x80000000) == 0;
|
||||
|
||||
/// <summary>
|
||||
/// Check if RTR is allowed (bit 30 của COB-ID = 0)
|
||||
/// </summary>
|
||||
public bool RtrAllowed => (CobId & 0x40000000) == 0;
|
||||
|
||||
/// <summary>
|
||||
/// Total bits mapped in this PDO
|
||||
/// </summary>
|
||||
public int TotalMappedBits => _mappings.Sum(m => m.BitLength);
|
||||
|
||||
/// <summary>
|
||||
/// Check if mapping is valid (không vượt quá 64 bits)
|
||||
/// </summary>
|
||||
public bool IsMappingValid => TotalMappedBits <= 64;
|
||||
|
||||
/// <summary>
|
||||
/// Add mapping với validation
|
||||
/// </summary>
|
||||
public void AddMapping(PdoMapping mapping)
|
||||
{
|
||||
if (mapping.BitLength == 0 || mapping.BitLength > 64)
|
||||
throw new ArgumentException("BitLength must be between 1 and 64", nameof(mapping));
|
||||
|
||||
if (TotalMappedBits + mapping.BitLength > 64)
|
||||
throw new InvalidOperationException($"Adding this mapping would exceed 64 bits limit. Current: {TotalMappedBits}, Adding: {mapping.BitLength}");
|
||||
|
||||
_mappings.Add(mapping);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Remove mapping
|
||||
/// </summary>
|
||||
public bool RemoveMapping(PdoMapping mapping)
|
||||
{
|
||||
return _mappings.Remove(mapping);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Clear all mappings
|
||||
/// </summary>
|
||||
public void ClearMappings()
|
||||
{
|
||||
_mappings.Clear();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Validate entire configuration
|
||||
/// </summary>
|
||||
public ValidationResult ValidateConfiguration()
|
||||
{
|
||||
var errors = new List<string>();
|
||||
|
||||
if (PdoNumber < 1 || PdoNumber > 4)
|
||||
errors.Add("PDO number must be between 1 and 4");
|
||||
|
||||
if (!IsValid)
|
||||
errors.Add("PDO is disabled (bit 31 of COB-ID is set)");
|
||||
|
||||
if (!IsMappingValid)
|
||||
errors.Add($"Total mapped bits ({TotalMappedBits}) exceeds 64 bits limit");
|
||||
|
||||
if (_mappings.Count == 0)
|
||||
errors.Add("No mappings configured");
|
||||
|
||||
// Validate individual mappings
|
||||
for (int i = 0; i < _mappings.Count; i++)
|
||||
{
|
||||
var mapping = _mappings[i];
|
||||
if (mapping.Index == 0)
|
||||
errors.Add($"Mapping {i}: Invalid index (0)");
|
||||
|
||||
if (mapping.BitLength == 0)
|
||||
errors.Add($"Mapping {i}: Invalid bit length (0)");
|
||||
}
|
||||
|
||||
return new ValidationResult(errors.Count == 0, errors);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// PDO Data (received or to-send)
|
||||
/// </summary>
|
||||
public readonly struct PdoData(byte pdoNumber, uint cobId, byte[] data, DateTime timestamp)
|
||||
{
|
||||
public byte PdoNumber { get; init; } = pdoNumber;
|
||||
public uint CobId { get; init; } = cobId;
|
||||
public byte[] Data { get; init; } = data;
|
||||
public DateTime Timestamp { get; init; } = timestamp;
|
||||
|
||||
/// <summary>
|
||||
/// Extract một value từ PDO data theo mapping
|
||||
/// </summary>
|
||||
public T ExtractValue<T>(PdoMapping mapping) where T : struct
|
||||
{
|
||||
if (mapping.BitLength == 0 || mapping.BitLength > 64)
|
||||
throw new ArgumentException("BitLength must be 1-64");
|
||||
|
||||
int bitOffset = 0;
|
||||
foreach (var _ in Enumerable.Range(0, mapping.SubIndex))
|
||||
{
|
||||
// Tính bit offset dựa trên các mappings trước đó
|
||||
// (simplified - trong thực tế cần biết tất cả mappings)
|
||||
}
|
||||
|
||||
return ExtractValue<T>(bitOffset, mapping.BitLength);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Extract value tại bit offset cụ thể
|
||||
/// </summary>
|
||||
public T ExtractValue<T>(int bitOffset, int bitLength) where T : struct
|
||||
{
|
||||
ulong value = 0;
|
||||
int byteOffset = bitOffset / 8;
|
||||
int bitInByteOffset = bitOffset % 8;
|
||||
int bitsRead = 0;
|
||||
|
||||
while (bitsRead < bitLength && byteOffset < Data.Length)
|
||||
{
|
||||
int bitsToRead = Math.Min(8 - bitInByteOffset, bitLength - bitsRead);
|
||||
byte mask = (byte)((1 << bitsToRead) - 1);
|
||||
byte byteValue = (byte)((Data[byteOffset] >> bitInByteOffset) & mask);
|
||||
|
||||
value |= (ulong)byteValue << bitsRead;
|
||||
|
||||
bitsRead += bitsToRead;
|
||||
byteOffset++;
|
||||
bitInByteOffset = 0;
|
||||
}
|
||||
|
||||
// Convert to target type with sign extension if needed
|
||||
return ConvertValue<T>(value, bitLength);
|
||||
}
|
||||
|
||||
private static T ConvertValue<T>(ulong value, int bitLength) where T : struct
|
||||
{
|
||||
var type = typeof(T);
|
||||
|
||||
if (type == typeof(bool))
|
||||
return (T)(object)(value != 0);
|
||||
|
||||
if (type == typeof(byte))
|
||||
return (T)(object)(byte)value;
|
||||
|
||||
if (type == typeof(sbyte))
|
||||
{
|
||||
// Sign extension
|
||||
if (bitLength < 8 && (value & (1UL << (bitLength - 1))) != 0)
|
||||
value |= (ulong)((1L << bitLength) - 1) << bitLength;
|
||||
return (T)(object)(sbyte)value;
|
||||
}
|
||||
|
||||
if (type == typeof(ushort))
|
||||
return (T)(object)(ushort)value;
|
||||
|
||||
if (type == typeof(short))
|
||||
{
|
||||
if (bitLength < 16 && (value & (1UL << (bitLength - 1))) != 0)
|
||||
value |= (ulong)((1L << bitLength) - 1) << bitLength;
|
||||
return (T)(object)(short)value;
|
||||
}
|
||||
|
||||
if (type == typeof(uint))
|
||||
return (T)(object)(uint)value;
|
||||
|
||||
if (type == typeof(int))
|
||||
{
|
||||
if (bitLength < 32 && (value & (1UL << (bitLength - 1))) != 0)
|
||||
value |= (ulong)((1L << bitLength) - 1) << bitLength;
|
||||
return (T)(object)(int)value;
|
||||
}
|
||||
|
||||
if (type == typeof(ulong))
|
||||
return (T)(object)value;
|
||||
|
||||
if (type == typeof(long))
|
||||
{
|
||||
if (bitLength < 64 && (value & (1UL << (bitLength - 1))) != 0)
|
||||
value |= ulong.MaxValue << bitLength;
|
||||
return (T)(object)(long)value;
|
||||
}
|
||||
|
||||
throw new NotSupportedException($"Type {type.Name} is not supported");
|
||||
}
|
||||
}
|
||||
@@ -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);
|
||||
}
|
||||
@@ -0,0 +1,30 @@
|
||||
namespace RobotNet10.CANOpen.Models;
|
||||
|
||||
/// <summary>
|
||||
/// Validation result container
|
||||
/// </summary>
|
||||
public class ValidationResult
|
||||
{
|
||||
public bool IsValid { get; }
|
||||
public IReadOnlyList<string> Errors { get; }
|
||||
|
||||
public ValidationResult(bool isValid, IEnumerable<string> errors)
|
||||
{
|
||||
IsValid = isValid;
|
||||
Errors = errors.ToList().AsReadOnly();
|
||||
}
|
||||
|
||||
public static ValidationResult Success() => new(true, Array.Empty<string>());
|
||||
|
||||
public static ValidationResult Failure(params string[] errors) => new(false, errors);
|
||||
|
||||
public static ValidationResult Failure(IEnumerable<string> errors) => new(false, errors);
|
||||
|
||||
public override string ToString()
|
||||
{
|
||||
if (IsValid)
|
||||
return "Valid";
|
||||
|
||||
return $"Invalid: {string.Join(", ", Errors)}";
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user