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

update

Browse Source
This commit is contained in:
Sergey Sklyarov
2025-05-22 01:47:54 +03:00
parent e0f3e3db19
commit 90cec037eb
9 changed files with 553 additions and 176 deletions
Download Patch File Download Diff File Expand all files Collapse all files

38
DroneClient/Drone.cs
View File

@ -1,5 +1,7 @@
using System.Diagnostics.Metrics;
using DroneData;
using System.Diagnostics.Metrics;
using System.Drawing;
using System.Numerics;
using System.Runtime.InteropServices;
namespace DroneClient
@ -15,6 +17,8 @@ namespace DroneClient
public float LaserRange;
public uint TimeRange;
public Vector2 OF = Vector2.Zero;
public float MotorUL, MotorUR, MotorDL, MotorDR;
public static byte[] getBytes(object data)
@ -116,6 +120,16 @@ namespace DroneClient
return new byte[0];
}
private byte[]? RecvDataOF(byte[] data)
{
DroneData.DataOF of = (DroneData.DataOF)fromBytes(data, typeof(DroneData.DataOF));
OF.X = of.X;
OF.Y = of.Y;
return new byte[0];
}
private byte[]? ClientRequestResponse(DroneData.DataHead head, byte[] body)
{
byte[] zero = new byte[0];
@ -186,6 +200,22 @@ namespace DroneClient
}
}
case DroneData.DataType.DataOF:
{
if (head.Mode == DroneData.DataMode.Request)
{
// Запрос данных
// ... //
//
return zero;
}
else
{
// Пришли данные
return RecvDataOF(body);
}
}
case DroneData.DataType.DataMotor4:
{
if (head.Mode == DroneData.DataMode.Request)
@ -271,12 +301,18 @@ namespace DroneClient
local.Mode = DroneData.DataMode.Request;
local.Type = DroneData.DataType.DataLocal;
DroneData.DataHead of = new DroneData.DataHead();
of.Size = DroneData.DataHead.StrLen;
of.Mode = DroneData.DataMode.Request;
of.Type = DroneData.DataType.DataOF;
List<byte[]> list = new List<byte[]>();
list.Add(getBytes(acc));
list.Add(getBytes(gyr));
list.Add(getBytes(range));
list.Add(getBytes(local));
list.Add(getBytes(of));
list.Add(SendDataMotor4());
int count = 0;

61
DroneClient/DroneData.cs
View File

@ -1,4 +1,4 @@
using System.Runtime.InteropServices;
using System.Runtime.InteropServices;
namespace DroneData
{
@ -12,10 +12,13 @@ namespace DroneData
None = 0, Head = 1,
// Output
DataAcc = 1001, DataGyr = 1002, DataMag = 1003, DataRange = 1004, DataLocal = 1005,
DataAcc = 1001, DataGyr = 1002, DataMag = 1003, DataRange = 1004, DataLocal = 1005, DataBar = 1006, DataOF = 1007,
// Input
DataMotor4 = 2001, DataMotor6 = 2002
DataMotor4 = 2001, DataMotor6 = 2002,
// State
DataQuat = 3001,
};
public struct DataHead
@ -25,7 +28,7 @@ namespace DroneData
public DataMode Mode;
public DataType Type;
public uint Time; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD>
public uint Time; // Общее время
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataHead));
}
@ -37,7 +40,7 @@ namespace DroneData
public DataHead Head;
public XYZ Acc;
public uint Time; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
public uint Time; // Последнее время изменения данных
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataAcc));
}
@ -47,7 +50,7 @@ namespace DroneData
public DataHead Head;
public XYZ Gyr;
public uint Time; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
public uint Time; // Последнее время изменения данных
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataGyr));
}
@ -57,31 +60,51 @@ namespace DroneData
public DataHead Head;
public XYZ Mag;
public uint Time; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
public uint Time; // Последнее время изменения данных
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataMag));
}
public struct DataLocal
{
public DataHead Head;
public XYZ Local; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
public uint Time; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataLocal));
}
public struct DataRange
{
public DataHead Head;
public float LiDAR; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
public float LiDAR; // Датчик посадки
public uint Time; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
public uint Time; // Последнее время изменения данных
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataRange));
}
public struct DataLocal
{
public DataHead Head;
public XYZ Local; // Локальные координаты
public uint Time; // Последнее время изменения данных
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataLocal));
}
public struct DataBar
{
public DataHead Head;
public float Pressure; // Давление
public uint Time; // Последнее время изменения данных
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataBar));
}
public struct DataOF
{
public DataHead Head;
public float X, Y; // Угловой сдвиг
public uint Time; // Последнее время изменения данных
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataOF));
}
public struct DataMotor4
{
public DataHead Head;

131
DroneClient/FormMain.Designer.cs generated
View File

@ -38,12 +38,14 @@
label3 = new Label();
label1 = new Label();
groupBox2 = new GroupBox();
label_time_acc = new Label();
label_Acc_Z = new Label();
label7 = new Label();
label_Acc_Y = new Label();
label5 = new Label();
label_Acc_X = new Label();
groupBox3 = new GroupBox();
label_time_gyr = new Label();
label_Gyr_Z = new Label();
label9 = new Label();
label_Gyr_Y = new Label();
@ -51,6 +53,7 @@
label_Gyr_X = new Label();
label13 = new Label();
groupBox4 = new GroupBox();
label_time_range = new Label();
label_Pos_L = new Label();
label6 = new Label();
label_Pos_Y = new Label();
@ -65,15 +68,19 @@
label_Pow = new Label();
button_ML = new Button();
button_MR = new Button();
label_time_acc = new Label();
label_time_range = new Label();
label_time_gyr = new Label();
groupBox5 = new GroupBox();
label_time_of = new Label();
label_OF_Y = new Label();
label17 = new Label();
label_OF_X = new Label();
label19 = new Label();
groupBox1.SuspendLayout();
((System.ComponentModel.ISupportInitialize)numericUpDown_Server_Port).BeginInit();
groupBox2.SuspendLayout();
groupBox3.SuspendLayout();
groupBox4.SuspendLayout();
((System.ComponentModel.ISupportInitialize)trackBar_Power).BeginInit();
groupBox5.SuspendLayout();
SuspendLayout();
//
// timer_Test
@ -92,7 +99,7 @@
groupBox1.Dock = DockStyle.Top;
groupBox1.Location = new Point(0, 0);
groupBox1.Name = "groupBox1";
groupBox1.Size = new Size(275, 80);
groupBox1.Size = new Size(357, 80);
groupBox1.TabIndex = 3;
groupBox1.TabStop = false;
groupBox1.Tag = "";
@ -173,6 +180,15 @@
groupBox2.TabStop = false;
groupBox2.Text = "Acc";
//
// label_time_acc
//
label_time_acc.AutoSize = true;
label_time_acc.Location = new Point(6, 100);
label_time_acc.Name = "label_time_acc";
label_time_acc.Size = new Size(13, 15);
label_time_acc.TabIndex = 25;
label_time_acc.Text = "0";
//
// label_Acc_Z
//
label_Acc_Z.AutoSize = true;
@ -227,13 +243,22 @@
groupBox3.Controls.Add(label11);
groupBox3.Controls.Add(label_Gyr_X);
groupBox3.Controls.Add(label13);
groupBox3.Location = new Point(95, 86);
groupBox3.Location = new Point(94, 86);
groupBox3.Name = "groupBox3";
groupBox3.Size = new Size(78, 118);
groupBox3.TabIndex = 6;
groupBox3.TabStop = false;
groupBox3.Text = "Gyr";
//
// label_time_gyr
//
label_time_gyr.AutoSize = true;
label_time_gyr.Location = new Point(3, 100);
label_time_gyr.Name = "label_time_gyr";
label_time_gyr.Size = new Size(13, 15);
label_time_gyr.TabIndex = 26;
label_time_gyr.Text = "0";
//
// label_Gyr_Z
//
label_Gyr_Z.AutoSize = true;
@ -297,13 +322,22 @@
groupBox4.Controls.Add(label10);
groupBox4.Controls.Add(label_Pos_X);
groupBox4.Controls.Add(label14);
groupBox4.Location = new Point(188, 86);
groupBox4.Location = new Point(182, 86);
groupBox4.Name = "groupBox4";
groupBox4.Size = new Size(78, 118);
groupBox4.TabIndex = 7;
groupBox4.TabStop = false;
groupBox4.Text = "Pos";
//
// label_time_range
//
label_time_range.AutoSize = true;
label_time_range.Location = new Point(6, 100);
label_time_range.Name = "label_time_range";
label_time_range.Size = new Size(13, 15);
label_time_range.TabIndex = 27;
label_time_range.Text = "0";
//
// label_Pos_L
//
label_Pos_L.AutoSize = true;
@ -443,38 +477,71 @@
button_MR.MouseDown += button_UU_MouseDown;
button_MR.MouseUp += button_UU_MouseUp;
//
// label_time_acc
// groupBox5
//
label_time_acc.AutoSize = true;
label_time_acc.Location = new Point(6, 100);
label_time_acc.Name = "label_time_acc";
label_time_acc.Size = new Size(13, 15);
label_time_acc.TabIndex = 25;
label_time_acc.Text = "0";
groupBox5.Controls.Add(label_time_of);
groupBox5.Controls.Add(label_OF_Y);
groupBox5.Controls.Add(label17);
groupBox5.Controls.Add(label_OF_X);
groupBox5.Controls.Add(label19);
groupBox5.Location = new Point(270, 86);
groupBox5.Name = "groupBox5";
groupBox5.Size = new Size(78, 118);
groupBox5.TabIndex = 24;
groupBox5.TabStop = false;
groupBox5.Text = "OF";
//
// label_time_range
// label_time_of
//
label_time_range.AutoSize = true;
label_time_range.Location = new Point(6, 100);
label_time_range.Name = "label_time_range";
label_time_range.Size = new Size(13, 15);
label_time_range.TabIndex = 27;
label_time_range.Text = "0";
label_time_of.AutoSize = true;
label_time_of.Location = new Point(6, 100);
label_time_of.Name = "label_time_of";
label_time_of.Size = new Size(13, 15);
label_time_of.TabIndex = 27;
label_time_of.Text = "0";
//
// label_time_gyr
// label_OF_Y
//
label_time_gyr.AutoSize = true;
label_time_gyr.Location = new Point(3, 100);
label_time_gyr.Name = "label_time_gyr";
label_time_gyr.Size = new Size(13, 15);
label_time_gyr.TabIndex = 26;
label_time_gyr.Text = "0";
label_OF_Y.AutoSize = true;
label_OF_Y.Location = new Point(19, 45);
label_OF_Y.Name = "label_OF_Y";
label_OF_Y.Size = new Size(13, 15);
label_OF_Y.TabIndex = 7;
label_OF_Y.Text = "0";
//
// label17
//
label17.AutoSize = true;
label17.Location = new Point(6, 45);
label17.Name = "label17";
label17.Size = new Size(17, 15);
label17.TabIndex = 6;
label17.Text = "Y:";
//
// label_OF_X
//
label_OF_X.AutoSize = true;
label_OF_X.Location = new Point(19, 19);
label_OF_X.Name = "label_OF_X";
label_OF_X.Size = new Size(13, 15);
label_OF_X.TabIndex = 5;
label_OF_X.Text = "0";
//
// label19
//
label19.AutoSize = true;
label19.Location = new Point(6, 19);
label19.Name = "label19";
label19.Size = new Size(17, 15);
label19.TabIndex = 4;
label19.Text = "X:";
//
// Form_Main
//
AutoScaleDimensions = new SizeF(7F, 15F);
AutoScaleMode = AutoScaleMode.Font;
ClientSize = new Size(275, 447);
ClientSize = new Size(357, 447);
Controls.Add(groupBox5);
Controls.Add(button_MR);
Controls.Add(button_ML);
Controls.Add(label_Pow);
@ -501,6 +568,8 @@
groupBox4.ResumeLayout(false);
groupBox4.PerformLayout();
((System.ComponentModel.ISupportInitialize)trackBar_Power).EndInit();
groupBox5.ResumeLayout(false);
groupBox5.PerformLayout();
ResumeLayout(false);
PerformLayout();
}
@ -546,5 +615,11 @@
private Label label_time_acc;
private Label label_time_range;
private Label label_time_gyr;
private GroupBox groupBox5;
private Label label_time_of;
private Label label_OF_Y;
private Label label17;
private Label label_OF_X;
private Label label19;
}
}

