Simulator/DroneClient/Drone.cs

using System.Diagnostics.Metrics;
using System.Drawing;
using System.Runtime.InteropServices;

namespace DroneClient
{
  internal class Drone
  {

    public float AccX, AccY, AccZ;
    public float GyrX, GyrY, GyrZ;
    public uint TimeAcc, TimeGyr;

    public float PosX, PosY;
    public float LaserRange;
    public uint TimeRange;

    public float MotorUL, MotorUR, MotorDL, MotorDR;

    public static byte[] getBytes(object data)
    {
      int size = Marshal.SizeOf(data);
      byte[] arr = new byte[size];

      IntPtr ptr = IntPtr.Zero;
      try
      {
        ptr = Marshal.AllocHGlobal(size);
        Marshal.StructureToPtr(data, ptr, true);
        Marshal.Copy(ptr, arr, 0, size);
      }
      finally
      {
        Marshal.FreeHGlobal(ptr);
      }
      return arr;
    }

    public static object fromBytes(byte[] arr, Type type)
    {
      object mem = new object();

      int size = Marshal.SizeOf(type);
      IntPtr ptr = IntPtr.Zero;
      try
      {
        ptr = Marshal.AllocHGlobal(size);

        Marshal.Copy(arr, 0, ptr, size);

        mem = Marshal.PtrToStructure(ptr, type);
      }
      finally
      {
        Marshal.FreeHGlobal(ptr);
      }

      return mem;
    }

    private byte[] SendDataMotor4()
    {
      DroneData.DataMotor4 mot4 = new DroneData.DataMotor4();

      mot4.Head.Size = Marshal.SizeOf(typeof(DroneData.DataMotor4));
      mot4.Head.Mode = DroneData.DataMode.Response;
      mot4.Head.Type = DroneData.DataType.DataMotor4;

      mot4.UL = MotorUL;
      mot4.UR = MotorUR;
      mot4.DL = MotorDL;
      mot4.DR = MotorDR;

      return getBytes(mot4);
    }

    private byte[]? RecvDataAcc(byte[] data)
    {
      DroneData.DataAcc imu = (DroneData.DataAcc)fromBytes(data, typeof(DroneData.DataAcc));

      AccX = imu.Acc.X; AccY = imu.Acc.Y; AccZ = imu.Acc.Z;

      TimeAcc= imu.Time;

      return new byte[0];
    }

    private byte[]? RecvDataGyr(byte[] data)
    {
      DroneData.DataGyr imu = (DroneData.DataGyr)fromBytes(data, typeof(DroneData.DataGyr));

      GyrX = imu.Gyr.X; GyrY = imu.Gyr.Y; GyrZ = imu.Gyr.Z;

      TimeGyr = imu.Time;

      return new byte[0];
    }

    private byte[]? RecvDataRange(byte[] data)
    {
      DroneData.DataRange pos = (DroneData.DataRange)fromBytes(data, typeof(DroneData.DataRange));

      LaserRange = pos.LiDAR;

      TimeRange = pos.Time;

      return new byte[0];
    }

    private byte[]? RecvDataLocal(byte[] data)
    {
      DroneData.DataLocal pos = (DroneData.DataLocal)fromBytes(data, typeof(DroneData.DataLocal));

      PosX = pos.Local.X; PosY = pos.Local.Y;

      return new byte[0];
    }

