QUYVN/BQP
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-13 09:25:40 +07:00
parent c08ff54676
commit bccfb156d7
1938 changed files with 641646 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

211
srcs/RobotNet10/RobotApp/Communication/Sick.ColaB/ColaBHelper.cs Normal file
View File

@@ -0,0 +1,211 @@
using System.Text;
namespace Sick.ColaB;
/// <summary>
/// Helper functions for ColaB protocol encoding/decoding
/// ColaB is a binary protocol used by older SICK scanners like TiM781s
/// </summary>
public static class ColaBHelper
{
/// <summary>
/// ColaB frame header: 0x02 0x02 0x02 0x02
/// </summary>
private static readonly byte[] FrameHeader = [0x02, 0x02, 0x02, 0x02];
/// <summary>
/// Creates a ColaB frame from command data
/// Format: [STX STX STX STX] [Length (4 bytes BE)] ['s'] [Command Data] [Checksum (1 byte)]
/// </summary>
public static byte[] CreateFrame(ReadOnlySpan<byte> commandData)
{
// Calculate total payload length: length field (4) + 's' (1) + command data + checksum (1)
// But length field itself contains: 's' (1) + command data length
var payloadLength = 1 + commandData.Length; // 's' + command data
// Total frame size: header (4) + length (4) + payload + checksum (1)
var totalFrameSize = 4 + 4 + payloadLength + 1;
var frame = new byte[totalFrameSize];
var pos = 0;
// Write header (STX STX STX STX)
FrameHeader.CopyTo(frame, pos);
pos += 4;
// Write length (4 bytes, Big Endian) - this is the length of payload ('s' + command data)
WriteUint32BigEndian(frame, pos, (uint)payloadLength);
pos += 4;
// Write 's' character
frame[pos++] = (byte)'s';
// Write command data
commandData.CopyTo(frame.AsSpan(pos));
pos += commandData.Length;
// Calculate checksum (XOR of all bytes from 's' character to end of command data)
// According to C++ code: checksum starts from byte 8 (the 's' character)
byte checksum = frame[8]; // Start with 's' character (byte 8)
for (int i = 9; i < pos; i++)
{
checksum ^= frame[i];
}
// Write checksum
frame[pos++] = checksum;
return frame;
}
/// <summary>
/// Parses a ColaB frame and extracts the command data
/// </summary>
public static bool TryParseFrame(ReadOnlySpan<byte> frame, out ReadOnlySpan<byte> commandData, out int consumedBytes)
{
commandData = default;
consumedBytes = 0;
if (frame.Length < 9) // Minimum: header (4) + length (4) + 's' (1)
return false;
// Check header
if (frame[0] != 0x02 || frame[1] != 0x02 || frame[2] != 0x02 || frame[3] != 0x02)
return false;
// Read length (Big Endian)
var payloadLength = ReadUint32BigEndian(frame, 4);
var payloadLengthInt = (int)payloadLength;
// Check if we have enough data
var totalFrameSize = 4 + 4 + payloadLengthInt + 1; // header + length + payload + checksum
if (frame.Length < totalFrameSize)
return false;
// Check 's' character
if (frame[8] != (byte)'s')
return false;
// Verify checksum (XOR of all bytes from 's' character to end of command data)
// According to C++ code: checksum starts from byte 8 (the 's' character)
byte checksum = frame[8]; // Start with 's' character (byte 8)
for (int i = 9; i < 8 + payloadLengthInt; i++)
{
checksum ^= frame[i];
}
if (checksum != frame[8 + payloadLengthInt])
return false; // Checksum mismatch
// Extract command data (skip 's' character)
commandData = frame.Slice(9, payloadLengthInt - 1);
consumedBytes = totalFrameSize;
return true;
}
/// <summary>
/// Encodes a SOPAS command string to ColaB binary format
/// Example: "sRN DeviceIdent" -> binary command data
/// </summary>
public static byte[] EncodeCommand(string command)
{
return Encoding.ASCII.GetBytes(command);
}
/// <summary>
/// Decodes ColaB binary command data to SOPAS command string
/// </summary>
public static string DecodeCommand(ReadOnlySpan<byte> data)
{
return Encoding.ASCII.GetString(data);
}
/// <summary>
/// Writes a 32-bit unsigned integer in Big Endian format
/// </summary>
private static void WriteUint32BigEndian(byte[] buffer, int offset, uint value)
{
if (offset + 3 < buffer.Length)
{
buffer[offset + 0] = (byte)((value & 0xff000000) >> 24);
buffer[offset + 1] = (byte)((value & 0xff0000) >> 16);
buffer[offset + 2] = (byte)((value & 0xff00) >> 8);
buffer[offset + 3] = (byte)(value & 0xff);
}
}
/// <summary>
/// Reads a 32-bit unsigned integer in Big Endian format
/// </summary>
private static uint ReadUint32BigEndian(ReadOnlySpan<byte> buffer, int offset)
{
if (offset + 3 < buffer.Length)
{
return ((uint)buffer[offset + 0] << 24) +
((uint)buffer[offset + 1] << 16) +
((uint)buffer[offset + 2] << 8) +
buffer[offset + 3];
}
return 0;
}
/// <summary>
/// Writes an integer value to buffer in Big Endian format
/// </summary>
public static void WriteIntegerBigEndian(byte[] buffer, ref int pos, uint value, int byteWidth)
{
for (int i = 0; i < byteWidth; i++)
{
if (pos < buffer.Length)
{
buffer[pos + byteWidth - 1 - i] = (byte)((value >> (8 * i)) & 0xff);
}
}
pos += byteWidth;
}
/// <summary>
/// Reads an integer value from buffer in Big Endian format
/// </summary>
public static uint ReadIntegerBigEndian(ReadOnlySpan<byte> buffer, ref int pos, int byteWidth)
{
uint value = 0;
for (int i = 0; i < byteWidth; i++)
{
if (pos + byteWidth - 1 - i < buffer.Length)
{
value += (uint)buffer[pos + byteWidth - 1 - i] << (8 * i);
}
}
pos += byteWidth;
return value;
}
/// <summary>
/// Writes a string to buffer
/// </summary>
public static void WriteString(byte[] buffer, ref int pos, string value)
{
var bytes = Encoding.ASCII.GetBytes(value);
if (pos + bytes.Length <= buffer.Length)
{
bytes.CopyTo(buffer, pos);
pos += bytes.Length;
}
}
/// <summary>
/// Reads a string from buffer
/// </summary>
public static string ReadString(ReadOnlySpan<byte> buffer, ref int pos, int length)
{
if (pos + length <= buffer.Length)
{
var result = Encoding.ASCII.GetString(buffer.Slice(pos, length));
pos += length;
return result;
}
return string.Empty;
}
}