forked from CPL/Simulator
Update
This commit is contained in:
@ -21,6 +21,8 @@ namespace DroneSimulator
|
||||
public Vector3 Acc, Gyr; // Имитация: Акселерометр, Гироскоп
|
||||
public float LaserRange; // Имитация: Дальномер
|
||||
|
||||
private uint DataTimer;
|
||||
|
||||
private const float Gravity = 9.8f;
|
||||
|
||||
private const float TO_GRAD = 180 / MathF.PI;
|
||||
@ -72,33 +74,6 @@ namespace DroneSimulator
|
||||
return mem;
|
||||
}
|
||||
|
||||
/*public struct DataOut
|
||||
{
|
||||
public float AccX, AccY, AccZ;
|
||||
public float GyrX, GyrY, GyrZ;
|
||||
public float PosX, PosY;
|
||||
public float LaserRange;
|
||||
}
|
||||
|
||||
public DataOut GetDataOut()
|
||||
{
|
||||
DataOut data = new DataOut();
|
||||
|
||||
data.AccX = Acc.X; data.AccY = Acc.Y; data.AccZ = Acc.Z;
|
||||
data.GyrX = Gyr.X; data.GyrY = Gyr.Y; data.GyrZ = Gyr.Z;
|
||||
|
||||
data.PosX = PosXYZ.X; data.PosY = PosXYZ.Y;
|
||||
|
||||
data.LaserRange = LaserRange;
|
||||
|
||||
return data;
|
||||
}
|
||||
|
||||
public struct DataIn
|
||||
{
|
||||
public float MotorUL, MotorUR, MotorDL, MotorDR;
|
||||
}*/
|
||||
|
||||
public struct DataVisual
|
||||
{
|
||||
public int ID; // Идентификатор
|
||||
@ -264,6 +239,8 @@ namespace DroneSimulator
|
||||
if (tilt < 90 && ori.W > 0) LaserRange = PosXYZ.Z / MathF.Cos(tilt);
|
||||
else LaserRange = float.MaxValue;
|
||||
}
|
||||
|
||||
DataTimer = (uint)tick;
|
||||
}
|
||||
|
||||
private float Range(float pow)
|
||||
@ -292,33 +269,79 @@ namespace DroneSimulator
|
||||
SetQadroPow(mot.UL, mot.UR, mot.DL, mot.DR);
|
||||
}
|
||||
|
||||
private byte[] SendDataIMU()
|
||||
private byte[] SendDataAcc()
|
||||
{
|
||||
DroneData.DataIMU imu = new DroneData.DataIMU();
|
||||
DroneData.DataAcc acc = new DroneData.DataAcc();
|
||||
|
||||
imu.Head.Size = Marshal.SizeOf(typeof(DroneData.DataIMU));
|
||||
imu.Head.Mode = DroneData.DataMode.Response;
|
||||
imu.Head.Type = DroneData.DataType.DataIMU;
|
||||
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;
|
||||
|
||||
imu.Acc.X = Acc.X; imu.Acc.Y = Acc.Y; imu.Acc.Z = Acc.Z;
|
||||
imu.Gyr.X = Gyr.X; imu.Gyr.Y = Gyr.Y; imu.Gyr.Z = Gyr.Z;
|
||||
imu.Mag.X = 0; imu.Mag.Y = 0; imu.Mag.Z = 0;
|
||||
acc.Acc.X = Acc.X; acc.Acc.Y = Acc.Y; acc.Acc.Z = Acc.Z;
|
||||
acc.Time = DataTimer;
|
||||
|
||||
return getBytes(imu);
|
||||
return getBytes(acc);
|
||||
}
|
||||
|
||||
private byte[] SendDataPos()
|
||||
private byte[] SendDataGyr()
|
||||
{
|
||||
DroneData.DataPos pos = new DroneData.DataPos();
|
||||
DroneData.DataGyr gyr = new DroneData.DataGyr();
|
||||
|
||||
pos.Head.Size = Marshal.SizeOf(typeof(DroneData.DataPos));
|
||||
pos.Head.Mode = DroneData.DataMode.Response;
|
||||
pos.Head.Type = DroneData.DataType.DataPos;
|
||||
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;
|
||||
|
||||
pos.Local.X = PosXYZ.X; pos.Local.Y = PosXYZ.Y; pos.Local.Z = PosXYZ.Z;
|
||||
pos.LiDAR = LaserRange;
|
||||
gyr.Gyr.X = Gyr.X; gyr.Gyr.Y = Gyr.Y; gyr.Gyr.Z = Gyr.Z;
|
||||
gyr.Time = DataTimer;
|
||||
|
||||
return getBytes(pos);
|
||||
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)
|
||||
@ -327,12 +350,12 @@ namespace DroneSimulator
|
||||
|
||||
switch (head.Type)
|
||||
{
|
||||
case DroneData.DataType.DataIMU:
|
||||
case DroneData.DataType.DataAcc:
|
||||
{
|
||||
if (head.Mode == DroneData.DataMode.Request)
|
||||
{
|
||||
// Запрос данных
|
||||
return SendDataIMU();
|
||||
return SendDataAcc();
|
||||
}
|
||||
else
|
||||
{
|
||||
@ -343,12 +366,60 @@ namespace DroneSimulator
|
||||
}
|
||||
}
|
||||
|
||||
case DroneData.DataType.DataPos:
|
||||
case DroneData.DataType.DataGyr:
|
||||
{
|
||||
if (head.Mode == DroneData.DataMode.Request)
|
||||
{
|
||||
// Запрос данных
|
||||
return SendDataPos();
|
||||
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
|
||||
{
|
||||
|
||||
Reference in New Issue
Block a user