    private byte[]? ClientRequestResponse(DroneData.DataHead head, byte[] body)
    {
      byte[] zero = new byte[0];

      switch (head.Type)
      {
        case DroneData.DataType.DataAcc:
          {
            if (head.Mode == DroneData.DataMode.Request)
            {
              // Запрос данных
              // ... //
              //
              return zero;
            }
            else
            {
              // Пришли данные
              return RecvDataAcc(body);
            }
          }

        case DroneData.DataType.DataGyr:
          {
            if (head.Mode == DroneData.DataMode.Request)
            {
              // Запрос данных
              // ... //
              //
              return zero;
            }
            else
            {
              // Пришли данные
              return RecvDataGyr(body);
            }
          }

        case DroneData.DataType.DataRange:
          {
            if (head.Mode == DroneData.DataMode.Request)
            {
              // Запрос данных
              // ... //
              //
              return zero;
            }
            else
            {
              // Пришли данные
              return RecvDataRange(body);
            }
          }

        case DroneData.DataType.DataLocal:
          {
            if (head.Mode == DroneData.DataMode.Request)
            {
              // Запрос данных
              // ... //
              //
              return zero;
            }
            else
            {
              // Пришли данные
              return RecvDataLocal(body);
            }
          }

        case DroneData.DataType.DataMotor4:
          {
            if (head.Mode == DroneData.DataMode.Request)
            {
              // Запрос данных
              return SendDataMotor4();
            }
            else
            {
              // Пришли данные
              // ... //
              //
              return zero;
            }
          }
      }

      return zero;
    }

    private const int DroneStreamCount = 512;
    private byte[] DroneStreamData = new byte[DroneStreamCount];
    private int DroneStreamIndex = 0;
    private DroneData.DataHead DroneStreamHead = new DroneData.DataHead() { Mode = DroneData.DataMode.None, Size = 0, Type = DroneData.DataType.None };

    public List<byte[]?>? DataStream(byte[]? data, int size)
    {
      List<byte[]?> ret = new List<byte[]?>();

      if (data == null) return ret; // Последовательность не сформирована, ждать данных

      if (size + DroneStreamIndex > DroneStreamCount) return null; // Ошибка переполнения, поток сломан (конец)

      Array.Copy(data, 0, DroneStreamData, DroneStreamIndex, size);
      DroneStreamIndex += size;

      while (true)
      {
        if (DroneStreamHead.Size == 0) // Заголовок ещё не получен
        {
          if (DroneStreamIndex < DroneData.DataHead.StrLen) return ret; // Нечего слать
          DroneStreamHead = (DroneData.DataHead)fromBytes(DroneStreamData, typeof(DroneData.DataHead));
        }

        if (DroneStreamHead.Size > DroneStreamIndex) break; // Пакет ещё не полный

        byte[] body = new byte[DroneStreamHead.Size];

        Array.Copy(DroneStreamData, 0, body, 0, DroneStreamHead.Size);

        int shift = DroneStreamHead.Size;

        DroneStreamIndex -= shift;
        Array.Copy(DroneStreamData, shift, DroneStreamData, 0, DroneStreamIndex); // Сдвигаем массив влево, убрав использованные данные

        DroneStreamHead.Size = 0; // Отменяем прошлый заголовок

        ret.Add(ClientRequestResponse(DroneStreamHead, body));
      }

      return ret;
    }

    public byte[] SendReqest()
    {
      DroneData.DataHead acc = new DroneData.DataHead();
      acc.Size = DroneData.DataHead.StrLen;
      acc.Mode = DroneData.DataMode.Request;
      acc.Type = DroneData.DataType.DataAcc;

      DroneData.DataHead gyr = new DroneData.DataHead();
      gyr.Size = DroneData.DataHead.StrLen;
      gyr.Mode = DroneData.DataMode.Request;
      gyr.Type = DroneData.DataType.DataGyr;

      DroneData.DataHead range = new DroneData.DataHead();
      range.Size = DroneData.DataHead.StrLen;
      range.Mode = DroneData.DataMode.Request;
      range.Type = DroneData.DataType.DataRange;

      DroneData.DataHead local = new DroneData.DataHead();
      local.Size = DroneData.DataHead.StrLen;
      local.Mode = DroneData.DataMode.Request;
      local.Type = DroneData.DataType.DataLocal;

      List<byte[]> list = new List<byte[]>();

      list.Add(getBytes(acc));
      list.Add(getBytes(gyr));
      list.Add(getBytes(range));
      list.Add(getBytes(local));
      list.Add(SendDataMotor4());

      int count = 0;

      foreach (byte[] d in list) count += d.Length;

      byte[] send = new byte[count];

      count = 0;
      foreach (byte[] d in list)
      {
        d.CopyTo(send, count);
        count += d.Length;
      }

      return send;
    }
  }
}