123 lines
5.1 KiB
C#
123 lines
5.1 KiB
C#
using Sick.SafetyScanners.DataStructures;
|
|
using Sick.SafetyScanners.Helpers;
|
|
using System.Net;
|
|
|
|
namespace Sick.SafetyScanners.Cola2.Commands;
|
|
|
|
/// <summary>
|
|
/// Command to change communication settings on the sensor
|
|
/// </summary>
|
|
public sealed class ChangeCommSettingsCommand : MethodCommand
|
|
{
|
|
private readonly CommSettings _settings;
|
|
|
|
public ChangeCommSettingsCommand(CommSettings settings)
|
|
: base(0x00b0) // Method index for ChangeCommSettings
|
|
{
|
|
_settings = settings ?? throw new ArgumentNullException(nameof(settings));
|
|
}
|
|
|
|
public CommSettings Settings => _settings;
|
|
|
|
public override bool CanBeExecutedWithoutSessionId => true;
|
|
|
|
protected override ReadOnlyMemory<byte> AddTelegramData()
|
|
{
|
|
// Base method index (2 bytes) + 28 bytes for settings data
|
|
var data = new byte[2 + 28];
|
|
var span = data.AsSpan();
|
|
|
|
// Write base method index (from MethodCommand.AddTelegramData)
|
|
ReadWriteHelper.WriteUint16LittleEndian(span, 0, MethodIndex);
|
|
|
|
// Write settings data starting at offset 2 (after method index)
|
|
WriteDataToSpan(span.Slice(2));
|
|
|
|
return data;
|
|
}
|
|
|
|
private void WriteDataToSpan(Span<byte> span)
|
|
{
|
|
// Channel (offset 0)
|
|
ReadWriteHelper.WriteUint8LittleEndian(span, 0, _settings.Channel);
|
|
|
|
// Skip 3 bytes (offsets 1, 2, 3)
|
|
|
|
// Enabled (offset 4)
|
|
ReadWriteHelper.WriteUint8LittleEndian(span, 4, (byte)(_settings.Enabled ? 1 : 0));
|
|
|
|
// Interface type (offset 5)
|
|
ReadWriteHelper.WriteUint8LittleEndian(span, 5, (byte)_settings.EInterfaceType);
|
|
|
|
// Skip 2 bytes (offsets 6, 7)
|
|
|
|
// Host IP (offset 8, 4 bytes, little endian)
|
|
if (!IPAddress.TryParse(_settings.HostIp, out var ipAddress) ||
|
|
ipAddress.AddressFamily != System.Net.Sockets.AddressFamily.InterNetwork)
|
|
{
|
|
throw new ArgumentException($"Invalid IPv4 address: {_settings.HostIp}", nameof(_settings));
|
|
}
|
|
|
|
var bytes = ipAddress.GetAddressBytes();
|
|
if (bytes.Length != 4)
|
|
{
|
|
throw new InvalidOperationException($"IPAddress.GetAddressBytes() returned {bytes.Length} bytes, expected 4");
|
|
}
|
|
|
|
// Convert to uint32 (little endian)
|
|
// IPAddress.GetAddressBytes() returns bytes in network byte order (big endian): [b0, b1, b2, b3]
|
|
// For little endian uint32, we need: bytes[0] | (bytes[1] << 8) | (bytes[2] << 16) | (bytes[3] << 24)
|
|
// uint ipUint = (uint)(bytes[0] | (bytes[1] << 8) | (bytes[2] << 16) | (bytes[3] << 24));
|
|
uint ipUint = (uint)(bytes[3] | (bytes[2] << 8) | (bytes[1] << 16) | (bytes[0] << 24));
|
|
ReadWriteHelper.WriteUint32LittleEndian(span, 8, ipUint);
|
|
|
|
// Host UDP port (offset 12, 2 bytes, little endian)
|
|
ReadWriteHelper.WriteUint16LittleEndian(span, 12, _settings.HostUdpPort);
|
|
|
|
// Publishing frequency (offset 14, 2 bytes, little endian)
|
|
ReadWriteHelper.WriteUint16LittleEndian(span, 14, _settings.PublishingFrequency);
|
|
|
|
// Start angle (offset 16, 4 bytes, little endian, multiplied by 4194304.0)
|
|
int startAngleInt = (int)(_settings.StartAngle * 4194304.0);
|
|
ReadWriteHelper.WriteInt32LittleEndian(span, 16, startAngleInt);
|
|
|
|
// End angle (offset 20, 4 bytes, little endian, multiplied by 4194304.0)
|
|
int endAngleInt = (int)(_settings.EndAngle * 4194304.0);
|
|
ReadWriteHelper.WriteInt32LittleEndian(span, 20, endAngleInt);
|
|
|
|
// Features (offset 24, 2 bytes, little endian)
|
|
ReadWriteHelper.WriteUint16LittleEndian(span, 24, _settings.Features);
|
|
}
|
|
|
|
protected override bool ProcessReplyInternal(ReadOnlyMemory<byte> replyData,
|
|
byte replyCommandType, byte replyCommandMode)
|
|
{
|
|
// According to C++ reference, this inverts the result from base class
|
|
// Base class returns true for 'A' + 'I' (Acknowledge)
|
|
// But ChangeCommSettingsCommand expects different reply format
|
|
// Let's check for error response: if we get 'E' + 'I' (Error), return false
|
|
// Otherwise, check base class logic
|
|
|
|
// Error response: 'E' (0x45) and 'I' (0x49)
|
|
if ((replyCommandType == 0x45 && replyCommandMode == 0x49) ||
|
|
(replyCommandType == 'E' && replyCommandMode == 'I'))
|
|
{
|
|
return false;
|
|
}
|
|
|
|
// Success: 'A' (0x41) and 'I' (0x49)
|
|
if ((replyCommandType == 0x41 && replyCommandMode == 0x49) ||
|
|
(replyCommandType == 'A' && replyCommandMode == 'I'))
|
|
{
|
|
return true;
|
|
}
|
|
|
|
// For ChangeCommSettings, the C++ code inverts the base result
|
|
// which suggests it might have different error handling
|
|
// Let's use standard acknowledge check
|
|
return (replyCommandType == 0x41 && replyCommandMode == 0x49) ||
|
|
(replyCommandType == 'A' && replyCommandMode == 'I');
|
|
}
|
|
}
|
|
|