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

+++

Browse Source
This commit is contained in:
Sergey Sklyarov
2025-06-28 04:19:45 +03:00
parent 782d24a8e6
commit bf389cdf06
2 changed files with 11 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
DroneSimulator/Drone.cs
View File

@ -155,13 +155,14 @@ namespace DroneSimulator
if (drone.StepTime > 0) if (drone.StepTime > 0)
{ {
uint tick = Tick(); uint tick = Tick();
uint step = drone.Timer + drone.StepTime; uint prev = drone.Timer;
uint step = prev + drone.StepTime;
if (TimeLimit && (step > tick)) step = tick; if (TimeLimit && (step > tick)) step = tick;
drone.Action(step); drone.Action(step);
drone.StepTime = 0; drone.StepTime = 0;
drone.SendStep(step); drone.SendStep(step, prev);
break; break;
} }
} }
@ -377,16 +378,17 @@ namespace DroneSimulator
AccPRY.Z = ((ul + dr) - (dl + ur)) / 4; AccPRY.Z = ((ul + dr) - (dl + ur)) / 4;
} }
private void SendStep(uint time) private void SendStep(uint time, uint prev)
{ {
DroneData.Step step = new DroneData.Step(); DroneData.Step step = new DroneData.Step();
step.Head.Size = Marshal.SizeOf(typeof(DroneData.DataAcc)); step.Head.Size = Marshal.SizeOf(typeof(DroneData.Step));
step.Head.Mode = DroneData.DataMode.Response; step.Head.Mode = DroneData.DataMode.Response;
step.Head.Type = DroneData.DataType.Step; step.Head.Type = DroneData.DataType.Step;
step.Head.Time = Tick(); step.Head.Time = Tick();
step.StepTime = time; step.StepTime = time;
step.PrevTime = prev;
byte[] bytes = getBytes(step); byte[] bytes = getBytes(step);

10
DroneSimulator/DroneData.cs
View File

@ -1,5 +1,4 @@
using System.Net; using System.Runtime.InteropServices;
using System.Runtime.InteropServices;
namespace DroneData namespace DroneData
{ {
@ -9,8 +8,8 @@ namespace DroneData
}; };
public enum DataType : ushort public enum DataType : ushort
{ {
Ping = 0, Step = 1, None = 0, Ping = 1, Step = 2,
// Output // Output
DataAcc = 1001, DataGyr = 1002, DataMag = 1003, DataRange = 1004, DataLocal = 1005, DataBar = 1006, DataOF = 1007, DataGPS = 1008, DataAcc = 1001, DataGyr = 1002, DataMag = 1003, DataRange = 1004, DataLocal = 1005, DataBar = 1006, DataOF = 1007, DataGPS = 1008,
@ -39,8 +38,9 @@ namespace DroneData
public DataHead Head; public DataHead Head;
public uint StepTime; // ms, запрос: шаг работы симулятора; ответ: Последнее время изменения данных public uint StepTime; // ms, запрос: шаг работы симулятора; ответ: Последнее время изменения данных
public uint PrevTime; // ms, ответ: Предпоследнее время изменения данных
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataAcc)); static public int StrLen = Marshal.SizeOf(typeof(DroneData.Step));
} }
public struct XYZ { public float X, Y, Z; } public struct XYZ { public float X, Y, Z; }