Simulator/DroneClient/Drone.cs

using System.Drawing;
using System.Runtime.InteropServices;

namespace DroneClient
{
  internal class Drone
  {

    public float AccX, AccY, AccZ;
    public float GyrX, GyrY, GyrZ;
    public float PosX, PosY;
    public float LaserRange;

    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[]? RecvDataIMU(byte[] data)
    {
      DroneData.DataIMU imu = (DroneData.DataIMU)fromBytes(data, typeof(DroneData.DataIMU));

      AccX = imu.Acc.X; AccY = imu.Acc.Y; AccZ = imu.Acc.Z;
      GyrX = imu.Gyr.X; GyrY = imu.Gyr.Y; GyrZ = imu.Gyr.Z;

      return new byte[0];
    }

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

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

      return new byte[0];
    }

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

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

        case DroneData.DataType.DataPos:
          {
            if (head.Mode == DroneData.DataMode.Request)
            {
              // Запрос данных
              // ... //
              //
              return zero;
            }
            else
            {
              // Пришли данные
              return RecvDataPos(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 imu = new DroneData.DataHead();
      imu.Size = DroneData.DataHead.StrLen;
      imu.Mode = DroneData.DataMode.Request;
      imu.Type = DroneData.DataType.DataIMU;

      DroneData.DataHead pos = new DroneData.DataHead();
      pos.Size = DroneData.DataHead.StrLen;
      pos.Mode = DroneData.DataMode.Request;
      pos.Type = DroneData.DataType.DataPos;

      byte[] si = getBytes(imu);
      byte[] sp = getBytes(pos);
      byte[] sm = SendDataMotor4();

      byte[] send = new byte[si.Length + sp.Length + sm.Length];
      si.CopyTo(send, 0);
      sp.CopyTo(send, si.Length);
      sm.CopyTo(send, si.Length + sp.Length);

      return send;
    }
  }
}