CPL/Simulator
5
0
Fork 2
Code Issues 1 Pull Requests Actions Packages Projects Releases Activity

Add drone state window

Browse Source
This commit is contained in:
Sergey Sklyarov
2025-05-16 01:27:03 +03:00
parent 7bf2553455
commit e0f3e3db19
5 changed files with 51 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
DroneSimulator/Drone.cs
View File

@ -24,6 +24,8 @@ namespace DroneSimulator
public Vector3 Acc, Gyr; // Имитация: Акселерометр, Гироскоп
public float LaserRange; // Имитация: Дальномер
public Vector4 Orientation;
private uint DataTimer;
private const float Gravity = 9.8f;
@ -210,6 +212,8 @@ namespace DroneSimulator
Vector4 ori = GetOrientation();
Orientation = ori;
if (PosXYZ.Z < 0)
{
PosXYZ.Z = 0;
@ -374,9 +378,23 @@ namespace DroneSimulator
return getBytes(bar);
}
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)Environment.TickCount;
quat.X = Quat.X; quat.Y = Quat.Y; quat.Z = Quat.Z; quat.W = Quat.W;
return getBytes(quat);
}
private byte[]? ServerRequestResponse(DroneData.DataHead head, byte[] body)
{
byte[] zero = new byte[0];
byte[] zero = Array.Empty<byte>();
switch (head.Type)
{
@ -406,6 +424,8 @@ namespace DroneSimulator
case DroneData.DataType.DataBar: if (head.Mode == DroneData.DataMode.Request) return SendDataBarometer(); 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;
}