7
DroneClient/FormMain.cs
View File

@ -1,4 +1,4 @@
using System.Text;
using System.Text;
using System.Numerics;
using System.Windows.Forms;
using static DroneSimulator.NetClient;
@ -109,6 +109,11 @@ namespace DroneSimulator
label_time_range.Text = dataDrone.TimeRange.ToString();
label_OF_X.Text = dataDrone.OF.X.ToString();
label_OF_Y.Text = dataDrone.OF.Y.ToString();
label_time_of.Text = dataDrone.TimeRange.ToString();
netClient.SendData(dataDrone.SendReqest());
}

40
DroneSimulator/Drone.cs
View File

@ -26,6 +26,8 @@ namespace DroneSimulator
public Vector4 Orientation;
public static bool[] Freeze = new bool[3];
private uint DataTimer;
private const float Gravity = 9.8f;
@ -36,13 +38,14 @@ namespace DroneSimulator
private Thread DroneThread;
private int Timer;
private Random MainRandom = new Random();
private Vector2 MoveOF = Vector2.Zero;
RealMode.Accelerometer RealAcc = new RealMode.Accelerometer();
RealMode.Gyroscope RealGyr = new RealMode.Gyroscope();
RealMode.Position RealPos = new RealMode.Position();
RealMode.Barometer RealBar = new RealMode.Barometer();
RealMode.Range RealRange = new RealMode.Range();
RealMode.OpticalFlow RealOF = new RealMode.OpticalFlow();
public static byte[] getBytes(object data)
{
@ -169,7 +172,7 @@ namespace DroneSimulator
{
Quaternion grav = new Quaternion(0, 0, 1, 0);
grav = (Quat * grav) * Quaternion.Inverse(Quat);
grav = Quat * grav * Quaternion.Inverse(Quat);
float yaw = 2 * MathF.Atan2(Quat.Z, Quat.W) * TO_GRAD;
if (yaw < 0.0f) yaw = 360.0f + yaw;
@ -201,6 +204,10 @@ namespace DroneSimulator
AccXYZ /= Gravity; // Вернуть измерения в G
if (Freeze[0]) { SpdXYZ.X = 0; PosXYZ.X = 0; }
if (Freeze[1]) { SpdXYZ.Y = 0; PosXYZ.Y = 0; }
if (Freeze[2]) { SpdXYZ.Z = 0; PosXYZ.Z = 5; }
if (PosXYZ.Z < 0)
{
SpdPRY = Vector3.Zero;
@ -241,23 +248,26 @@ namespace DroneSimulator
//Active = false; // Перевернулся вверх ногами
}
Quaternion grav = new Quaternion(AccXYZ.X, AccXYZ.Y, AccXYZ.Z, 0);
//grav = (Quat * grav) * Quaternion.Inverse(Quat); // Инерциальный акселерометр (тест)
Quaternion grav = Quat * new Quaternion(0, 0, flow/Mass, 0) * Quaternion.Inverse(Quat);
Acc = new Vector3(grav.X, grav.Y, grav.Z);
Gyr = SpdPRY;
float tilt = (MathF.Abs(ori.X) + MathF.Abs(ori.Y)) * TO_RADI;
float tilt = MathF.Sqrt((ori.X * ori.X) + (ori.Y * ori.Y)) * TO_RADI;
if (tilt < 90 && ori.W > 0) LaserRange = PosXYZ.Z / MathF.Cos(tilt);
else LaserRange = float.MaxValue;
}
MoveOF.X += SpdXYZ.X - Gyr.Y;
MoveOF.Y += SpdXYZ.Y + Gyr.X;
RealAcc.Update(Acc, (uint)tick);
RealGyr.Update(Gyr, (uint)tick);
RealRange.Update(LaserRange, (uint)tick);
RealBar.Update(PosXYZ.Z * 11, (uint)tick);
RealPos.Update(PosXYZ, (uint)tick);
RealOF.Update(MoveOF, LaserRange, (uint)tick);
DataTimer = (uint)tick;
}
@ -378,6 +388,24 @@ namespace DroneSimulator
return getBytes(bar);
}
private byte[] SendDataOF()
{
DroneData.DataOF of = new DroneData.DataOF();
of.Head.Size = Marshal.SizeOf(typeof(DroneData.DataOF));
of.Head.Mode = DroneData.DataMode.Response;
of.Head.Type = DroneData.DataType.DataOF;
of.Head.Time = (uint)Environment.TickCount;
of.X = RealOF.result.X;
of.Y = RealOF.result.Y;
of.Time = RealBar.timer;
MoveOF = Vector2.Zero;
return getBytes(of);
}
private byte[] SendDataQuaternion()
{
DroneData.DataQuat quat = new DroneData.DataQuat();
@ -424,6 +452,8 @@ namespace DroneSimulator
case DroneData.DataType.DataBar: if (head.Mode == DroneData.DataMode.Request) return SendDataBarometer(); else return zero;
case DroneData.DataType.DataOF: if (head.Mode == DroneData.DataMode.Request) return SendDataOF(); 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;

12
DroneSimulator/DroneData.cs
View File

@ -12,7 +12,7 @@ namespace DroneData
None = 0, Head = 1,
// Output
DataAcc = 1001, DataGyr = 1002, DataMag = 1003, DataRange = 1004, DataLocal = 1005, DataBar = 1006,
DataAcc = 1001, DataGyr = 1002, DataMag = 1003, DataRange = 1004, DataLocal = 1005, DataBar = 1006, DataOF = 1007,
// Input
DataMotor4 = 2001, DataMotor6 = 2002,
@ -95,6 +95,16 @@ namespace DroneData
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataBar));
}
public struct DataOF
{
public DataHead Head;
public float X, Y; // Угловой сдвиг
public uint Time; // Последнее время изменения данных
static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataOF));
}
public struct DataMotor4
{
public DataHead Head;

338
DroneSimulator/FormMain.Designer.cs generated
View File

@ -70,17 +70,17 @@
label46 = new Label();
label47 = new Label();
groupBox_OF = new GroupBox();
label53 = new Label();
numericUpDown17 = new NumericUpDown();
label54 = new Label();
numericUpDown8 = new NumericUpDown();
numericUpDown7 = new NumericUpDown();
label41 = new Label();
label40 = new Label();
numericUpDown_OF_Lens = new NumericUpDown();
label39 = new Label();
numericUpDown_OF_Wait = new NumericUpDown();
label53 = new Label();
numericUpDown_OF_Laten = new NumericUpDown();
label54 = new Label();
numericUpDown_OF_Error = new NumericUpDown();
label38 = new Label();
label37 = new Label();
numericUpDown1 = new NumericUpDown();
numericUpDown_OF_Len = new NumericUpDown();
checkBox_OF_Enable = new CheckBox();
label17 = new Label();
label16 = new Label();
@ -151,15 +151,21 @@
numericUpDown_Acc_Freq = new NumericUpDown();
label20 = new Label();
groupBox2 = new GroupBox();
checkBox_Mode_Real = new CheckBox();
radioButton1 = new RadioButton();
radioButton_Real = new RadioButton();
tabPage_Area = new TabPage();
groupBox_Navi = new GroupBox();
panel1 = new Panel();
listBox_Drones = new ListBox();
button_Drone_Color = new Button();
comboBox_Drone_Rotor = new ComboBox();
comboBox_Drone = new ComboBox();
timer_Test = new System.Windows.Forms.Timer(components);
listBox_Drones = new ListBox();
groupBox3 = new GroupBox();
checkBox_Area_Freeze_X = new CheckBox();
label41 = new Label();
checkBox_Area_Freeze_Y = new CheckBox();
checkBox_Area_Freeze_Z = new CheckBox();
menuStrip_Menu.SuspendLayout();
groupBox_Screen.SuspendLayout();
((System.ComponentModel.ISupportInitialize)pictureBox_2D).BeginInit();
@ -179,10 +185,11 @@
((System.ComponentModel.ISupportInitialize)numericUpDown_Range_Noise).BeginInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_Range_Freq).BeginInit();
groupBox_OF.SuspendLayout();
((System.ComponentModel.ISupportInitialize)numericUpDown17).BeginInit();
((System.ComponentModel.ISupportInitialize)numericUpDown8).BeginInit();
((System.ComponentModel.ISupportInitialize)numericUpDown7).BeginInit();
((System.ComponentModel.ISupportInitialize)numericUpDown1).BeginInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Lens).BeginInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Wait).BeginInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Laten).BeginInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Error).BeginInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Len).BeginInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Noise).BeginInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Freq).BeginInit();
groupBox_Barometer.SuspendLayout();
@ -210,8 +217,10 @@
((System.ComponentModel.ISupportInitialize)numericUpDown_Acc_Noise).BeginInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_Acc_Freq).BeginInit();
groupBox2.SuspendLayout();
tabPage_Area.SuspendLayout();
groupBox_Navi.SuspendLayout();
panel1.SuspendLayout();
groupBox3.SuspendLayout();
SuspendLayout();
//
// menuStrip_Menu
@ -506,7 +515,7 @@
groupBox1.Controls.Add(label46);
groupBox1.Controls.Add(label47);
groupBox1.Dock = DockStyle.Top;
groupBox1.Location = new Point(0, 749);
groupBox1.Location = new Point(0, 732);
groupBox1.Name = "groupBox1";
groupBox1.Size = new Size(204, 133);
groupBox1.TabIndex = 8;
@ -648,17 +657,17 @@
//
// groupBox_OF
//
groupBox_OF.Controls.Add(label53);
groupBox_OF.Controls.Add(numericUpDown17);
groupBox_OF.Controls.Add(label54);
groupBox_OF.Controls.Add(numericUpDown8);
groupBox_OF.Controls.Add(numericUpDown7);
groupBox_OF.Controls.Add(label41);
groupBox_OF.Controls.Add(label40);
groupBox_OF.Controls.Add(numericUpDown_OF_Lens);
groupBox_OF.Controls.Add(label39);
groupBox_OF.Controls.Add(numericUpDown_OF_Wait);
groupBox_OF.Controls.Add(label53);
groupBox_OF.Controls.Add(numericUpDown_OF_Laten);
groupBox_OF.Controls.Add(label54);
groupBox_OF.Controls.Add(numericUpDown_OF_Error);
groupBox_OF.Controls.Add(label38);
groupBox_OF.Controls.Add(label37);
groupBox_OF.Controls.Add(numericUpDown1);
groupBox_OF.Controls.Add(numericUpDown_OF_Len);
groupBox_OF.Controls.Add(checkBox_OF_Enable);
groupBox_OF.Controls.Add(label17);
groupBox_OF.Controls.Add(label16);
@ -669,95 +678,101 @@
groupBox_OF.Controls.Add(label14);
groupBox_OF.Controls.Add(label15);
groupBox_OF.Dock = DockStyle.Top;
groupBox_OF.Enabled = false;
groupBox_OF.Location = new Point(0, 563);
groupBox_OF.Name = "groupBox_OF";
groupBox_OF.Size = new Size(204, 186);
groupBox_OF.Size = new Size(204, 169);
groupBox_OF.TabIndex = 4;
groupBox_OF.TabStop = false;
groupBox_OF.Text = "Optical flow";
//
// label40
//
label40.AutoSize = true;
label40.Location = new Point(158, 18);
label40.Name = "label40";
label40.Size = new Size(28, 15);
label40.TabIndex = 36;
label40.Text = "lens";
//
// numericUpDown_OF_Lens
//
numericUpDown_OF_Lens.DecimalPlaces = 1;
numericUpDown_OF_Lens.Increment = new decimal(new int[] { 1, 0, 0, 65536 });
numericUpDown_OF_Lens.Location = new Point(153, 36);
numericUpDown_OF_Lens.Maximum = new decimal(new int[] { 10, 0, 0, 0 });
numericUpDown_OF_Lens.Minimum = new decimal(new int[] { 1, 0, 0, 65536 });
numericUpDown_OF_Lens.Name = "numericUpDown_OF_Lens";
numericUpDown_OF_Lens.Size = new Size(40, 23);
numericUpDown_OF_Lens.TabIndex = 35;
numericUpDown_OF_Lens.Value = new decimal(new int[] { 1, 0, 0, 0 });
numericUpDown_OF_Lens.ValueChanged += numericUpDown_OF_Update;
//
// label39
//
label39.AutoSize = true;
label39.Location = new Point(168, 111);
label39.Name = "label39";
label39.Size = new Size(24, 15);
label39.TabIndex = 34;
label39.Text = "sec";
//
// numericUpDown_OF_Wait
//
numericUpDown_OF_Wait.DecimalPlaces = 2;
numericUpDown_OF_Wait.Increment = new decimal(new int[] { 1, 0, 0, 65536 });
numericUpDown_OF_Wait.Location = new Point(119, 109);
numericUpDown_OF_Wait.Maximum = new decimal(new int[] { 1, 0, 0, 0 });
numericUpDown_OF_Wait.Name = "numericUpDown_OF_Wait";
numericUpDown_OF_Wait.Size = new Size(47, 23);
numericUpDown_OF_Wait.TabIndex = 33;
numericUpDown_OF_Wait.Value = new decimal(new int[] { 1, 0, 0, 65536 });
numericUpDown_OF_Wait.ValueChanged += numericUpDown_OF_Update;
//
// label53
//
label53.AutoSize = true;
label53.Location = new Point(113, 155);
label53.Location = new Point(113, 139);
label53.Name = "label53";
label53.Size = new Size(24, 15);
label53.TabIndex = 32;
label53.Text = "sec";
//
// numericUpDown17
// numericUpDown_OF_Laten
//
numericUpDown17.DecimalPlaces = 2;
numericUpDown17.Increment = new decimal(new int[] { 2, 0, 0, 131072 });
numericUpDown17.Location = new Point(66, 153);
numericUpDown17.Maximum = new decimal(new int[] { 1, 0, 0, 0 });
numericUpDown17.Name = "numericUpDown17";
numericUpDown17.Size = new Size(41, 23);
numericUpDown17.TabIndex = 31;
numericUpDown17.Value = new decimal(new int[] { 1, 0, 0, 65536 });
numericUpDown_OF_Laten.DecimalPlaces = 2;
numericUpDown_OF_Laten.Increment = new decimal(new int[] { 2, 0, 0, 131072 });
numericUpDown_OF_Laten.Location = new Point(66, 137);
numericUpDown_OF_Laten.Maximum = new decimal(new int[] { 1, 0, 0, 0 });
numericUpDown_OF_Laten.Name = "numericUpDown_OF_Laten";
numericUpDown_OF_Laten.Size = new Size(41, 23);
numericUpDown_OF_Laten.TabIndex = 31;
numericUpDown_OF_Laten.Value = new decimal(new int[] { 1, 0, 0, 65536 });
numericUpDown_OF_Laten.ValueChanged += numericUpDown_OF_Update;
//
// label54
//
label54.AutoSize = true;
label54.Location = new Point(3, 155);
label54.Location = new Point(3, 139);
label54.Name = "label54";
label54.Size = new Size(55, 15);
label54.TabIndex = 30;
label54.Text = "Lateness:";
//
// numericUpDown8
// numericUpDown_OF_Error
//
numericUpDown8.DecimalPlaces = 2;
numericUpDown8.Increment = new decimal(new int[] { 1, 0, 0, 65536 });
numericUpDown8.Location = new Point(126, 125);
numericUpDown8.Maximum = new decimal(new int[] { 10, 0, 0, 0 });
numericUpDown8.Name = "numericUpDown8";
numericUpDown8.Size = new Size(46, 23);
numericUpDown8.TabIndex = 26;
numericUpDown8.Value = new decimal(new int[] { 1, 0, 0, 65536 });
//
// numericUpDown7
//
numericUpDown7.DecimalPlaces = 1;
numericUpDown7.Increment = new decimal(new int[] { 1, 0, 0, 65536 });
numericUpDown7.Location = new Point(45, 125);
numericUpDown7.Name = "numericUpDown7";
numericUpDown7.Size = new Size(40, 23);
numericUpDown7.TabIndex = 24;
numericUpDown7.Value = new decimal(new int[] { 3, 0, 0, 0 });
//
// label41
//
label41.AutoSize = true;
label41.Location = new Point(171, 127);
label41.Name = "label41";
label41.Size = new Size(24, 15);
label41.TabIndex = 29;
label41.Text = "sec";
//
// label40
//
label40.AutoSize = true;
label40.Location = new Point(37, 107);
label40.Name = "label40";
label40.Size = new Size(64, 15);
label40.TabIndex = 28;
label40.Text = "probability";
//
// label39
//
label39.AutoSize = true;
label39.Location = new Point(135, 107);
label39.Name = "label39";
label39.Size = new Size(31, 15);
label39.TabIndex = 27;
label39.Text = "time";
numericUpDown_OF_Error.DecimalPlaces = 1;
numericUpDown_OF_Error.Increment = new decimal(new int[] { 1, 0, 0, 65536 });
numericUpDown_OF_Error.Location = new Point(38, 109);
numericUpDown_OF_Error.Name = "numericUpDown_OF_Error";
numericUpDown_OF_Error.Size = new Size(47, 23);
numericUpDown_OF_Error.TabIndex = 24;
numericUpDown_OF_Error.Value = new decimal(new int[] { 1, 0, 0, 0 });
numericUpDown_OF_Error.ValueChanged += numericUpDown_OF_Update;
//
// label38
//
label38.AutoSize = true;
label38.Location = new Point(84, 127);
label38.Location = new Point(83, 111);
label38.Name = "label38";
label38.Size = new Size(17, 15);
label38.TabIndex = 25;
@ -766,33 +781,35 @@
// label37
//
label37.AutoSize = true;
label37.Location = new Point(1, 127);
label37.Location = new Point(1, 111);
label37.Name = "label37";
label37.Size = new Size(35, 15);
label37.TabIndex = 23;
label37.Text = "Error:";
//
// numericUpDown1
// numericUpDown_OF_Len
//
numericUpDown1.Location = new Point(68, 80);
numericUpDown1.Minimum = new decimal(new int[] { 1, 0, 0, 0 });
numericUpDown1.Name = "numericUpDown1";
numericUpDown1.Size = new Size(40, 23);
numericUpDown1.TabIndex = 20;
numericUpDown1.Value = new decimal(new int[] { 8, 0, 0, 0 });
numericUpDown_OF_Len.Location = new Point(68, 80);
numericUpDown_OF_Len.Minimum = new decimal(new int[] { 1, 0, 0, 0 });
numericUpDown_OF_Len.Name = "numericUpDown_OF_Len";
numericUpDown_OF_Len.Size = new Size(40, 23);
numericUpDown_OF_Len.TabIndex = 20;
numericUpDown_OF_Len.Value = new decimal(new int[] { 8, 0, 0, 0 });
numericUpDown_OF_Len.ValueChanged += numericUpDown_OF_Update;
//
// checkBox_OF_Enable
//
checkBox_OF_Enable.AutoSize = true;
checkBox_OF_Enable.Checked = true;
checkBox_OF_Enable.CheckState = CheckState.Checked;
checkBox_OF_Enable.Location = new Point(159, 157);
checkBox_OF_Enable.Location = new Point(159, 141);
checkBox_OF_Enable.Name = "checkBox_OF_Enable";
checkBox_OF_Enable.Size = new Size(39, 19);
checkBox_OF_Enable.TabIndex = 22;
checkBox_OF_Enable.Tag = "#en";
checkBox_OF_Enable.Text = "En";
checkBox_OF_Enable.UseVisualStyleBackColor = true;
checkBox_OF_Enable.CheckedChanged += numericUpDown_OF_Update;
//
// label17
//
@ -811,7 +828,7 @@
label16.Size = new Size(70, 15);
label16.TabIndex = 19;
label16.Tag = "#max_height";
label16.Text = "Max height:";
label16.Text = "Max length:";
//
// label12
//
@ -831,6 +848,7 @@
numericUpDown_OF_Noise.Size = new Size(40, 23);
numericUpDown_OF_Noise.TabIndex = 17;
numericUpDown_OF_Noise.Value = new decimal(new int[] { 1, 0, 0, 0 });
numericUpDown_OF_Noise.ValueChanged += numericUpDown_OF_Update;
//
// label13
//
@ -851,6 +869,7 @@
numericUpDown_OF_Freq.Size = new Size(40, 23);
numericUpDown_OF_Freq.TabIndex = 13;
numericUpDown_OF_Freq.Value = new decimal(new int[] { 100, 0, 0, 0 });
numericUpDown_OF_Freq.ValueChanged += numericUpDown_OF_Update;
//
// label14
//
@ -1541,7 +1560,8 @@
//
// groupBox2
//
groupBox2.Controls.Add(checkBox_Mode_Real);
groupBox2.Controls.Add(radioButton1);
groupBox2.Controls.Add(radioButton_Real);
groupBox2.Dock = DockStyle.Top;
groupBox2.Location = new Point(0, 0);
groupBox2.Name = "groupBox2";
@ -1550,19 +1570,33 @@
groupBox2.TabStop = false;
groupBox2.Text = "Mode";
//
// checkBox_Mode_Real
// radioButton1
//
checkBox_Mode_Real.AutoSize = true;
checkBox_Mode_Real.Location = new Point(10, 22);
checkBox_Mode_Real.Name = "checkBox_Mode_Real";
checkBox_Mode_Real.Size = new Size(107, 19);
checkBox_Mode_Real.TabIndex = 0;
checkBox_Mode_Real.Text = "Real simulation";
checkBox_Mode_Real.UseVisualStyleBackColor = true;
checkBox_Mode_Real.CheckedChanged += checkBox_Mode_Real_CheckedChanged;
radioButton1.AutoSize = true;
radioButton1.Checked = true;
radioButton1.Location = new Point(9, 19);
radioButton1.Name = "radioButton1";
radioButton1.Size = new Size(50, 19);
radioButton1.TabIndex = 2;
radioButton1.TabStop = true;
radioButton1.Text = "Ideal";
radioButton1.UseVisualStyleBackColor = true;
radioButton1.CheckedChanged += checkBox_Mode_Real_CheckedChanged;
//
// radioButton_Real
//
radioButton_Real.AutoSize = true;
radioButton_Real.Location = new Point(150, 19);
radioButton_Real.Name = "radioButton_Real";
radioButton_Real.Size = new Size(47, 19);
radioButton_Real.TabIndex = 1;
radioButton_Real.Text = "Real";
radioButton_Real.UseVisualStyleBackColor = true;
radioButton_Real.CheckedChanged += checkBox_Mode_Real_CheckedChanged;
//
// tabPage_Area
//
tabPage_Area.Controls.Add(groupBox3);
tabPage_Area.Location = new Point(4, 24);
tabPage_Area.Name = "tabPage_Area";
tabPage_Area.Size = new Size(210, 889);
@ -1595,6 +1629,14 @@
panel1.Size = new Size(194, 872);
panel1.TabIndex = 3;
//
// listBox_Drones
//
listBox_Drones.FormattingEnabled = true;
listBox_Drones.Location = new Point(3, 45);
listBox_Drones.Name = "listBox_Drones";
listBox_Drones.Size = new Size(188, 214);
listBox_Drones.TabIndex = 3;
//
// button_Drone_Color
//
button_Drone_Color.BackColor = Color.Tomato;
@ -1629,13 +1671,61 @@
timer_Test.Interval = 10;
timer_Test.Tick += timer_Test_Tick;
//
// listBox_Drones
// groupBox3
//
listBox_Drones.FormattingEnabled = true;
listBox_Drones.Location = new Point(3, 45);
listBox_Drones.Name = "listBox_Drones";
listBox_Drones.Size = new Size(188, 214);
listBox_Drones.TabIndex = 3;
groupBox3.Controls.Add(checkBox_Area_Freeze_Z);
groupBox3.Controls.Add(checkBox_Area_Freeze_Y);
groupBox3.Controls.Add(label41);
groupBox3.Controls.Add(checkBox_Area_Freeze_X);
groupBox3.Dock = DockStyle.Top;
groupBox3.Location = new Point(0, 0);
groupBox3.Name = "groupBox3";
groupBox3.Size = new Size(210, 59);
groupBox3.TabIndex = 0;
groupBox3.TabStop = false;
groupBox3.Text = "Position";
//
// checkBox_Area_Freeze_X
//
checkBox_Area_Freeze_X.AutoSize = true;
checkBox_Area_Freeze_X.Location = new Point(55, 23);
checkBox_Area_Freeze_X.Name = "checkBox_Area_Freeze_X";
checkBox_Area_Freeze_X.Size = new Size(33, 19);
checkBox_Area_Freeze_X.TabIndex = 0;
checkBox_Area_Freeze_X.Text = "X";
checkBox_Area_Freeze_X.UseVisualStyleBackColor = true;
checkBox_Area_Freeze_X.CheckedChanged += checkBox_Area_Freeze_CheckedChanged;
//
// label41
//
label41.AutoSize = true;
label41.Location = new Point(6, 24);
label41.Name = "label41";
label41.Size = new Size(43, 15);
label41.TabIndex = 1;
label41.Text = "Freeze:";
//
// checkBox_Area_Freeze_Y
//
checkBox_Area_Freeze_Y.AutoSize = true;
checkBox_Area_Freeze_Y.Location = new Point(103, 23);
checkBox_Area_Freeze_Y.Name = "checkBox_Area_Freeze_Y";
checkBox_Area_Freeze_Y.Size = new Size(33, 19);
checkBox_Area_Freeze_Y.TabIndex = 2;
checkBox_Area_Freeze_Y.Text = "Y";
checkBox_Area_Freeze_Y.UseVisualStyleBackColor = true;
checkBox_Area_Freeze_Y.CheckedChanged += checkBox_Area_Freeze_CheckedChanged;
//
// checkBox_Area_Freeze_Z
//
checkBox_Area_Freeze_Z.AutoSize = true;
checkBox_Area_Freeze_Z.Location = new Point(151, 23);
checkBox_Area_Freeze_Z.Name = "checkBox_Area_Freeze_Z";
checkBox_Area_Freeze_Z.Size = new Size(33, 19);
checkBox_Area_Freeze_Z.TabIndex = 3;
checkBox_Area_Freeze_Z.Text = "Z";
checkBox_Area_Freeze_Z.UseVisualStyleBackColor = true;
checkBox_Area_Freeze_Z.CheckedChanged += checkBox_Area_Freeze_CheckedChanged;
//
// Form_Main
//
@ -1676,10 +1766,11 @@
((System.ComponentModel.ISupportInitialize)numericUpDown_Range_Freq).EndInit();
groupBox_OF.ResumeLayout(false);
groupBox_OF.PerformLayout();
((System.ComponentModel.ISupportInitialize)numericUpDown17).EndInit();
((System.ComponentModel.ISupportInitialize)numericUpDown8).EndInit();
((System.ComponentModel.ISupportInitialize)numericUpDown7).EndInit();
((System.ComponentModel.ISupportInitialize)numericUpDown1).EndInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Lens).EndInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Wait).EndInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Laten).EndInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Error).EndInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Len).EndInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Noise).EndInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_OF_Freq).EndInit();
groupBox_Barometer.ResumeLayout(false);
@ -1713,8 +1804,11 @@
((System.ComponentModel.ISupportInitialize)numericUpDown_Acc_Freq).EndInit();
groupBox2.ResumeLayout(false);
groupBox2.PerformLayout();
tabPage_Area.ResumeLayout(false);
groupBox_Navi.ResumeLayout(false);
panel1.ResumeLayout(false);
groupBox3.ResumeLayout(false);
groupBox3.PerformLayout();
ResumeLayout(false);
PerformLayout();
}
@ -1769,7 +1863,7 @@
private Label label15;
private CheckBox checkBox_Pos_Enable;
private Label label17;
private NumericUpDown numericUpDown1;
private NumericUpDown numericUpDown_OF_Len;
private Label label16;
private CheckBox checkBox_OF_Enable;
private CheckBox checkBox_Bar_Enable;
@ -1809,12 +1903,8 @@
private Label label34;
private Label label36;
private Label label35;
private NumericUpDown numericUpDown7;
private NumericUpDown numericUpDown_OF_Error;
private Label label37;
private Label label41;
private Label label40;
private Label label39;
private NumericUpDown numericUpDown8;
private Label label38;
private GroupBox groupBox1;
private CheckBox checkBox_Range_Enable;
@ -1843,13 +1933,23 @@
private NumericUpDown numericUpDown_Acc_Laten;
private Label label50;
private Label label53;
private NumericUpDown numericUpDown17;
private NumericUpDown numericUpDown_OF_Laten;
private Label label54;
private Label label55;
private NumericUpDown numericUpDown_Range_Laten;
private Label label56;
private GroupBox groupBox2;
private CheckBox checkBox_Mode_Real;
private ListBox listBox_Drones;
private Label label39;
private NumericUpDown numericUpDown_OF_Wait;
private Label label40;
private NumericUpDown numericUpDown_OF_Lens;
private RadioButton radioButton1;
private RadioButton radioButton_Real;
private GroupBox groupBox3;
private CheckBox checkBox_Area_Freeze_X;
private CheckBox checkBox_Area_Freeze_Z;
private CheckBox checkBox_Area_Freeze_Y;
private Label label41;
}
}

