715 lines
21 KiB
C#
715 lines
21 KiB
C#
using System.Net.Sockets;
|
||
using System.Diagnostics;
|
||
using System.Numerics;
|
||
using System.Runtime.InteropServices;
|
||
|
||
namespace DroneSimulator
|
||
{
|
||
internal class Drone
|
||
{
|
||
public int ID;
|
||
|
||
private Socket? Client = null;
|
||
|
||
public bool Active = false; // Живой?
|
||
public const float Dynamic = 10; // Динамика вращения
|
||
public Vector3 PosXYZ = Vector3.Zero, SpdXYZ = Vector3.Zero, AccXYZ = Vector3.Zero; // Положение в пространстве: Позиция, Скорость, Ускорение
|
||
public Quaternion Quat = Quaternion.Identity; // Основной кватернион
|
||
public float Power = 0; // Тяга всех двигателей (0-1)
|
||
|
||
public Vector3 SpdPRY, AccPRY; // Поворот в пространстве: pitch roll yaw
|
||
|
||
public Vector3 Acc, Gyr; // Имитация: Акселерометр, Гироскоп
|
||
public float LaserRange; // Имитация: Дальномер
|
||
|
||
public Vector4 Orientation;
|
||
|
||
private const float Gravity = 9.8f;
|
||
|
||
private const float TO_GRAD = 180 / MathF.PI;
|
||
private const float TO_RADI = MathF.PI / 180;
|
||
|
||
public static List<Drone> AllDrones = new List<Drone>();
|
||
private static Thread? DroneThread = null;
|
||
private uint StepTime = 0;
|
||
|
||
public static Semaphore StepSemaphore = new Semaphore(0, 10);
|
||
|
||
private uint Timer;
|
||
|
||
public static bool TimeLimit = false;
|
||
|
||
private Vector2 MoveOF = Vector2.Zero;
|
||
private uint CountOF = 0;
|
||
private bool ReadyOF = false;
|
||
|
||
public struct Physics
|
||
{
|
||
static public float Mass; // Масса
|
||
static public float Length; // Длинна лучей
|
||
static public float MaxPower; // Максимальная Тяга всех двигателей (КГ)
|
||
}
|
||
|
||
public struct Propeller
|
||
{
|
||
static public float Diameter; // Диаметр лопостей
|
||
static public float MaxRotate; // Максимальнные обороты в секунду
|
||
}
|
||
|
||
private RealMode.Accelerometer RealAcc = new RealMode.Accelerometer();
|
||
private RealMode.Gyroscope RealGyr = new RealMode.Gyroscope();
|
||
private RealMode.Position RealPos = new RealMode.Position();
|
||
private RealMode.Barometer RealBar = new RealMode.Barometer();
|
||
private RealMode.Range RealRange = new RealMode.Range();
|
||
private RealMode.OpticalFlow RealOF = new RealMode.OpticalFlow();
|
||
|
||
private uint TickCount = 0;
|
||
|
||
private uint Ticks()
|
||
{
|
||
uint tick = (uint)(Stopwatch.GetTimestamp() / Stopwatch.Frequency / 1000);
|
||
TickCount += tick;
|
||
return TickCount;
|
||
}
|
||
|
||
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;
|
||
}
|
||
|
||
public VisualData.VisualDrone GetVisual(int count, int index)
|
||
{
|
||
VisualData.VisualDrone drone = new VisualData.VisualDrone();
|
||
|
||
drone.Head.Size = Marshal.SizeOf(typeof(VisualData.VisualDrone));
|
||
drone.Head.Type = VisualData.VisualHead.VisualType.Drone;
|
||
|
||
drone.Count = count;
|
||
drone.Index = index;
|
||
|
||
drone.ID = ID;
|
||
drone.Color.A = 255; drone.Color.R = 255; drone.Color.G = 0; drone.Color.B = 0;
|
||
|
||
drone.Rotate.X = Quat.X; drone.Rotate.Y = Quat.Y; drone.Rotate.Z = Quat.Z; drone.Rotate.W = Quat.W;
|
||
drone.Position.X = PosXYZ.X; drone.Position.Y = PosXYZ.Y; drone.Position.Z = PosXYZ.Z;
|
||
drone.Scale = 1.0f;
|
||
|
||
drone.State = VisualData.VisualDrone.DroneState.Active;
|
||
|
||
drone.Power = Power;
|
||
|
||
return drone;
|
||
}
|
||
|
||
public static void StartThread()
|
||
{
|
||
if (DroneThread != null) return;
|
||
|
||
DroneThread = new Thread(new ThreadStart(Drone.ThreadFunction));
|
||
DroneThread.Priority = ThreadPriority.Highest;
|
||
DroneThread.Start();
|
||
}
|
||
|
||
public static void StopThread()
|
||
{
|
||
if (DroneThread == null) return;
|
||
|
||
DroneThread = null;
|
||
}
|
||
|
||
private static void ThreadFunction()
|
||
{
|
||
while (DroneThread != null)
|
||
{
|
||
if (StepSemaphore.WaitOne(1))
|
||
{
|
||
lock (AllDrones)
|
||
{
|
||
foreach (Drone drone in AllDrones)
|
||
{
|
||
if (drone.StepTime > 0)
|
||
{
|
||
uint prev = drone.Timer;
|
||
uint next = prev + drone.StepTime;
|
||
|
||
drone.Action(next);
|
||
drone.StepTime = 0;
|
||
drone.SendStep(next, prev);
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
public Drone(int id, Socket? client)
|
||
{
|
||
ID = id;
|
||
Timer = 0;
|
||
Client = client;
|
||
}
|
||
|
||
public int Create()
|
||
{
|
||
Active = true;
|
||
|
||
return ID;
|
||
}
|
||
|
||
private float GetAngle(float a1, float a2, float az)
|
||
{
|
||
if (a2 == 0.0f && az == 0.0f)
|
||
{
|
||
if (a1 > 0) return 90;
|
||
if (a1 < 0) return -90;
|
||
return 0;
|
||
}
|
||
return MathF.Atan(a1 / MathF.Sqrt(a2 * a2 + az * az)) * TO_GRAD;
|
||
}
|
||
|
||
public void Rotate(float x, float y, float z)
|
||
{
|
||
x = x * MathF.PI / 180;
|
||
y = y * MathF.PI / 180;
|
||
z = -z * MathF.PI / 180;
|
||
|
||
Quaternion map = Quat;
|
||
Quaternion spd = new Quaternion(x, y, z, 0);
|
||
Quaternion aq = spd * map;
|
||
|
||
map.W -= 0.5f * aq.W;
|
||
map.X -= 0.5f * aq.X;
|
||
map.Y -= 0.5f * aq.Y;
|
||
map.Z -= 0.5f * aq.Z;
|
||
|
||
Quat = Quaternion.Normalize(map);
|
||
}
|
||
|
||
public Vector4 GetOrientation()
|
||
{
|
||
Quaternion grav = new Quaternion(0, 0, 1, 0);
|
||
|
||
grav = Quat * grav * Quaternion.Inverse(Quat);
|
||
|
||
float yaw = 2 * MathF.Atan2(Quat.Z, Quat.W) * TO_GRAD;
|
||
if (yaw < 0.0f) yaw = 360.0f + yaw;
|
||
|
||
return new Vector4(GetAngle(grav.Y, grav.X, grav.Z), GetAngle(-grav.X, grav.Y, grav.Z), yaw, grav.Z);
|
||
}
|
||
|
||
public void Action(uint tick)
|
||
{
|
||
uint period = tick - Timer;
|
||
|
||
if (period == 0) return;
|
||
|
||
float time = period / 1000.0f;
|
||
Timer = tick;
|
||
|
||
if (!Active) return;
|
||
|
||
float flow = Power;
|
||
|
||
if (PosXYZ.Z < 0.3f)
|
||
{
|
||
flow += flow * 0.1f; // Воздушная подушка
|
||
}
|
||
|
||
float wind_x = 0, wind_y = 0, wind_z = 0;
|
||
float wind_p = 0, wind_r = 0, wind_w = 0;
|
||
|
||
if (Area.Wind.Enable)
|
||
{
|
||
Quaternion dir = Quaternion.CreateFromAxisAngle(new Vector3(0, 0, 1), Area.Wind.Direction * TO_RADI * 2);
|
||
|
||
Quaternion spd = new Quaternion(0, Area.Wind.Speed.From, 0, 0) * dir;
|
||
|
||
float spd_x = spd.X - SpdXYZ.X;
|
||
float spd_y = spd.Y - SpdXYZ.Y;
|
||
float spd_z = spd.Z - SpdXYZ.Z;
|
||
|
||
wind_x = 0.5f * Area.Wind.Density * Area.Wind.PosResist * (spd_x * MathF.Abs(spd_x));
|
||
wind_y = 0.5f * Area.Wind.Density * Area.Wind.PosResist * (spd_y * MathF.Abs(spd_y));
|
||
wind_z = 0.5f * Area.Wind.Density * Area.Wind.PosResist * (spd_z * MathF.Abs(spd_z));
|
||
|
||
wind_p = 0.5f * Area.Wind.Density * Area.Wind.RotResist * (SpdPRY.X * MathF.Abs(SpdPRY.X));
|
||
wind_r = 0.5f * Area.Wind.Density * Area.Wind.RotResist * (SpdPRY.Y * MathF.Abs(SpdPRY.Y));
|
||
wind_w = 0.5f * Area.Wind.Density * Area.Wind.RotResist * (SpdPRY.Z * MathF.Abs(SpdPRY.Z));
|
||
|
||
AccPRY.X -= wind_p; AccPRY.Y -= wind_r; AccPRY.Z -= wind_w;
|
||
}
|
||
|
||
SpdPRY += AccPRY * (Dynamic * time / (Physics.Mass * Physics.Length));
|
||
|
||
Quaternion pow = Quaternion.Inverse(Quat) * new Quaternion(0, 0, flow, 0) * Quat;
|
||
AccXYZ = new Vector3(pow.X + wind_x, pow.Y + wind_y, pow.Z + wind_z) * (Gravity / Physics.Mass);
|
||
SpdXYZ += (AccXYZ + new Vector3(0, 0, -Gravity)) * time;
|
||
PosXYZ += SpdXYZ * time;
|
||
|
||
AccXYZ /= Gravity; // Вернуть измерения в G
|
||
|
||
if (Area.Poisition.Freeze.X) { SpdXYZ.X = 0; PosXYZ.X = 0; }
|
||
if (Area.Poisition.Freeze.Y) { SpdXYZ.Y = 0; PosXYZ.Y = 0; }
|
||
if (Area.Poisition.Freeze.Z) { SpdXYZ.Z = 0; PosXYZ.Z = 5; }
|
||
|
||
/*if (PosXYZ.Z < 0)
|
||
{
|
||
SpdPRY = Vector3.Zero;
|
||
SpdXYZ.X = 0;
|
||
SpdXYZ.Y = 0;
|
||
Quat = Quaternion.Identity;
|
||
}
|
||
else */
|
||
Rotate(SpdPRY.X * time, SpdPRY.Y * time, SpdPRY.Z * time);
|
||
|
||
Vector4 ori = GetOrientation();
|
||
|
||
Orientation = ori;
|
||
float range = 0;
|
||
|
||
if (PosXYZ.Z <= 0)
|
||
{
|
||
PosXYZ.Z = 0;
|
||
SpdXYZ.Z = 0;
|
||
LaserRange = 0;
|
||
Acc = new Vector3(0, 0, 1);
|
||
}
|
||
|
||
/*if (PosXYZ.Z < 0)
|
||
{
|
||
PosXYZ.Z = 0;
|
||
|
||
//if (SpdXYZ.Z < -5)
|
||
//{
|
||
// Active = false; // Сильно ударился о землю
|
||
//}
|
||
|
||
//if (MathF.Abs(ori.X) > 20 || MathF.Abs(ori.Y) > 20)
|
||
//{
|
||
// Active = false; // Повредил винты при посадке
|
||
//}
|
||
|
||
SpdXYZ.Z = 0;
|
||
|
||
Acc = new Vector3(0, 0, 1);
|
||
Gyr = Vector3.Zero;
|
||
LaserRange = 0;
|
||
}*/
|
||
else
|
||
{
|
||
if (ori.W < 0)
|
||
{
|
||
//Active = false; // Перевернулся вверх ногами
|
||
}
|
||
|
||
Quaternion grav = Quat * new Quaternion(AccXYZ.X, AccXYZ.Y, AccXYZ.Z, 0) * Quaternion.Inverse(Quat);
|
||
Acc = new Vector3(grav.X, grav.Y, grav.Z);
|
||
|
||
Gyr = SpdPRY;
|
||
|
||
float tilt = MathF.Sqrt((ori.X * ori.X) + (ori.Y * ori.Y)) * TO_RADI;
|
||
|
||
range = PosXYZ.Z / MathF.Cos(tilt);
|
||
|
||
if (tilt < 90 && ori.W > 0) LaserRange = range;
|
||
else LaserRange = float.MaxValue;
|
||
}
|
||
|
||
RealAcc.Update(Acc, tick);
|
||
RealGyr.Update(Gyr, tick);
|
||
RealRange.Update(LaserRange, tick);
|
||
RealBar.Update(PosXYZ.Z, tick);
|
||
RealPos.Update(PosXYZ, tick);
|
||
|
||
Vector2 of_xy;
|
||
if (range > 0.1) of_xy = new Vector2(SpdXYZ.X / range - Gyr.Y, SpdXYZ.Y / range + Gyr.X);
|
||
else of_xy = Vector2.Zero;
|
||
|
||
bool of = RealOF.Update(of_xy, LaserRange, tick);
|
||
|
||
lock (this)
|
||
{
|
||
MoveOF += RealOF.result;
|
||
CountOF += 1;
|
||
|
||
if (of) ReadyOF = true;
|
||
}
|
||
}
|
||
|
||
private float Range(float pow)
|
||
{
|
||
if (pow > 1) pow = 1;
|
||
if (pow < 0) pow = 0;
|
||
|
||
return pow * Physics.MaxPower;
|
||
}
|
||
|
||
public void SetQadroPow(float ul, float ur, float dl, float dr)
|
||
{
|
||
ul = Range(ul); ur = Range(ur); dl = Range(dl); dr = Range(dr);
|
||
|
||
float coef = Area.Wind.Density * MathF.Pow(Propeller.Diameter, 4);
|
||
|
||
//ul = MathF.Pow(ul * Propeller.MaxRotate, 2) * coef; // я хз как делать
|
||
//ur = MathF.Pow(ur * Propeller.MaxRotate, 2) * coef;
|
||
//dl = MathF.Pow(dl * Propeller.MaxRotate, 2) * coef;
|
||
//dr = MathF.Pow(dr * Propeller.MaxRotate, 2) * coef;
|
||
|
||
Power = (ul + ur + dl + dr) / 4;
|
||
|
||
AccPRY.Y = ((ul + dl) - (ur + dr));
|
||
AccPRY.X = ((ul + ur) - (dl + dr));
|
||
AccPRY.Z = ((ul + dr) - (dl + ur)) / 4;
|
||
}
|
||
|
||
private void SendStep(uint time, uint prev)
|
||
{
|
||
DroneData.Step step = new DroneData.Step();
|
||
|
||
step.Head.Size = Marshal.SizeOf(typeof(DroneData.Step));
|
||
step.Head.Mode = DroneData.DataMode.Response;
|
||
step.Head.Type = DroneData.DataType.Step;
|
||
step.Head.Time = (uint)(DateTime.Now.Ticks / Stopwatch.Frequency / 1000);
|
||
|
||
step.StepTime = time;
|
||
step.PrevTime = prev;
|
||
|
||
byte[] bytes = getBytes(step);
|
||
|
||
try { Client?.Send(bytes); } catch { }
|
||
}
|
||
|
||
private void RecvStep(byte[] data)
|
||
{
|
||
DroneData.Step step = (DroneData.Step)fromBytes(data, typeof(DroneData.Step));
|
||
|
||
StepTime = step.StepTime;
|
||
|
||
StepSemaphore.Release();
|
||
}
|
||
|
||
private void RecvDataMotor4(byte[] data)
|
||
{
|
||
DroneData.DataMotor4 mot = (DroneData.DataMotor4)fromBytes(data, typeof(DroneData.DataMotor4));
|
||
/* обработка */
|
||
SetQadroPow(mot.UL, mot.UR, mot.DL, mot.DR);
|
||
}
|
||
|
||
private byte[] SendDataAcc()
|
||
{
|
||
DroneData.DataAcc acc = new DroneData.DataAcc();
|
||
|
||
acc.Head.Size = Marshal.SizeOf(typeof(DroneData.DataAcc));
|
||
acc.Head.Mode = DroneData.DataMode.Response;
|
||
acc.Head.Type = DroneData.DataType.DataAcc;
|
||
acc.Head.Time = (uint)(DateTime.Now.Ticks / Stopwatch.Frequency / 1000);
|
||
|
||
acc.Acc.X = RealAcc.result.X; acc.Acc.Y = RealAcc.result.Y; acc.Acc.Z = RealAcc.result.Z;
|
||
acc.Time = RealAcc.timer;
|
||
|
||
return getBytes(acc);
|
||
}
|
||
|
||
private byte[] SendDataGyr()
|
||
{
|
||
DroneData.DataGyr gyr = new DroneData.DataGyr();
|
||
|
||
gyr.Head.Size = Marshal.SizeOf(typeof(DroneData.DataGyr));
|
||
gyr.Head.Mode = DroneData.DataMode.Response;
|
||
gyr.Head.Type = DroneData.DataType.DataGyr;
|
||
gyr.Head.Time = (uint)(DateTime.Now.Ticks / Stopwatch.Frequency / 1000);
|
||
|
||
gyr.Gyr.X = RealGyr.result.X; gyr.Gyr.Y = RealGyr.result.Y; gyr.Gyr.Z = RealGyr.result.Z;
|
||
gyr.Time = RealGyr.timer;
|
||
|
||
return getBytes(gyr);
|
||
}
|
||
|
||
private byte[] SendDataMag()
|
||
{
|
||
DroneData.DataMag mag = new DroneData.DataMag();
|
||
|
||
mag.Head.Size = Marshal.SizeOf(typeof(DroneData.DataMag));
|
||
mag.Head.Mode = DroneData.DataMode.Response;
|
||
mag.Head.Type = DroneData.DataType.DataMag;
|
||
mag.Head.Time = (uint)(DateTime.Now.Ticks / Stopwatch.Frequency / 1000);
|
||
|
||
mag.Mag.X = 0; mag.Mag.Y = 0; mag.Mag.Z = 0;
|
||
mag.Time = Timer;
|
||
|
||
return getBytes(mag);
|
||
}
|
||
|
||
private byte[] SendDataRange()
|
||
{
|
||
DroneData.DataRange range = new DroneData.DataRange();
|
||
|
||
range.Head.Size = Marshal.SizeOf(typeof(DroneData.DataRange));
|
||
range.Head.Mode = DroneData.DataMode.Response;
|
||
range.Head.Type = DroneData.DataType.DataRange;
|
||
range.Head.Time = (uint)(DateTime.Now.Ticks / Stopwatch.Frequency / 1000);
|
||
|
||
range.LiDAR = RealRange.result;
|
||
range.Time = RealRange.timer;
|
||
|
||
return getBytes(range);
|
||
}
|
||
|
||
private byte[] SendDataLocal()
|
||
{
|
||
DroneData.DataLocal local = new DroneData.DataLocal();
|
||
|
||
local.Head.Size = Marshal.SizeOf(typeof(DroneData.DataLocal));
|
||
local.Head.Mode = DroneData.DataMode.Response;
|
||
local.Head.Type = DroneData.DataType.DataLocal;
|
||
local.Head.Time = (uint)(DateTime.Now.Ticks / Stopwatch.Frequency / 1000);
|
||
|
||
local.Local.X = RealPos.result.X; local.Local.Y = RealPos.result.Y; local.Local.Z = RealPos.result.Z;
|
||
local.Time = RealPos.timer;
|
||
|
||
return getBytes(local);
|
||
}
|
||
|
||
private byte[] SendDataBarometer()
|
||
{
|
||
DroneData.DataBar bar = new DroneData.DataBar();
|
||
|
||
bar.Head.Size = Marshal.SizeOf(typeof(DroneData.DataBar));
|
||
bar.Head.Mode = DroneData.DataMode.Response;
|
||
bar.Head.Type = DroneData.DataType.DataBar;
|
||
bar.Head.Time = (uint)(DateTime.Now.Ticks / Stopwatch.Frequency / 1000);
|
||
|
||
bar.Pressure = RealBar.result;
|
||
bar.Time = RealBar.timer;
|
||
|
||
return getBytes(bar);
|
||
}
|
||
|
||
private byte[] SendDataOF()
|
||
{
|
||
DroneData.DataOF of = new DroneData.DataOF();
|
||
|
||
of.Head.Size = Marshal.SizeOf(typeof(DroneData.DataOF));
|
||
of.Head.Mode = DroneData.DataMode.Response;
|
||
of.Head.Type = DroneData.DataType.DataOF;
|
||
of.Head.Time = (uint)(DateTime.Now.Ticks / Stopwatch.Frequency / 1000);
|
||
|
||
lock (this)
|
||
{
|
||
if (ReadyOF && CountOF != 0)
|
||
{
|
||
of.X = MoveOF.X / CountOF;
|
||
of.Y = MoveOF.Y / CountOF;
|
||
}
|
||
else
|
||
{
|
||
of.X = 0;
|
||
of.Y = 0;
|
||
}
|
||
|
||
of.Time = RealOF.timer;
|
||
|
||
MoveOF = Vector2.Zero;
|
||
CountOF = 0;
|
||
ReadyOF = false;
|
||
}
|
||
|
||
return getBytes(of);
|
||
}
|
||
|
||
private byte[] SendDataGPS()
|
||
{
|
||
DroneData.DataGPS gps = new DroneData.DataGPS();
|
||
|
||
gps.Head.Size = Marshal.SizeOf(typeof(DroneData.DataGPS));
|
||
gps.Head.Mode = DroneData.DataMode.Response;
|
||
gps.Head.Type = DroneData.DataType.DataGPS;
|
||
gps.Head.Time = (uint)(DateTime.Now.Ticks / Stopwatch.Frequency / 1000);
|
||
|
||
GPS.Point p = new GPS.Point();
|
||
p.x = RealPos.result.Y; p.y= RealPos.result.X;
|
||
|
||
GPS.GlobalCoords g = new GPS.GlobalCoords();
|
||
g.latitude=GPS.Home.Lat; g.longitude=GPS.Home.Lon;
|
||
|
||
g=GPS.localToGlobal(p, g);
|
||
|
||
gps.Lat = g.latitude; gps.Lon = g.longitude;
|
||
|
||
gps.Speed = MathF.Sqrt(SpdXYZ.X * SpdXYZ.X + SpdXYZ.Y * SpdXYZ.Y + SpdXYZ.Z * SpdXYZ.Z);
|
||
gps.Alt = GPS.Home.Alt + RealPos.result.Z;
|
||
|
||
DateTime tim = DateTime.Now;
|
||
gps.UTC = tim.Second + tim.Minute * 100 + tim.Hour * 10000;
|
||
|
||
gps.Fix = GPS.State.Fix;
|
||
gps.SatVisible = GPS.State.SatVisible;
|
||
gps.SatUsed = GPS.State.SatUsed;
|
||
gps.Noise = GPS.State.Noise;
|
||
gps.Hdop = GPS.State.Hdop;
|
||
gps.Vdop = GPS.State.Vdop;
|
||
gps.Pdop = GPS.State.Pdop;
|
||
|
||
gps.Time = RealPos.timer;
|
||
|
||
return getBytes(gps);
|
||
}
|
||
|
||
private byte[] SendDataQuaternion()
|
||
{
|
||
DroneData.DataQuat quat = new DroneData.DataQuat();
|
||
|
||
quat.Head.Size = Marshal.SizeOf(typeof(DroneData.DataQuat));
|
||
quat.Head.Mode = DroneData.DataMode.Response;
|
||
quat.Head.Type = DroneData.DataType.DataQuat;
|
||
quat.Head.Time = (uint)(DateTime.Now.Ticks / Stopwatch.Frequency / 1000);
|
||
|
||
quat.X = Quat.X; quat.Y = Quat.Y; quat.Z = Quat.Z; quat.W = Quat.W;
|
||
|
||
return getBytes(quat);
|
||
}
|
||
|
||
private byte[] SendPingPong()
|
||
{
|
||
DroneData.DataHead head = new DroneData.DataHead();
|
||
|
||
head.Size = Marshal.SizeOf(typeof(DroneData.DataHead));
|
||
head.Mode = DroneData.DataMode.Response;
|
||
head.Type = DroneData.DataType.Ping;
|
||
head.Time = (uint)(DateTime.Now.Ticks / Stopwatch.Frequency / 1000);
|
||
|
||
return getBytes(head);
|
||
}
|
||
|
||
private byte[]? ServerRequestResponse(DroneData.DataHead head, byte[] body)
|
||
{
|
||
byte[] zero = Array.Empty<byte>();
|
||
|
||
switch (head.Type)
|
||
{
|
||
case DroneData.DataType.Step: if (head.Mode == DroneData.DataMode.Request) RecvStep(body); return zero;
|
||
|
||
case DroneData.DataType.DataAcc:
|
||
{
|
||
if (head.Mode == DroneData.DataMode.Request)
|
||
{
|
||
// Запрос данных
|
||
return SendDataAcc();
|
||
}
|
||
else
|
||
{
|
||
// Пришли данные
|
||
// ... //
|
||
//
|
||
return zero;
|
||
}
|
||
}
|
||
|
||
case DroneData.DataType.DataGyr: if (head.Mode == DroneData.DataMode.Request) return SendDataGyr(); else return zero;
|
||
|
||
case DroneData.DataType.DataMag: if (head.Mode == DroneData.DataMode.Request) return SendDataMag(); else return zero;
|
||
|
||
case DroneData.DataType.DataRange: if (head.Mode == DroneData.DataMode.Request) return SendDataRange(); else return zero;
|
||
|
||
case DroneData.DataType.DataLocal: if (head.Mode == DroneData.DataMode.Request) return SendDataLocal(); else return zero;
|
||
|
||
case DroneData.DataType.DataBar: if (head.Mode == DroneData.DataMode.Request) return SendDataBarometer(); else return zero;
|
||
|
||
case DroneData.DataType.DataOF: if (head.Mode == DroneData.DataMode.Request) return SendDataOF(); else return zero;
|
||
|
||
case DroneData.DataType.DataGPS: if (head.Mode == DroneData.DataMode.Request) return SendDataGPS(); else return zero;
|
||
|
||
case DroneData.DataType.DataQuat: if (head.Mode == DroneData.DataMode.Request) return SendDataQuaternion(); else return zero;
|
||
|
||
case DroneData.DataType.DataMotor4: if (head.Mode == DroneData.DataMode.Response) RecvDataMotor4(body); return zero;
|
||
|
||
case DroneData.DataType.Ping: if (head.Mode == DroneData.DataMode.Request) return SendPingPong(); 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.Ping };
|
||
|
||
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 == 0) return null; // Поток сломан (конец)
|
||
|
||
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(ServerRequestResponse(DroneStreamHead, body));
|
||
}
|
||
|
||
return ret;
|
||
}
|
||
}
|
||
}
|