private PositionControllerResponse DecodeResponse(byte[] buffer)
{
using (Stream s = new MemoryStream(buffer))
{
using (BinaryReader br = new BinaryReader(s))
{
ushort header = br.ReadUInt16();
if (header != DATA_HEADER)
{
return(new PositionControllerResponse(PositionControllerStatus.CS_ERR_COMMUNICATION));
}
ushort status = br.ReadUInt16();
uint position = br.ReadUInt32();
uint crc = br.ReadUInt32();
s.Seek(0, SeekOrigin.Begin);
Stm32Crc32 crc32 = new Stm32Crc32(CRC32_INITIAL_VALUE, CRC32_POLYNOMIAL);
crc32.Encode(br.ReadUInt32());
crc32.Encode(br.ReadUInt32());
if (crc32.GetValue() != crc)
{
return(new PositionControllerResponse(PositionControllerStatus.CS_ERR_CRC));
}
return(new PositionControllerResponse((PositionControllerStatus)status, position));
}
}
}
private static IBuffer CreateRequest(PositionControllerCommand command, PositionControllerDirection direction, uint steps, byte[] buffer)
{
if (buffer == null || buffer.Length < REQUEST_LENGTH)
{
throw new ArgumentException($"'{nameof(buffer)}' parameter is null or too small");
}
using (Stream s = new MemoryStream(buffer))
{
using (BinaryWriter bw = new BinaryWriter(s))
{
bw.Write((ushort)DATA_HEADER);
bw.Write((byte)command);
bw.Write((byte)direction);
bw.Write((uint)steps);
}
}
Stm32Crc32 crc32 = new Stm32Crc32(CRC32_INITIAL_VALUE, CRC32_POLYNOMIAL);
using (Stream s = new MemoryStream(buffer))
{
using (BinaryReader br = new BinaryReader(s))
{
crc32.Encode(br.ReadUInt32());
crc32.Encode(br.ReadUInt32());
}
}
using (Stream s = new MemoryStream(buffer, 8, 4))
{
using (BinaryWriter bw = new BinaryWriter(s))
{
bw.Write((uint)crc32.GetValue());
}
}
return(buffer.AsBuffer());
}