25
DroneSimulator/FormMain.cs
View File

@ -23,7 +23,7 @@ namespace DroneSimulator
{
InitializeComponent();
RealMode.RealSimulation = checkBox_Mode_Real.Checked;
RealMode.RealSimulation = radioButton_Real.Checked;
numericUpDown_Acc_Update(null, null);
numericUpDown_Gyr_Update(null, null);
numericUpDown_Pos_Update(null, null);
@ -236,7 +236,7 @@ namespace DroneSimulator
private void checkBox_Mode_Real_CheckedChanged(object sender, EventArgs e)
{
RealMode.RealSimulation = checkBox_Mode_Real.Checked;
RealMode.RealSimulation = radioButton_Real.Checked;
}
private void numericUpDown_Acc_Update(object sender, EventArgs e)
@ -277,5 +277,26 @@ namespace DroneSimulator
RealMode.Range.MaxHeight = (float)numericUpDown_Range_Max.Value;
}
private void numericUpDown_OF_Update(object sender, EventArgs e)
{
RealMode.OpticalFlow.Freq = (uint)numericUpDown_OF_Freq.Value;
RealMode.OpticalFlow.Noise = (float)numericUpDown_OF_Noise.Value;
RealMode.OpticalFlow.Lateness = (float)numericUpDown_OF_Laten.Value;
RealMode.OpticalFlow.Enable = checkBox_OF_Enable.Checked;
RealMode.OpticalFlow.Lens = (uint)numericUpDown_OF_Lens.Value * 10;
RealMode.OpticalFlow.MaxHeight = (float)numericUpDown_OF_Len.Value;
RealMode.OpticalFlow.Error = (float)numericUpDown_OF_Error.Value * 10;
RealMode.OpticalFlow.Wait = (uint)numericUpDown_OF_Wait.Value * 1000;
}
private void checkBox_Area_Freeze_CheckedChanged(object sender, EventArgs e)
{
Drone.Freeze[0] = checkBox_Area_Freeze_X.Checked;
Drone.Freeze[1] = checkBox_Area_Freeze_Y.Checked;
Drone.Freeze[2] = checkBox_Area_Freeze_Z.Checked;
}
}
}

