dmarkova/Simulator
1
0
Fork 0
forked from CPL/Simulator
Code Pull Requests Activity

update

Browse Source
This commit is contained in:
Sergey Sklyarov
2025-04-12 01:10:12 +03:00
parent bac52d315b
commit 376ce81a8a
4 changed files with 1367 additions and 485 deletions
Download Patch File Download Diff File Expand all files Collapse all files

120
DroneSimulator/Drone.cs
View File

@ -2,6 +2,8 @@
using System.Numerics;
using System.Runtime.InteropServices;
using System.Security.Cryptography;
using static DroneSimulator.Drone;
using static System.Windows.Forms.VisualStyles.VisualStyleElement.Rebar;
namespace DroneSimulator
{
@ -31,7 +33,17 @@ namespace DroneSimulator
private Thread DroneThread;
private int Timer;
private static int CounterID = 0;
private Random MainRandom = new Random();
public struct DataBarometer
{
public int Value, Pressure; // Значение давления в Паскальях
public float Accuracy;
public int Frequency;
public uint Time;
}
public DataBarometer dataBarometer;
public static byte[] getBytes(object data)
{
@ -240,6 +252,14 @@ namespace DroneSimulator
else LaserRange = float.MaxValue;
}
if ((dataBarometer.Time + 1000 / dataBarometer.Frequency) < DataTimer)
{
float bar = dataBarometer.Pressure - PosXYZ.Z * 11;
int rnd = MainRandom.Next(-(int)dataBarometer.Accuracy * 10, (int)dataBarometer.Accuracy * 10) / 10;
dataBarometer.Value = (int)(bar + rnd);
dataBarometer.Time = DataTimer;
}
DataTimer = (uint)tick;
}
@ -344,6 +364,21 @@ namespace DroneSimulator
return getBytes(local);
}
private byte[] SendDataBarometer()
{
DroneData.DataBar bar = new DroneData.DataBar();
bar.Head.Size = Marshal.SizeOf(typeof(DroneData.DataBar));
bar.Head.Mode = DroneData.DataMode.Response;
bar.Head.Type = DroneData.DataType.DataBar;
bar.Head.Time = (uint)Environment.TickCount;
bar.Pressure = dataBarometer.Value;
bar.Time = dataBarometer.Time;
return getBytes(bar);
}
private byte[]? ServerRequestResponse(DroneData.DataHead head, byte[] body)
{
byte[] zero = new byte[0];
@ -366,86 +401,17 @@ namespace DroneSimulator
}
}
case DroneData.DataType.DataGyr:
{
if (head.Mode == DroneData.DataMode.Request)
{
// Запрос данных
return SendDataGyr();
}
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.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.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.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;
}
}
case DroneData.DataType.DataBar: if (head.Mode == DroneData.DataMode.Request) return SendDataBarometer(); else return zero;
case DroneData.DataType.DataMotor4: if (head.Mode == DroneData.DataMode.Response) RecvDataMotor4(body); return zero;
}
return zero;