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-07-17 23:10:22 +03:00
parent d8ed10b4be
commit 9061794bf1
3 changed files with 152 additions and 130 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
DroneSimulator/Drone.cs
View File

@ -233,6 +233,9 @@ namespace DroneSimulator
return new Vector4(GetAngle(grav.Y, grav.X, grav.Z), GetAngle(-grav.X, grav.Y, grav.Z), yaw, grav.Z); return new Vector4(GetAngle(grav.Y, grav.X, grav.Z), GetAngle(-grav.X, grav.Y, grav.Z), yaw, grav.Z);
} }
int TestGyr = 0;
int TestDir = 1;
public void Action(uint tick) public void Action(uint tick)
{ {
uint period = tick - Timer; uint period = tick - Timer;

136
DroneSimulator/FormMain.Designer.cs generated
View File

@ -115,11 +115,12 @@
label48 = new Label(); label48 = new Label();
checkBox_Pos_Enable = new CheckBox(); checkBox_Pos_Enable = new CheckBox();
groupBox_Mag = new GroupBox(); groupBox_Mag = new GroupBox();
numericUpDown_Mag_Freq = new NumericUpDown();
numericUpDown_Mag_Noise = new NumericUpDown(); numericUpDown_Mag_Noise = new NumericUpDown();
label28 = new Label(); label28 = new Label();
numericUpDown_Mag_Freq = new NumericUpDown();
label22 = new Label(); label22 = new Label();
groupBox_Gyr = new GroupBox(); groupBox_Gyr = new GroupBox();
numericUpDown_Gyr_Freq = new NumericUpDown();
checkBox_Model_Gyr_Real = new CheckBox(); checkBox_Model_Gyr_Real = new CheckBox();
label51 = new Label(); label51 = new Label();
numericUpDown_Gyr_Laten = new NumericUpDown(); numericUpDown_Gyr_Laten = new NumericUpDown();
@ -134,9 +135,9 @@
numericUpDown_Gyr_Noise = new NumericUpDown(); numericUpDown_Gyr_Noise = new NumericUpDown();
label25 = new Label(); label25 = new Label();
label26 = new Label(); label26 = new Label();
numericUpDown_Gyr_Freq = new NumericUpDown();
label21 = new Label(); label21 = new Label();
groupBox_Acc = new GroupBox(); groupBox_Acc = new GroupBox();
numericUpDown_Acc_Freq = new NumericUpDown();
checkBox_Model_Acc_Real = new CheckBox(); checkBox_Model_Acc_Real = new CheckBox();
label49 = new Label(); label49 = new Label();
numericUpDown_Acc_Laten = new NumericUpDown(); numericUpDown_Acc_Laten = new NumericUpDown();
@ -148,7 +149,6 @@
label24 = new Label(); label24 = new Label();
numericUpDown_Acc_Noise = new NumericUpDown(); numericUpDown_Acc_Noise = new NumericUpDown();
label23 = new Label(); label23 = new Label();
numericUpDown_Acc_Freq = new NumericUpDown();
label20 = new Label(); label20 = new Label();
tabPage_Area = new TabPage(); tabPage_Area = new TabPage();
groupBox4 = new GroupBox(); groupBox4 = new GroupBox();
@ -255,21 +255,21 @@
((System.ComponentModel.ISupportInitialize)numericUpDown_Pos_Noise).BeginInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_Pos_Noise).BeginInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_Pos_Freq).BeginInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_Pos_Freq).BeginInit();
groupBox_Mag.SuspendLayout(); groupBox_Mag.SuspendLayout();
((System.ComponentModel.ISupportInitialize)numericUpDown_Mag_Noise).BeginInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_Mag_Freq).BeginInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_Mag_Freq).BeginInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_Mag_Noise).BeginInit();
groupBox_Gyr.SuspendLayout(); groupBox_Gyr.SuspendLayout();
((System.ComponentModel.ISupportInitialize)numericUpDown_Gyr_Freq).BeginInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_Gyr_Laten).BeginInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_Gyr_Laten).BeginInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_Gyr_Shift_Z).BeginInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_Gyr_Shift_Z).BeginInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_Gyr_Shift_Y).BeginInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_Gyr_Shift_Y).BeginInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_Gyr_Shift_X).BeginInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_Gyr_Shift_X).BeginInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_Gyr_Noise).BeginInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_Gyr_Noise).BeginInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_Gyr_Freq).BeginInit();
groupBox_Acc.SuspendLayout(); groupBox_Acc.SuspendLayout();
((System.ComponentModel.ISupportInitialize)numericUpDown_Acc_Freq).BeginInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_Acc_Laten).BeginInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_Acc_Laten).BeginInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_Acc_Scale_Left).BeginInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_Acc_Scale_Left).BeginInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_Acc_Scale_Rigth).BeginInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_Acc_Scale_Rigth).BeginInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_Acc_Noise).BeginInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_Acc_Noise).BeginInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_Acc_Freq).BeginInit();
tabPage_Area.SuspendLayout(); tabPage_Area.SuspendLayout();
groupBox4.SuspendLayout(); groupBox4.SuspendLayout();
((System.ComponentModel.ISupportInitialize)numericUpDown_Area_Wind_RotResist).BeginInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_Area_Wind_RotResist).BeginInit();
@ -628,7 +628,7 @@
// label55 // label55
// //
label55.AutoSize = true; label55.AutoSize = true;
label55.Location = new Point(115, 100); label55.Location = new Point(119, 100);
label55.Name = "label55"; label55.Name = "label55";
label55.Size = new Size(24, 15); label55.Size = new Size(24, 15);
label55.TabIndex = 35; label55.TabIndex = 35;
@ -639,9 +639,9 @@
numericUpDown_Range_Laten.DecimalPlaces = 2; numericUpDown_Range_Laten.DecimalPlaces = 2;
numericUpDown_Range_Laten.Increment = new decimal(new int[] { 2, 0, 0, 131072 }); numericUpDown_Range_Laten.Increment = new decimal(new int[] { 2, 0, 0, 131072 });
numericUpDown_Range_Laten.Location = new Point(68, 98); numericUpDown_Range_Laten.Location = new Point(68, 98);
numericUpDown_Range_Laten.Maximum = new decimal(new int[] { 1, 0, 0, 0 }); numericUpDown_Range_Laten.Maximum = new decimal(new int[] { 99, 0, 0, 131072 });
numericUpDown_Range_Laten.Name = "numericUpDown_Range_Laten"; numericUpDown_Range_Laten.Name = "numericUpDown_Range_Laten";
numericUpDown_Range_Laten.Size = new Size(41, 23); numericUpDown_Range_Laten.Size = new Size(49, 23);
numericUpDown_Range_Laten.TabIndex = 34; numericUpDown_Range_Laten.TabIndex = 34;
numericUpDown_Range_Laten.Value = new decimal(new int[] { 1, 0, 0, 65536 }); numericUpDown_Range_Laten.Value = new decimal(new int[] { 1, 0, 0, 65536 });
numericUpDown_Range_Laten.ValueChanged += numericUpDown_Range_Update; numericUpDown_Range_Laten.ValueChanged += numericUpDown_Range_Update;
@ -734,7 +734,7 @@
numericUpDown_Range_Freq.Maximum = new decimal(new int[] { 1000, 0, 0, 0 }); numericUpDown_Range_Freq.Maximum = new decimal(new int[] { 1000, 0, 0, 0 });
numericUpDown_Range_Freq.Minimum = new decimal(new int[] { 1, 0, 0, 0 }); numericUpDown_Range_Freq.Minimum = new decimal(new int[] { 1, 0, 0, 0 });
numericUpDown_Range_Freq.Name = "numericUpDown_Range_Freq"; numericUpDown_Range_Freq.Name = "numericUpDown_Range_Freq";
numericUpDown_Range_Freq.Size = new Size(40, 23); numericUpDown_Range_Freq.Size = new Size(47, 23);
numericUpDown_Range_Freq.TabIndex = 22; numericUpDown_Range_Freq.TabIndex = 22;
numericUpDown_Range_Freq.Value = new decimal(new int[] { 50, 0, 0, 0 }); numericUpDown_Range_Freq.Value = new decimal(new int[] { 50, 0, 0, 0 });
numericUpDown_Range_Freq.ValueChanged += numericUpDown_Range_Update; numericUpDown_Range_Freq.ValueChanged += numericUpDown_Range_Update;
@ -742,7 +742,7 @@
// label46 // label46
// //
label46.AutoSize = true; label46.AutoSize = true;
label46.Location = new Point(113, 18); label46.Location = new Point(119, 18);
label46.Name = "label46"; label46.Name = "label46";
label46.Size = new Size(21, 15); label46.Size = new Size(21, 15);
label46.TabIndex = 24; label46.TabIndex = 24;
@ -821,7 +821,7 @@
// label53 // label53
// //
label53.AutoSize = true; label53.AutoSize = true;
label53.Location = new Point(114, 107); label53.Location = new Point(119, 107);
label53.Name = "label53"; label53.Name = "label53";
label53.Size = new Size(24, 15); label53.Size = new Size(24, 15);
label53.TabIndex = 32; label53.TabIndex = 32;
@ -832,9 +832,9 @@
numericUpDown_OF_Laten.DecimalPlaces = 2; numericUpDown_OF_Laten.DecimalPlaces = 2;
numericUpDown_OF_Laten.Increment = new decimal(new int[] { 2, 0, 0, 131072 }); numericUpDown_OF_Laten.Increment = new decimal(new int[] { 2, 0, 0, 131072 });
numericUpDown_OF_Laten.Location = new Point(67, 105); numericUpDown_OF_Laten.Location = new Point(67, 105);
numericUpDown_OF_Laten.Maximum = new decimal(new int[] { 1, 0, 0, 0 }); numericUpDown_OF_Laten.Maximum = new decimal(new int[] { 99, 0, 0, 131072 });
numericUpDown_OF_Laten.Name = "numericUpDown_OF_Laten"; numericUpDown_OF_Laten.Name = "numericUpDown_OF_Laten";
numericUpDown_OF_Laten.Size = new Size(41, 23); numericUpDown_OF_Laten.Size = new Size(50, 23);
numericUpDown_OF_Laten.TabIndex = 31; numericUpDown_OF_Laten.TabIndex = 31;
numericUpDown_OF_Laten.Value = new decimal(new int[] { 1, 0, 0, 65536 }); numericUpDown_OF_Laten.Value = new decimal(new int[] { 1, 0, 0, 65536 });
numericUpDown_OF_Laten.ValueChanged += numericUpDown_OF_Update; numericUpDown_OF_Laten.ValueChanged += numericUpDown_OF_Update;
@ -927,7 +927,7 @@
numericUpDown_OF_Freq.Maximum = new decimal(new int[] { 1000, 0, 0, 0 }); numericUpDown_OF_Freq.Maximum = new decimal(new int[] { 1000, 0, 0, 0 });
numericUpDown_OF_Freq.Minimum = new decimal(new int[] { 1, 0, 0, 0 }); numericUpDown_OF_Freq.Minimum = new decimal(new int[] { 1, 0, 0, 0 });
numericUpDown_OF_Freq.Name = "numericUpDown_OF_Freq"; numericUpDown_OF_Freq.Name = "numericUpDown_OF_Freq";
numericUpDown_OF_Freq.Size = new Size(40, 23); numericUpDown_OF_Freq.Size = new Size(48, 23);
numericUpDown_OF_Freq.TabIndex = 13; numericUpDown_OF_Freq.TabIndex = 13;
numericUpDown_OF_Freq.Value = new decimal(new int[] { 100, 0, 0, 0 }); numericUpDown_OF_Freq.Value = new decimal(new int[] { 100, 0, 0, 0 });
numericUpDown_OF_Freq.ValueChanged += numericUpDown_OF_Update; numericUpDown_OF_Freq.ValueChanged += numericUpDown_OF_Update;
@ -935,7 +935,7 @@
// label14 // label14
// //
label14.AutoSize = true; label14.AutoSize = true;
label14.Location = new Point(112, 24); label14.Location = new Point(119, 24);
label14.Name = "label14"; label14.Name = "label14";
label14.Size = new Size(21, 15); label14.Size = new Size(21, 15);
label14.TabIndex = 15; label14.TabIndex = 15;
@ -991,7 +991,7 @@
// label30 // label30
// //
label30.AutoSize = true; label30.AutoSize = true;
label30.Location = new Point(115, 98); label30.Location = new Point(119, 98);
label30.Name = "label30"; label30.Name = "label30";
label30.Size = new Size(24, 15); label30.Size = new Size(24, 15);
label30.TabIndex = 23; label30.TabIndex = 23;
@ -1002,9 +1002,9 @@
numericUpDown_Bar_Laten.DecimalPlaces = 2; numericUpDown_Bar_Laten.DecimalPlaces = 2;
numericUpDown_Bar_Laten.Increment = new decimal(new int[] { 2, 0, 0, 131072 }); numericUpDown_Bar_Laten.Increment = new decimal(new int[] { 2, 0, 0, 131072 });
numericUpDown_Bar_Laten.Location = new Point(68, 96); numericUpDown_Bar_Laten.Location = new Point(68, 96);
numericUpDown_Bar_Laten.Maximum = new decimal(new int[] { 1, 0, 0, 0 }); numericUpDown_Bar_Laten.Maximum = new decimal(new int[] { 99, 0, 0, 131072 });
numericUpDown_Bar_Laten.Name = "numericUpDown_Bar_Laten"; numericUpDown_Bar_Laten.Name = "numericUpDown_Bar_Laten";
numericUpDown_Bar_Laten.Size = new Size(41, 23); numericUpDown_Bar_Laten.Size = new Size(49, 23);
numericUpDown_Bar_Laten.TabIndex = 22; numericUpDown_Bar_Laten.TabIndex = 22;
numericUpDown_Bar_Laten.Value = new decimal(new int[] { 3, 0, 0, 65536 }); numericUpDown_Bar_Laten.Value = new decimal(new int[] { 3, 0, 0, 65536 });
numericUpDown_Bar_Laten.ValueChanged += numericUpDown_Bar_Update; numericUpDown_Bar_Laten.ValueChanged += numericUpDown_Bar_Update;
@ -1096,7 +1096,7 @@
numericUpDown_Bar_Freq.Maximum = new decimal(new int[] { 1000, 0, 0, 0 }); numericUpDown_Bar_Freq.Maximum = new decimal(new int[] { 1000, 0, 0, 0 });
numericUpDown_Bar_Freq.Minimum = new decimal(new int[] { 1, 0, 0, 0 }); numericUpDown_Bar_Freq.Minimum = new decimal(new int[] { 1, 0, 0, 0 });
numericUpDown_Bar_Freq.Name = "numericUpDown_Bar_Freq"; numericUpDown_Bar_Freq.Name = "numericUpDown_Bar_Freq";
numericUpDown_Bar_Freq.Size = new Size(40, 23); numericUpDown_Bar_Freq.Size = new Size(49, 23);
numericUpDown_Bar_Freq.TabIndex = 7; numericUpDown_Bar_Freq.TabIndex = 7;
numericUpDown_Bar_Freq.Value = new decimal(new int[] { 50, 0, 0, 0 }); numericUpDown_Bar_Freq.Value = new decimal(new int[] { 50, 0, 0, 0 });
numericUpDown_Bar_Freq.ValueChanged += numericUpDown_Bar_Update; numericUpDown_Bar_Freq.ValueChanged += numericUpDown_Bar_Update;
@ -1104,7 +1104,7 @@
// label5 // label5
// //
label5.AutoSize = true; label5.AutoSize = true;
label5.Location = new Point(110, 44); label5.Location = new Point(119, 44);
label5.Name = "label5"; label5.Name = "label5";
label5.Size = new Size(21, 15); label5.Size = new Size(21, 15);
label5.TabIndex = 9; label5.TabIndex = 9;
@ -1156,7 +1156,7 @@
// label2 // label2
// //
label2.AutoSize = true; label2.AutoSize = true;
label2.Location = new Point(116, 72); label2.Location = new Point(119, 74);
label2.Name = "label2"; label2.Name = "label2";
label2.Size = new Size(24, 15); label2.Size = new Size(24, 15);
label2.TabIndex = 32; label2.TabIndex = 32;
@ -1167,9 +1167,9 @@
numericUpDown_Pos_Laten.DecimalPlaces = 2; numericUpDown_Pos_Laten.DecimalPlaces = 2;
numericUpDown_Pos_Laten.Increment = new decimal(new int[] { 2, 0, 0, 131072 }); numericUpDown_Pos_Laten.Increment = new decimal(new int[] { 2, 0, 0, 131072 });
numericUpDown_Pos_Laten.Location = new Point(69, 70); numericUpDown_Pos_Laten.Location = new Point(69, 70);
numericUpDown_Pos_Laten.Maximum = new decimal(new int[] { 1, 0, 0, 0 }); numericUpDown_Pos_Laten.Maximum = new decimal(new int[] { 99, 0, 0, 131072 });
numericUpDown_Pos_Laten.Name = "numericUpDown_Pos_Laten"; numericUpDown_Pos_Laten.Name = "numericUpDown_Pos_Laten";
numericUpDown_Pos_Laten.Size = new Size(41, 23); numericUpDown_Pos_Laten.Size = new Size(48, 23);
numericUpDown_Pos_Laten.TabIndex = 31; numericUpDown_Pos_Laten.TabIndex = 31;
numericUpDown_Pos_Laten.Value = new decimal(new int[] { 3, 0, 0, 65536 }); numericUpDown_Pos_Laten.Value = new decimal(new int[] { 3, 0, 0, 65536 });
numericUpDown_Pos_Laten.ValueChanged += numericUpDown_Pos_Update; numericUpDown_Pos_Laten.ValueChanged += numericUpDown_Pos_Update;
@ -1220,7 +1220,7 @@
numericUpDown_Pos_Freq.Maximum = new decimal(new int[] { 1000, 0, 0, 0 }); numericUpDown_Pos_Freq.Maximum = new decimal(new int[] { 1000, 0, 0, 0 });
numericUpDown_Pos_Freq.Minimum = new decimal(new int[] { 1, 0, 0, 0 }); numericUpDown_Pos_Freq.Minimum = new decimal(new int[] { 1, 0, 0, 0 });
numericUpDown_Pos_Freq.Name = "numericUpDown_Pos_Freq"; numericUpDown_Pos_Freq.Name = "numericUpDown_Pos_Freq";
numericUpDown_Pos_Freq.Size = new Size(40, 23); numericUpDown_Pos_Freq.Size = new Size(48, 23);
numericUpDown_Pos_Freq.TabIndex = 24; numericUpDown_Pos_Freq.TabIndex = 24;
numericUpDown_Pos_Freq.Value = new decimal(new int[] { 50, 0, 0, 0 }); numericUpDown_Pos_Freq.Value = new decimal(new int[] { 50, 0, 0, 0 });
numericUpDown_Pos_Freq.ValueChanged += numericUpDown_Pos_Update; numericUpDown_Pos_Freq.ValueChanged += numericUpDown_Pos_Update;
@ -1228,7 +1228,7 @@
// label11 // label11
// //
label11.AutoSize = true; label11.AutoSize = true;
label11.Location = new Point(111, 19); label11.Location = new Point(119, 19);
label11.Name = "label11"; label11.Name = "label11";
label11.Size = new Size(21, 15); label11.Size = new Size(21, 15);
label11.TabIndex = 26; label11.TabIndex = 26;
@ -1260,9 +1260,9 @@
// //
// groupBox_Mag // groupBox_Mag
// //
groupBox_Mag.Controls.Add(numericUpDown_Mag_Freq);
groupBox_Mag.Controls.Add(numericUpDown_Mag_Noise); groupBox_Mag.Controls.Add(numericUpDown_Mag_Noise);
groupBox_Mag.Controls.Add(label28); groupBox_Mag.Controls.Add(label28);
groupBox_Mag.Controls.Add(numericUpDown_Mag_Freq);
groupBox_Mag.Controls.Add(label22); groupBox_Mag.Controls.Add(label22);
groupBox_Mag.Dock = DockStyle.Top; groupBox_Mag.Dock = DockStyle.Top;
groupBox_Mag.Location = new Point(0, 220); groupBox_Mag.Location = new Point(0, 220);
@ -1272,6 +1272,16 @@
groupBox_Mag.TabStop = false; groupBox_Mag.TabStop = false;
groupBox_Mag.Text = "Magnetometer"; groupBox_Mag.Text = "Magnetometer";
// //
// numericUpDown_Mag_Freq
//
numericUpDown_Mag_Freq.Location = new Point(37, 18);
numericUpDown_Mag_Freq.Maximum = new decimal(new int[] { 1000, 0, 0, 0 });
numericUpDown_Mag_Freq.Minimum = new decimal(new int[] { 1, 0, 0, 0 });
numericUpDown_Mag_Freq.Name = "numericUpDown_Mag_Freq";
numericUpDown_Mag_Freq.Size = new Size(48, 23);
numericUpDown_Mag_Freq.TabIndex = 10;
numericUpDown_Mag_Freq.Value = new decimal(new int[] { 200, 0, 0, 0 });
//
// numericUpDown_Mag_Noise // numericUpDown_Mag_Noise
// //
numericUpDown_Mag_Noise.DecimalPlaces = 1; numericUpDown_Mag_Noise.DecimalPlaces = 1;
@ -1289,16 +1299,6 @@
label28.TabIndex = 12; label28.TabIndex = 12;
label28.Text = "Noise:"; label28.Text = "Noise:";
// //
// numericUpDown_Mag_Freq
//
numericUpDown_Mag_Freq.Location = new Point(37, 18);
numericUpDown_Mag_Freq.Maximum = new decimal(new int[] { 1000, 0, 0, 0 });
numericUpDown_Mag_Freq.Minimum = new decimal(new int[] { 1, 0, 0, 0 });
numericUpDown_Mag_Freq.Name = "numericUpDown_Mag_Freq";
numericUpDown_Mag_Freq.Size = new Size(40, 23);
numericUpDown_Mag_Freq.TabIndex = 10;
numericUpDown_Mag_Freq.Value = new decimal(new int[] { 200, 0, 0, 0 });
//
// label22 // label22
// //
label22.AutoSize = true; label22.AutoSize = true;
@ -1310,6 +1310,7 @@
// //
// groupBox_Gyr // groupBox_Gyr
// //
groupBox_Gyr.Controls.Add(numericUpDown_Gyr_Freq);
groupBox_Gyr.Controls.Add(checkBox_Model_Gyr_Real); groupBox_Gyr.Controls.Add(checkBox_Model_Gyr_Real);
groupBox_Gyr.Controls.Add(label51); groupBox_Gyr.Controls.Add(label51);
groupBox_Gyr.Controls.Add(numericUpDown_Gyr_Laten); groupBox_Gyr.Controls.Add(numericUpDown_Gyr_Laten);
@ -1324,7 +1325,6 @@
groupBox_Gyr.Controls.Add(numericUpDown_Gyr_Noise); groupBox_Gyr.Controls.Add(numericUpDown_Gyr_Noise);
groupBox_Gyr.Controls.Add(label25); groupBox_Gyr.Controls.Add(label25);
groupBox_Gyr.Controls.Add(label26); groupBox_Gyr.Controls.Add(label26);
groupBox_Gyr.Controls.Add(numericUpDown_Gyr_Freq);
groupBox_Gyr.Controls.Add(label21); groupBox_Gyr.Controls.Add(label21);
groupBox_Gyr.Dock = DockStyle.Top; groupBox_Gyr.Dock = DockStyle.Top;
groupBox_Gyr.Location = new Point(0, 104); groupBox_Gyr.Location = new Point(0, 104);
@ -1334,6 +1334,17 @@
groupBox_Gyr.TabStop = false; groupBox_Gyr.TabStop = false;
groupBox_Gyr.Text = "Gyroscope"; groupBox_Gyr.Text = "Gyroscope";
// //
// numericUpDown_Gyr_Freq
//
numericUpDown_Gyr_Freq.Location = new Point(37, 19);
numericUpDown_Gyr_Freq.Maximum = new decimal(new int[] { 1000, 0, 0, 0 });
numericUpDown_Gyr_Freq.Minimum = new decimal(new int[] { 1, 0, 0, 0 });
numericUpDown_Gyr_Freq.Name = "numericUpDown_Gyr_Freq";
numericUpDown_Gyr_Freq.Size = new Size(48, 23);
numericUpDown_Gyr_Freq.TabIndex = 10;
numericUpDown_Gyr_Freq.Value = new decimal(new int[] { 200, 0, 0, 0 });
numericUpDown_Gyr_Freq.ValueChanged += numericUpDown_Gyr_Update;
//
// checkBox_Model_Gyr_Real // checkBox_Model_Gyr_Real
// //
checkBox_Model_Gyr_Real.AutoSize = true; checkBox_Model_Gyr_Real.AutoSize = true;
@ -1349,7 +1360,7 @@
// label51 // label51
// //
label51.AutoSize = true; label51.AutoSize = true;
label51.Location = new Point(126, 90); label51.Location = new Point(119, 90);
label51.Name = "label51"; label51.Name = "label51";
label51.Size = new Size(24, 15); label51.Size = new Size(24, 15);
label51.TabIndex = 35; label51.TabIndex = 35;
@ -1360,7 +1371,7 @@
numericUpDown_Gyr_Laten.DecimalPlaces = 3; numericUpDown_Gyr_Laten.DecimalPlaces = 3;
numericUpDown_Gyr_Laten.Increment = new decimal(new int[] { 2, 0, 0, 131072 }); numericUpDown_Gyr_Laten.Increment = new decimal(new int[] { 2, 0, 0, 131072 });
numericUpDown_Gyr_Laten.Location = new Point(68, 88); numericUpDown_Gyr_Laten.Location = new Point(68, 88);
numericUpDown_Gyr_Laten.Maximum = new decimal(new int[] { 1, 0, 0, 0 }); numericUpDown_Gyr_Laten.Maximum = new decimal(new int[] { 999, 0, 0, 196608 });
numericUpDown_Gyr_Laten.Name = "numericUpDown_Gyr_Laten"; numericUpDown_Gyr_Laten.Name = "numericUpDown_Gyr_Laten";
numericUpDown_Gyr_Laten.Size = new Size(49, 23); numericUpDown_Gyr_Laten.Size = new Size(49, 23);
numericUpDown_Gyr_Laten.TabIndex = 34; numericUpDown_Gyr_Laten.TabIndex = 34;
@ -1481,17 +1492,6 @@
label26.TabIndex = 12; label26.TabIndex = 12;
label26.Text = "Noise:"; label26.Text = "Noise:";
// //
// numericUpDown_Gyr_Freq
//
numericUpDown_Gyr_Freq.Location = new Point(37, 19);
numericUpDown_Gyr_Freq.Maximum = new decimal(new int[] { 1000, 0, 0, 0 });
numericUpDown_Gyr_Freq.Minimum = new decimal(new int[] { 1, 0, 0, 0 });
numericUpDown_Gyr_Freq.Name = "numericUpDown_Gyr_Freq";
numericUpDown_Gyr_Freq.Size = new Size(40, 23);
numericUpDown_Gyr_Freq.TabIndex = 10;
numericUpDown_Gyr_Freq.Value = new decimal(new int[] { 200, 0, 0, 0 });
numericUpDown_Gyr_Freq.ValueChanged += numericUpDown_Gyr_Update;
//
// label21 // label21
// //
label21.AutoSize = true; label21.AutoSize = true;
@ -1503,6 +1503,7 @@
// //
// groupBox_Acc // groupBox_Acc
// //
groupBox_Acc.Controls.Add(numericUpDown_Acc_Freq);
groupBox_Acc.Controls.Add(checkBox_Model_Acc_Real); groupBox_Acc.Controls.Add(checkBox_Model_Acc_Real);
groupBox_Acc.Controls.Add(label49); groupBox_Acc.Controls.Add(label49);
groupBox_Acc.Controls.Add(numericUpDown_Acc_Laten); groupBox_Acc.Controls.Add(numericUpDown_Acc_Laten);
@ -1514,7 +1515,6 @@
groupBox_Acc.Controls.Add(label24); groupBox_Acc.Controls.Add(label24);
groupBox_Acc.Controls.Add(numericUpDown_Acc_Noise); groupBox_Acc.Controls.Add(numericUpDown_Acc_Noise);
groupBox_Acc.Controls.Add(label23); groupBox_Acc.Controls.Add(label23);
groupBox_Acc.Controls.Add(numericUpDown_Acc_Freq);
groupBox_Acc.Controls.Add(label20); groupBox_Acc.Controls.Add(label20);
groupBox_Acc.Dock = DockStyle.Top; groupBox_Acc.Dock = DockStyle.Top;
groupBox_Acc.Location = new Point(0, 0); groupBox_Acc.Location = new Point(0, 0);
@ -1524,6 +1524,17 @@
groupBox_Acc.TabStop = false; groupBox_Acc.TabStop = false;
groupBox_Acc.Text = "Accelerometer"; groupBox_Acc.Text = "Accelerometer";
// //
// numericUpDown_Acc_Freq
//
numericUpDown_Acc_Freq.Location = new Point(37, 16);
numericUpDown_Acc_Freq.Maximum = new decimal(new int[] { 1000, 0, 0, 0 });
numericUpDown_Acc_Freq.Minimum = new decimal(new int[] { 1, 0, 0, 0 });
numericUpDown_Acc_Freq.Name = "numericUpDown_Acc_Freq";
numericUpDown_Acc_Freq.Size = new Size(48, 23);
numericUpDown_Acc_Freq.TabIndex = 8;
numericUpDown_Acc_Freq.Value = new decimal(new int[] { 200, 0, 0, 0 });
numericUpDown_Acc_Freq.ValueChanged += numericUpDown_Acc_Update;
//
// checkBox_Model_Acc_Real // checkBox_Model_Acc_Real
// //
checkBox_Model_Acc_Real.AutoSize = true; checkBox_Model_Acc_Real.AutoSize = true;
@ -1539,7 +1550,7 @@
// label49 // label49
// //
label49.AutoSize = true; label49.AutoSize = true;
label49.Location = new Point(122, 72); label49.Location = new Point(119, 72);
label49.Name = "label49"; label49.Name = "label49";
label49.Size = new Size(24, 15); label49.Size = new Size(24, 15);
label49.TabIndex = 35; label49.TabIndex = 35;
@ -1550,7 +1561,7 @@
numericUpDown_Acc_Laten.DecimalPlaces = 3; numericUpDown_Acc_Laten.DecimalPlaces = 3;
numericUpDown_Acc_Laten.Increment = new decimal(new int[] { 1, 0, 0, 131072 }); numericUpDown_Acc_Laten.Increment = new decimal(new int[] { 1, 0, 0, 131072 });
numericUpDown_Acc_Laten.Location = new Point(68, 70); numericUpDown_Acc_Laten.Location = new Point(68, 70);
numericUpDown_Acc_Laten.Maximum = new decimal(new int[] { 1, 0, 0, 0 }); numericUpDown_Acc_Laten.Maximum = new decimal(new int[] { 999, 0, 0, 196608 });
numericUpDown_Acc_Laten.Name = "numericUpDown_Acc_Laten"; numericUpDown_Acc_Laten.Name = "numericUpDown_Acc_Laten";
numericUpDown_Acc_Laten.Size = new Size(49, 23); numericUpDown_Acc_Laten.Size = new Size(49, 23);
numericUpDown_Acc_Laten.TabIndex = 34; numericUpDown_Acc_Laten.TabIndex = 34;
@ -1640,17 +1651,6 @@
label23.TabIndex = 9; label23.TabIndex = 9;
label23.Text = "Noise:"; label23.Text = "Noise:";
// //
// numericUpDown_Acc_Freq
//
numericUpDown_Acc_Freq.Location = new Point(37, 16);
numericUpDown_Acc_Freq.Maximum = new decimal(new int[] { 1000, 0, 0, 0 });
numericUpDown_Acc_Freq.Minimum = new decimal(new int[] { 1, 0, 0, 0 });
numericUpDown_Acc_Freq.Name = "numericUpDown_Acc_Freq";
numericUpDown_Acc_Freq.Size = new Size(40, 23);
numericUpDown_Acc_Freq.TabIndex = 8;
numericUpDown_Acc_Freq.Value = new decimal(new int[] { 200, 0, 0, 0 });
numericUpDown_Acc_Freq.ValueChanged += numericUpDown_Acc_Update;
//
// label20 // label20
// //
label20.AutoSize = true; label20.AutoSize = true;
@ -2487,23 +2487,23 @@
((System.ComponentModel.ISupportInitialize)numericUpDown_Pos_Freq).EndInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_Pos_Freq).EndInit();
groupBox_Mag.ResumeLayout(false); groupBox_Mag.ResumeLayout(false);
groupBox_Mag.PerformLayout(); groupBox_Mag.PerformLayout();
((System.ComponentModel.ISupportInitialize)numericUpDown_Mag_Noise).EndInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_Mag_Freq).EndInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_Mag_Freq).EndInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_Mag_Noise).EndInit();
groupBox_Gyr.ResumeLayout(false); groupBox_Gyr.ResumeLayout(false);
groupBox_Gyr.PerformLayout(); groupBox_Gyr.PerformLayout();
((System.ComponentModel.ISupportInitialize)numericUpDown_Gyr_Freq).EndInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_Gyr_Laten).EndInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_Gyr_Laten).EndInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_Gyr_Shift_Z).EndInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_Gyr_Shift_Z).EndInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_Gyr_Shift_Y).EndInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_Gyr_Shift_Y).EndInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_Gyr_Shift_X).EndInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_Gyr_Shift_X).EndInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_Gyr_Noise).EndInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_Gyr_Noise).EndInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_Gyr_Freq).EndInit();
groupBox_Acc.ResumeLayout(false); groupBox_Acc.ResumeLayout(false);
groupBox_Acc.PerformLayout(); groupBox_Acc.PerformLayout();
((System.ComponentModel.ISupportInitialize)numericUpDown_Acc_Freq).EndInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_Acc_Laten).EndInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_Acc_Laten).EndInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_Acc_Scale_Left).EndInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_Acc_Scale_Left).EndInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_Acc_Scale_Rigth).EndInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_Acc_Scale_Rigth).EndInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_Acc_Noise).EndInit(); ((System.ComponentModel.ISupportInitialize)numericUpDown_Acc_Noise).EndInit();
((System.ComponentModel.ISupportInitialize)numericUpDown_Acc_Freq).EndInit();
tabPage_Area.ResumeLayout(false); tabPage_Area.ResumeLayout(false);
groupBox4.ResumeLayout(false); groupBox4.ResumeLayout(false);
groupBox4.PerformLayout(); groupBox4.PerformLayout();