77
DroneSimulator/RealMode.cs
View File

@ -4,6 +4,7 @@ using System.Linq;
using System.Text;
using System.Numerics;
using System.Reflection;
using static System.Windows.Forms.VisualStyles.VisualStyleElement.Rebar;
namespace DroneSimulator
{
@ -263,7 +264,83 @@ namespace DroneSimulator
internal class OpticalFlow
{
public static bool Enable;
public static float MaxHeight;
public static uint Freq;
public static float Noise;
public static float Lateness;
public static float Error;
public static uint Wait;
public static float Lens;
private uint last = 0;
private Random rand = new Random();
private const int count = 1000;
private Vector2[] laten = new Vector2[count];
private uint index = 0;
public uint delay = 0;
public uint timer = 0;
public Vector2 result;
public void Update(Vector2 value, float Range, uint time)
{
if (!RealSimulation)
{
result = value;
timer = time;
return;
}
if (!Enable)
{
result = Vector2.NaN;
return;
}
value *= Lens;
if (rand.Next(0, 1000) < (Error * 10))
{
value = Vector2.Zero;
delay = time + Wait;
}
else if (delay > time)
{
value = Vector2.Zero;
}
if (Range > MaxHeight) value = Vector2.Zero;
else
{
int noise = (int)(Noise * 1000);
value.X += ((float)rand.Next(-noise, noise)) / 1000;
value.Y += ((float)rand.Next(-noise, noise)) / 1000;
}
uint clock = (uint)(Lateness * 1000);
uint tick = time - last;
last = time;
while (tick != 0)
{
tick--;
laten[index++] = value;
if (index >= clock) index = 0;
}
value = laten[index];
uint freq = 1000 / Freq;
if (timer + freq < time)
{
result = value;
timer = time;
}
}
}
internal class Range