Initial commit
This commit is contained in:
@@ -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");
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user