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

+++

Browse Source
This commit is contained in:
Sergey Sklyarov
2025-06-06 03:27:21 +03:00
parent 72ea9fd6a6
commit afece52bb2
5 changed files with 437 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

29
DroneSimulator/Drone.cs
View File

@ -1,5 +1,6 @@
using System.Numerics;
using System.Runtime.InteropServices;
using static System.Net.Mime.MediaTypeNames;
namespace DroneSimulator
{
@ -436,6 +437,32 @@ namespace DroneSimulator
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)Environment.TickCount;
gps.Speed = MathF.Sqrt(SpdXYZ.X * SpdXYZ.X + SpdXYZ.Y * SpdXYZ.Y + SpdXYZ.Z * SpdXYZ.Z);
gps.Alt = GPS.Home.Alt + PosXYZ.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;
return getBytes(gps);
}
private byte[] SendDataQuaternion()
{
DroneData.DataQuat quat = new DroneData.DataQuat();
@ -484,6 +511,8 @@ namespace DroneSimulator
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;