143
DroneSimulator/RealMode.cs
View File

@ -1,10 +1,11 @@
using System; using System;
using System.Collections.Generic; using System.Collections.Generic;
using System.Linq; using System.Linq;
using System.Text;
using System.Numerics; using System.Numerics;
using System.Reflection; using System.Reflection;
using System.Text;
using static System.Windows.Forms.VisualStyles.VisualStyleElement.Rebar; using static System.Windows.Forms.VisualStyles.VisualStyleElement.Rebar;
using static System.Windows.Forms.VisualStyles.VisualStyleElement.TaskbarClock;
namespace DroneSimulator namespace DroneSimulator
{ {
@ -27,7 +28,7 @@ namespace DroneSimulator
private const int count = 1000; private const int count = 1000;
private Vector3[] laten = new Vector3[count]; private Vector3[] laten = new Vector3[count];
private uint index = 0; private int index = 0;
public uint timer = 0; public uint timer = 0;
public Vector3 result; public Vector3 result;
@ -53,18 +54,21 @@ namespace DroneSimulator
value.Y += ((float)rand.Next(-noise, noise)) / 1000; value.Y += ((float)rand.Next(-noise, noise)) / 1000;
value.Z += ((float)rand.Next(-noise, noise)) / 1000; value.Z += ((float)rand.Next(-noise, noise)) / 1000;
uint clock = (uint)(Lateness * 1000); uint clock = time - last;
while (true)
uint tick = time - last;
last = time;
while (tick != 0)
{ {
tick--; laten[index] = value;
laten[index++] = value; clock--;
if (index >= clock) index = 0; if (clock == 0) break;
index++;
if (index >= count) index = 0;
} }
last = time;
value = laten[index]; int move = (int)(Lateness * count);
move = index - move;
while (move < 0) move += count;
value = laten[move];
uint freq = 1000 / Freq; uint freq = 1000 / Freq;
@ -90,7 +94,7 @@ namespace DroneSimulator
private const int count = 1000; private const int count = 1000;
private Vector3[] laten = new Vector3[count]; private Vector3[] laten = new Vector3[count];
private uint index = 0; private int index = 0;
public uint timer = 0; public uint timer = 0;
public Vector3 result; public Vector3 result;
@ -113,18 +117,21 @@ namespace DroneSimulator
value.Y += ((float)rand.Next(-noise, noise)) / 1000; value.Y += ((float)rand.Next(-noise, noise)) / 1000;
value.Z += ((float)rand.Next(-noise, noise)) / 1000; value.Z += ((float)rand.Next(-noise, noise)) / 1000;
uint clock = (uint)(Lateness * 1000); uint clock = time - last;
while (true)
uint tick = time - last;
last = time;
while (tick != 0)
{ {
tick--; laten[index] = value;
laten[index++] = value; clock--;
if (index >= clock) index = 0; if (clock == 0) break;
index++;
if (index >= count) index = 0;
} }
last = time;
value = laten[index]; int move = (int)(Lateness * count);
move = index - move;
while (move < 0) move += count;
value = laten[move];
uint freq = 1000 / Freq; uint freq = 1000 / Freq;
@ -155,7 +162,7 @@ namespace DroneSimulator
private const int count = 1000; private const int count = 1000;
private Vector3[] laten = new Vector3[count]; private Vector3[] laten = new Vector3[count];
private uint index = 0; private int index = 0;
public uint timer = 0; public uint timer = 0;
public Vector3 result; public Vector3 result;
@ -169,16 +176,16 @@ namespace DroneSimulator
v.Y += ((float)rand.Next(-noise, noise)) / 1000; v.Y += ((float)rand.Next(-noise, noise)) / 1000;
v.Z += ((float)rand.Next(-noise, noise)) / 1000; v.Z += ((float)rand.Next(-noise, noise)) / 1000;
uint clock = (uint)(Lateness * 1000); uint clock = time - last;
while (true)
uint tick = time - last;
last = time;
while (tick != 0)
{ {
tick--; laten[index] = v;
laten[index++] = v; clock--;
if (index >= clock) index = 0; if (clock == 0) break;
index++;
if (index >= count) index = 0;
} }
last = time;
if (!Enable) if (!Enable)
{ {
@ -194,7 +201,10 @@ namespace DroneSimulator
return; return;
} }
v = laten[index]; int move = (int)(Lateness * count);
move = index - move;
while (move < 0) move += count;
v = laten[move];
uint freq = 1000 / Freq; uint freq = 1000 / Freq;
if (timer + freq <= time) if (timer + freq <= time)
@ -221,7 +231,7 @@ namespace DroneSimulator
private const int count = 1000; private const int count = 1000;
private float[] laten = new float[count]; private float[] laten = new float[count];
private uint index = 0; private int index = 0;
public uint timer = 0; public uint timer = 0;
public float result; public float result;
@ -235,16 +245,16 @@ namespace DroneSimulator
int noise = (int)(Noise * 1000); int noise = (int)(Noise * 1000);
v += ((float)rand.Next(-noise, noise)) / 1000; v += ((float)rand.Next(-noise, noise)) / 1000;
uint clock = (uint)(Lateness * 1000); uint clock = time - last;
while (true)
uint tick = time - last;
last = time;
while (tick != 0)
{ {
tick--; laten[index] = v;
laten[index++] = v; clock--;
if (index >= clock) index = 0; if (clock == 0) break;
index++;
if (index >= count) index = 0;
} }
last = time;
if (!Enable) if (!Enable)
{ {
@ -260,7 +270,10 @@ namespace DroneSimulator
return; return;
} }
v = laten[index]; int move = (int)(Lateness * count);
move = index - move;
while (move < 0) move += count;
v = laten[move];
uint freq = 1000 / Freq; uint freq = 1000 / Freq;
if (timer + freq <= time) if (timer + freq <= time)
@ -287,7 +300,7 @@ namespace DroneSimulator
private const int count = 1000; private const int count = 1000;
private Vector2[] laten = new Vector2[count]; private Vector2[] laten = new Vector2[count];
private uint index = 0; private int index = 0;
public uint delay = 0; public uint delay = 0;
@ -307,16 +320,16 @@ namespace DroneSimulator
v.Y += ((float)rand.Next(-noise, noise)) / 1000; v.Y += ((float)rand.Next(-noise, noise)) / 1000;
} }
uint clock = (uint)(Lateness * 1000); uint clock = time - last;
while (true)
uint tick = time - last;
last = time;
while (tick != 0)
{ {
tick--; laten[index] = v;
laten[index++] = v; clock--;
if (index >= clock) index = 0; if (clock == 0) break;
index++;
if (index >= count) index = 0;
} }
last = time;
if (!Enable) if (!Enable)
{ {
@ -332,9 +345,12 @@ namespace DroneSimulator
return true; return true;
} }
result = laten[index]; int move = (int)(Lateness * count);
move = index - move;
while (move < 0) move += count;
result = laten[move];
uint freq = 1000 / Freq; uint freq = count / Freq;
if (timer + freq <= time) if (timer + freq <= time)
{ {
timer = time; timer = time;
@ -360,7 +376,7 @@ namespace DroneSimulator
private const int count = 1000; private const int count = 1000;
private float[] laten = new float[count]; private float[] laten = new float[count];
private uint index = 0; private int index = 0;
public uint timer = 0; public uint timer = 0;
public float result; public float result;
@ -376,16 +392,16 @@ namespace DroneSimulator
v += ((float)rand.Next(-noise, noise)) / 1000; v += ((float)rand.Next(-noise, noise)) / 1000;
} }
uint clock = (uint)(Lateness * 1000); uint clock = time - last;
while (true)
uint tick = time - last;
last = time;
while (tick != 0)
{ {
tick--; laten[index] = v;
laten[index++] = v; clock--;
if (index >= clock) index = 0; if (clock == 0) break;
index++;
if (index >= count) index = 0;
} }
last = time;
if (!Enable) if (!Enable)
{ {
@ -401,7 +417,10 @@ namespace DroneSimulator
return; return;
} }
v = laten[index]; int move = (int)(Lateness * count);
move = index - move;
while (move < 0) move += count;
v = laten[move];
uint freq = 1000 / Freq; uint freq = 1000 / Freq;
if (timer + freq <= time) if (timer + freq <= time)