139 lines
4.3 KiB
C#
139 lines
4.3 KiB
C#
|
|
using System.Collections;
|
|
using System.Runtime.InteropServices;
|
|
using System;
|
|
using System.IO.Ports;
|
|
|
|
[StructLayout(LayoutKind.Sequential, Pack = 1)]
|
|
public struct UartCommandHeader {
|
|
public uint id;
|
|
public byte payloadSize;
|
|
}
|
|
|
|
[StructLayout(LayoutKind.Sequential, Pack = 1)]
|
|
public struct UartCommand {
|
|
public UartCommandHeader header;
|
|
public const int MaxPayloadSize = 32;
|
|
[MarshalAs(UnmanagedType.ByValArray, SizeConst = MaxPayloadSize)]
|
|
public byte[] payload;
|
|
|
|
public UartCommand(uint _id, byte[] payloadBytes = null) {
|
|
byte _payloadSize = 0;
|
|
if (payloadBytes != null) {
|
|
if (payloadBytes.Length > MaxPayloadSize) {
|
|
throw new Exception("Too big payload!");
|
|
}
|
|
_payloadSize = (byte)payloadBytes.Length;
|
|
}
|
|
header = new UartCommandHeader() { id = _id, payloadSize = _payloadSize };
|
|
payload = new byte[MaxPayloadSize];
|
|
|
|
if (_payloadSize > 0) {
|
|
Array.Copy(payloadBytes, payload, payloadBytes.Length);
|
|
}
|
|
}
|
|
|
|
public UartCommand Write<T>(T val) where T : struct {
|
|
var bytes = Utils.StructToBytes(val);
|
|
for (int i = 0; i < bytes.Length; ++i) {
|
|
payload[header.payloadSize++] = bytes[i];
|
|
}
|
|
return this;
|
|
}
|
|
|
|
public static UartCommand Create() {
|
|
var result = new UartCommand();
|
|
result.payload = new byte[MaxPayloadSize];
|
|
return result;
|
|
}
|
|
|
|
public static UartCommand Create(uint id) {
|
|
var result = Create();
|
|
result.header.id = id;
|
|
return result;
|
|
}
|
|
|
|
public UartCommand SetID(uint _id) {
|
|
header.id = _id;
|
|
return this;
|
|
}
|
|
|
|
public static int Size {
|
|
get { return Marshal.SizeOf(typeof(UartCommandHeader)) + MaxPayloadSize; }
|
|
}
|
|
|
|
public UartCommandReader CreateReader() {
|
|
return new UartCommandReader(ref this);
|
|
}
|
|
}
|
|
|
|
public class UartCommandReader {
|
|
private int offset = 0;
|
|
private byte[] buffer;
|
|
public UartCommandReader(ref UartCommand cmd) {
|
|
buffer = cmd.payload;
|
|
}
|
|
|
|
public T Read<T>() where T : struct {
|
|
var result = Utils.StructFromBytes<T>(buffer, offset);
|
|
offset += Marshal.SizeOf(typeof(T));
|
|
return result;
|
|
}
|
|
}
|
|
|
|
|
|
public static class Utils {
|
|
public static T StructFromBytes<T>(byte[] buffer, int offset = 0) where T : struct {
|
|
var handle = GCHandle.Alloc(buffer, GCHandleType.Pinned);
|
|
var result = (T)Marshal.PtrToStructure(new IntPtr(handle.AddrOfPinnedObject().ToInt64() + offset), typeof(T));
|
|
handle.Free();
|
|
return result;
|
|
}
|
|
|
|
public static byte[] StructToBytes<T>(T val) where T : struct {
|
|
var result = new byte[Marshal.SizeOf(typeof(T))];
|
|
var handle = GCHandle.Alloc(result, GCHandleType.Pinned);
|
|
Marshal.StructureToPtr(val, handle.AddrOfPinnedObject(), false);
|
|
handle.Free();
|
|
return result;
|
|
}
|
|
}
|
|
|
|
public static class SerialPortEx {
|
|
public static UartCommand ExecuteCommand(this SerialPort _this, UartCommand cmd) {
|
|
SendCommand(_this, cmd);
|
|
return ReceiveCommand(_this);
|
|
}
|
|
|
|
private static void SendCommand(SerialPort _this, UartCommand cmd) {
|
|
var bytes = Utils.StructToBytes(cmd);
|
|
_this.Write(bytes, 0, bytes.Length);
|
|
_this.BaseStream.Flush();
|
|
}
|
|
|
|
private static UartCommand ReceiveCommand(SerialPort _this) {
|
|
var buffer = new byte[UartCommand.Size];
|
|
int done = 0;
|
|
while (done != buffer.Length) {
|
|
var len = _this.Read(buffer, done, buffer.Length - done);
|
|
if (len <= 0) {
|
|
throw new Exception("Failed to read command");
|
|
}
|
|
done += len;
|
|
}
|
|
_this.BaseStream.Flush();
|
|
return Utils.StructFromBytes<UartCommand>(buffer);
|
|
}
|
|
}
|
|
|
|
public struct uart_commands {
|
|
public static uint FOURCC(char a, char b, char c, char d) {
|
|
return ((uint)((((uint)d) << 24) | (((uint)c) << 16) | (((uint)b) << 8) | ((uint)a)));
|
|
}
|
|
public static uint led_write = FOURCC('L', 'e', 'd', 'W');
|
|
public static uint led_read = FOURCC('L', 'e', 'd', 'R');
|
|
public static uint motor_write = FOURCC('M', 't', 'r', 'W');
|
|
public static uint play_sound = FOURCC('P', 'S', 'n', 'd');
|
|
public static uint acc_read = FOURCC('A', 'c', 'c', 'R');
|
|
public static uint gyro_read = FOURCC('G', 'r', 'o', 'R');
|
|
}; |