*

2025-05-13 03:26:56 +03:00
parent 87a14db6f4
commit 154af64dde
563 changed files with 797817 additions and 0 deletions
@@ -0,0 +1,139 @@
+
+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');
+};