forked from CPL/Simulator
498 lines
13 KiB
C#
498 lines
13 KiB
C#
using System.CodeDom;
|
||
using System.Numerics;
|
||
using System.Runtime.InteropServices;
|
||
using System.Security.Cryptography;
|
||
|
||
namespace DroneSimulator
|
||
{
|
||
internal class Drone
|
||
{
|
||
public int ID;
|
||
public float Mass; // Масса
|
||
public bool Active; // Живой?
|
||
public float Length; // Длинна лучей
|
||
public const float Dynamic = 10; // Динамика вращения
|
||
public Vector3 PosXYZ, SpdXYZ, AccXYZ; // Положение в пространстве: Позиция, Скорость, Ускорение
|
||
public Quaternion Quat; // Основной кватернион
|
||
public float Power = 0; // Тяга всех двигателей (0-1)
|
||
public float MaxPower; // Максимальная Тяга всех двигателей (КГ)
|
||
public Vector3 SpdPRY, AccPRY; // Поворот в пространстве: pitch roll yaw
|
||
|
||
public Vector3 Acc, Gyr; // Имитация: Акселерометр, Гироскоп
|
||
public float LaserRange; // Имитация: Дальномер
|
||
|
||
private uint DataTimer;
|
||
|
||
private const float Gravity = 9.8f;
|
||
|
||
private const float TO_GRAD = 180 / MathF.PI;
|
||
private const float TO_RADI = MathF.PI / 180;
|
||
|
||
private Thread DroneThread;
|
||
private int Timer;
|
||
|
||
private static int CounterID = 0;
|
||
|
||
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 struct DataVisual
|
||
{
|
||
public int ID; // Идентификатор
|
||
public float W, X, Y, Z; // Кватернион вращения
|
||
public float PosX, PosY, PosZ; // Координаты в пространстве
|
||
}
|
||
|
||
public DataVisual GetVisual()
|
||
{
|
||
return new DataVisual() { ID = this.ID, W = this.Quat.W, X = this.Quat.X, Y = this.Quat.Y, Z = this.Quat.Z, PosX = this.PosXYZ.X, PosY = this.PosXYZ.Y, PosZ = this.PosXYZ.Z };
|
||
}
|
||
|
||
private void ThreadFunction()
|
||
{
|
||
while (DroneThread != null)
|
||
{
|
||
Action(Environment.TickCount);
|
||
Thread.Sleep(1);
|
||
}
|
||
}
|
||
|
||
public Drone(int id)
|
||
{
|
||
ID = id;
|
||
Active = false;
|
||
PosXYZ = Vector3.Zero;
|
||
SpdXYZ = Vector3.Zero;
|
||
AccXYZ = Vector3.Zero;
|
||
Quat = Quaternion.Identity;
|
||
|
||
DroneThread = new Thread(new ThreadStart(ThreadFunction));
|
||
Timer = Environment.TickCount;
|
||
DroneThread.Start();
|
||
}
|
||
|
||
public int Create(float power, float mass, float len)
|
||
{
|
||
Mass = mass;
|
||
Length = len;
|
||
MaxPower = power;
|
||
|
||
Active = true;
|
||
|
||
return ID;
|
||
}
|
||
|
||
public void Close()
|
||
{
|
||
DroneThread = null;
|
||
}
|
||
|
||
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(int tick)
|
||
{
|
||
float time = (tick - Timer) / 1000.0f;
|
||
Timer = tick;
|
||
|
||
if (!Active) return;
|
||
|
||
float flow = Power;
|
||
|
||
if (PosXYZ.Z < 0.3f)
|
||
{
|
||
flow += flow * 0.1f; // Воздушная подушка
|
||
}
|
||
|
||
SpdPRY += AccPRY * (Dynamic * time / (Mass * Length));
|
||
|
||
Quaternion pow = Quaternion.Inverse(Quat) * new Quaternion(0, 0, flow, 0) * Quat;
|
||
AccXYZ = new Vector3(pow.X, pow.Y, pow.Z) * (Gravity / Mass);
|
||
|
||
SpdXYZ += (AccXYZ + new Vector3(0, 0, -Gravity)) * time;
|
||
PosXYZ += SpdXYZ * time;
|
||
|
||
AccXYZ /= Gravity; // Вернуть измерения в G
|
||
|
||
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();
|
||
|
||
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 = new Quaternion(AccXYZ.X, AccXYZ.Y, AccXYZ.Z, 0);
|
||
//grav = (Quat * grav) * Quaternion.Inverse(Quat); // Инерциальный акселерометр (тест)
|
||
Acc = new Vector3(grav.X, grav.Y, grav.Z);
|
||
|
||
Gyr = SpdPRY;
|
||
|
||
float tilt = (MathF.Abs(ori.X) + MathF.Abs(ori.Y)) * TO_RADI;
|
||
|
||
if (tilt < 90 && ori.W > 0) LaserRange = PosXYZ.Z / MathF.Cos(tilt);
|
||
else LaserRange = float.MaxValue;
|
||
}
|
||
|
||
DataTimer = (uint)tick;
|
||
}
|
||
|
||
private float Range(float pow)
|
||
{
|
||
if (pow > 1) pow = 1;
|
||
if (pow < 0) pow = 0;
|
||
|
||
return pow * MaxPower;
|
||
}
|
||
|
||
public void SetQadroPow(float ul, float ur, float dl, float dr)
|
||
{
|
||
ul = Range(ul); ur = Range(ur); dl = Range(dl); dr = Range(dr);
|
||
|
||
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 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)Environment.TickCount;
|
||
|
||
acc.Acc.X = Acc.X; acc.Acc.Y = Acc.Y; acc.Acc.Z = Acc.Z;
|
||
acc.Time = DataTimer;
|
||
|
||
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)Environment.TickCount;
|
||
|
||
gyr.Gyr.X = Gyr.X; gyr.Gyr.Y = Gyr.Y; gyr.Gyr.Z = Gyr.Z;
|
||
gyr.Time = DataTimer;
|
||
|
||
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)Environment.TickCount;
|
||
|
||
mag.Mag.X = 0; mag.Mag.Y = 0; mag.Mag.Z = 0;
|
||
mag.Time = DataTimer;
|
||
|
||
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)Environment.TickCount;
|
||
|
||
range.LiDAR = LaserRange;
|
||
range.Time = DataTimer;
|
||
|
||
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)Environment.TickCount;
|
||
|
||
local.Local.X = PosXYZ.X; local.Local.Y = PosXYZ.Y; local.Local.Z = PosXYZ.Z;
|
||
local.Time = DataTimer;
|
||
|
||
return getBytes(local);
|
||
}
|
||
|
||
private byte[]? ServerRequestResponse(DroneData.DataHead head, byte[] body)
|
||
{
|
||
byte[] zero = new byte[0];
|
||
|
||
switch (head.Type)
|
||
{
|
||
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.DataMotor4:
|
||
{
|
||
if (head.Mode == DroneData.DataMode.Request)
|
||
{
|
||
// Запрос данных
|
||
// ... //
|
||
//
|
||
return zero;
|
||
}
|
||
else
|
||
{
|
||
// Пришли данные
|
||
RecvDataMotor4(body);
|
||
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(ServerRequestResponse(DroneStreamHead, body));
|
||
}
|
||
|
||
return ret;
|
||
}
|
||
}
|
||
}
|