DroneSimulator/Drone.cs

@@ -2,6 +2,9 @@
 using System.Numerics;
 using System.Runtime.InteropServices;
 using System.Security.Cryptography;
+using static DroneSimulator.Drone;
+using static System.Windows.Forms.VisualStyles.VisualStyleElement;
+using static System.Windows.Forms.VisualStyles.VisualStyleElement.Rebar;
 
 namespace DroneSimulator
 {
@@ -31,7 +34,13 @@ namespace DroneSimulator
     private Thread DroneThread;
     private int Timer;
 
-    private static int CounterID = 0;
+    private Random MainRandom = new Random();
+
+    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();
 
     public static byte[] getBytes(object data)
     {
@@ -240,6 +249,12 @@ namespace DroneSimulator
         else LaserRange = float.MaxValue;
       }
 
+      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);
+
       DataTimer = (uint)tick;
     }
 
@@ -278,8 +293,8 @@ namespace DroneSimulator
       acc.Head.Type = DroneData.DataType.DataAcc;
       acc.Head.Time = (uint)Environment.TickCount;
 
-      acc.Acc.X = Acc.X; acc.Acc.Y = Acc.Y; acc.Acc.Z = Acc.Z;
-      acc.Time = DataTimer;
+      acc.Acc.X = RealAcc.result.X; acc.Acc.Y = RealAcc.result.Y; acc.Acc.Z = RealAcc.result.Z;
+      acc.Time = RealAcc.timer;
 
       return getBytes(acc);
     }
@@ -293,8 +308,8 @@ namespace DroneSimulator
       gyr.Head.Type = DroneData.DataType.DataGyr;
       gyr.Head.Time = (uint)Environment.TickCount;
 
-      gyr.Gyr.X = Gyr.X; gyr.Gyr.Y = Gyr.Y; gyr.Gyr.Z = Gyr.Z;
-      gyr.Time = DataTimer;
+      gyr.Gyr.X = RealGyr.result.X; gyr.Gyr.Y = RealGyr.result.Y; gyr.Gyr.Z = RealGyr.result.Z;
+      gyr.Time = RealGyr.timer;
 
       return getBytes(gyr);
     }
@@ -323,8 +338,8 @@ namespace DroneSimulator
       range.Head.Type = DroneData.DataType.DataRange;
       range.Head.Time = (uint)Environment.TickCount;
 
-      range.LiDAR = LaserRange;
-      range.Time = DataTimer;
+      range.LiDAR = RealRange.result;
+      range.Time = RealRange.timer;
 
       return getBytes(range);
     }
@@ -338,12 +353,27 @@ namespace DroneSimulator
       local.Head.Type = DroneData.DataType.DataLocal;
       local.Head.Time = (uint)Environment.TickCount;
 
-      local.Local.X = PosXYZ.X; local.Local.Y = PosXYZ.Y; local.Local.Z = PosXYZ.Z;
-      local.Time = DataTimer;
+      local.Local.X = RealPos.result.X; local.Local.Y = RealPos.result.Y; local.Local.Z = RealPos.result.Z;
+      local.Time = RealPos.timer;
 
       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 = RealBar.result;
+      bar.Time = RealBar.timer;
+
+      return getBytes(bar);
+    }
+
     private byte[]? ServerRequestResponse(DroneData.DataHead head, byte[] body)
     {
       byte[] zero = new byte[0];
@@ -366,86 +396,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;

DroneSimulator/DroneData.cs

@@ -1,4 +1,4 @@
-using System.Runtime.InteropServices;
+using System.Runtime.InteropServices;
 
 namespace DroneData
 {
@@ -12,7 +12,7 @@ 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,
 
     // Input
     DataMotor4 = 2001, DataMotor6 = 2002
@@ -25,7 +25,7 @@ namespace DroneData
     public DataMode Mode;
     public DataType Type;
 
-    public uint Time; // Îáùåå âðåìÿ
+    public uint Time; // Общее время
 
     static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataHead));
   }
@@ -37,7 +37,7 @@ namespace DroneData
     public DataHead Head;
     public XYZ Acc;
 
-    public uint Time; // Ïîñëåäíåå âðåìÿ èçìåíåíèÿ äàííûõ
+    public uint Time; // Последнее время изменения данных
 
     static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataAcc));
   }
@@ -47,7 +47,7 @@ namespace DroneData
     public DataHead Head;
     public XYZ Gyr;
 
-    public uint Time; // Ïîñëåäíåå âðåìÿ èçìåíåíèÿ äàííûõ
+    public uint Time; // Последнее время изменения данных
 
     static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataGyr));
   }
@@ -57,31 +57,41 @@ namespace DroneData
     public DataHead Head;
     public XYZ Mag;
 
-    public uint Time; // Ïîñëåäíåå âðåìÿ èçìåíåíèÿ äàííûõ
+    public uint Time; // Последнее время изменения данных
 
     static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataMag));
   }
 
-  public struct DataLocal
-  {
-    public DataHead Head;
-    public XYZ Local; // Ëîêàëüíûå êîîðäèíàòû
-
-    public uint Time; // Ïîñëåäíåå âðåìÿ èçìåíåíèÿ äàííûõ
-
-    static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataLocal));
-  }
-
   public struct DataRange
   {
     public DataHead Head;
-    public float LiDAR; // Äàò÷èê ïîñàäêè
+    public float LiDAR; // Датчик посадки
 
-    public uint Time; // Ïîñëåäíåå âðåìÿ èçìåíåíèÿ äàííûõ
+    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 DataMotor4
   {
     public DataHead Head;
@@ -97,4 +107,4 @@ namespace DroneData
 
     static public int StrLen = Marshal.SizeOf(typeof(DroneData.DataMotor6));
   }
-}
+}

DroneSimulator/FormMain.cs

@@ -1,4 +1,4 @@
-using System.Text;
+using System.Text;
 using System.Numerics;
 using System.Windows.Forms;
 using static System.Net.Mime.MediaTypeNames;
@@ -22,6 +22,13 @@ namespace DroneSimulator
     public Form_Main()
     {
       InitializeComponent();
+
+      RealMode.RealSimulation = checkBox_Mode_Real.Checked;
+      numericUpDown_Acc_Update(null, null);
+      numericUpDown_Gyr_Update(null, null);
+      numericUpDown_Pos_Update(null, null);
+      numericUpDown_Bar_Update(null, null);
+      numericUpDown_Range_Update(null, null);
     }
 
     private void ClientConnectionCallback(object o)
@@ -75,7 +82,7 @@ namespace DroneSimulator
       List<byte[]?>? send = drone.DataStream(data.Buffer, data.Size);
 
       if (send == null) return;
-      try 
+      try
       {
         foreach (byte[]? b in send)
         {
@@ -204,5 +211,65 @@ namespace DroneSimulator
           }
       }
     }
+
+    private void numericUpDown_Bar_Update(object sender, EventArgs e)
+    {
+      try { RealMode.Barometer.Pressure = uint.Parse(textBox_Bar_Pressure.Text); }
+      catch
+      {
+        RealMode.Barometer.Pressure = 102258;
+        MessageBox.Show("Pressure invalid format", "Barometer error", MessageBoxButtons.OK, MessageBoxIcon.Error);
+      }
+
+
+      RealMode.Barometer.Freq = (uint)numericUpDown_Bar_Freq.Value;
+      RealMode.Barometer.Noise = (float)numericUpDown_Bar_Noise.Value;
+      RealMode.Barometer.Lateness = (float)numericUpDown_Bar_Laten.Value;
+      RealMode.Barometer.Enable = checkBox_Bar_Enable.Checked;
+    }
+
+    private void checkBox_Mode_Real_CheckedChanged(object sender, EventArgs e)
+    {
+      RealMode.RealSimulation = checkBox_Mode_Real.Checked;
+    }
+
+    private void numericUpDown_Acc_Update(object sender, EventArgs e)
+    {
+      RealMode.Accelerometer.Freq = (uint)numericUpDown_Acc_Freq.Value;
+      RealMode.Accelerometer.Noise = (float)numericUpDown_Acc_Noise.Value;
+      RealMode.Accelerometer.Lateness = (float)numericUpDown_Acc_Laten.Value;
+
+      RealMode.Accelerometer.ScaleLeft = (float)numericUpDown_Acc_Scale_Left.Value;
+      RealMode.Accelerometer.ScaleRight = (float)numericUpDown_Acc_Scale_Rigth.Value;
+    }
+
+    private void numericUpDown_Gyr_Update(object sender, EventArgs e)
+    {
+      RealMode.Gyroscope.Freq = (uint)numericUpDown_Gyr_Freq.Value;
+      RealMode.Gyroscope.Noise = (float)numericUpDown_Gyr_Noise.Value;
+      RealMode.Gyroscope.Lateness = (float)numericUpDown_Gyr_Laten.Value;
+
+      RealMode.Gyroscope.Shift.X = (float)numericUpDown_Gyr_Shift_X.Value;
+      RealMode.Gyroscope.Shift.Y = (float)numericUpDown_Gyr_Shift_Y.Value;
+      RealMode.Gyroscope.Shift.Z = (float)numericUpDown_Gyr_Shift_Z.Value;
+    }
+
+    private void numericUpDown_Pos_Update(object sender, EventArgs e)
+    {
+      RealMode.Position.Freq = (uint)numericUpDown_Pos_Freq.Value;
+      RealMode.Position.Noise = (float)numericUpDown_Pos_Noise.Value;
+      RealMode.Position.Lateness = (float)numericUpDown_Pos_Laten.Value;
+      RealMode.Position.Enable = checkBox_Pos_Enable.Checked;
+    }
+
+    private void numericUpDown_Range_Update(object sender, EventArgs e)
+    {
+      RealMode.Range.Freq = (uint)numericUpDown_Range_Freq.Value;
+      RealMode.Range.Noise = (float)numericUpDown_Range_Noise.Value;
+      RealMode.Range.Lateness = (float)numericUpDown_Range_Laten.Value;
+      RealMode.Range.Enable = checkBox_Range_Enable.Checked;
+
+      RealMode.Range.MaxHeight = (float)numericUpDown_Range_Max.Value;
+    }
   }
 }

DroneSimulator/RealMode.cs

@@ -0,0 +1,333 @@
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+using System.Numerics;
+using System.Reflection;
+
+namespace DroneSimulator
+{
+  internal class RealMode
+  {
+    public static bool RealSimulation;
+
+    internal class Accelerometer
+    {
+      public static uint Freq;
+      public static float Noise;
+      public static float ScaleLeft;
+      public static float ScaleRight;
+      public static float Lateness;
+
+      private uint last = 0;
+
+      private Random rand = new Random();
+
+      private const int count = 1000;
+      private Vector3[] laten = new Vector3[count];
+      private uint index = 0;
+
+      public uint timer = 0;
+      public Vector3 result;
+
+      public void Update(Vector3 value, uint time)
+      {
+        if (!RealSimulation)
+        {
+          result = value;
+          timer = time;
+          return;
+        }
+
+        float scale = (ScaleRight - ScaleLeft) / 2;
+        float shift = scale + ScaleLeft;
+
+        value.X = (value.X * scale) + shift;
+        value.Y = (value.Y * scale) + shift;
+        value.Z = (value.Z * scale) + shift;
+
+        int noise = (int)(Noise * 1000);
+        value.X += ((float)rand.Next(-noise, noise)) / 1000;
+        value.Y += ((float)rand.Next(-noise, noise)) / 1000;
+        value.Z += ((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 Gyroscope
+    {
+      public static uint Freq;
+      public static float Noise;
+      public static Vector3 Shift;
+      public static float Lateness;
+
+      private uint last = 0;
+
+      private Random rand = new Random();
+
+      private const int count = 1000;
+      private Vector3[] laten = new Vector3[count];
+      private uint index = 0;
+
+      public uint timer = 0;
+      public Vector3 result;
+
+      public void Update(Vector3 value, uint time)
+      {
+        if (!RealSimulation)
+        {
+          result = value;
+          timer = time;
+          return;
+        }
+
+        value.X += Shift.X;
+        value.Y += Shift.Y;
+        value.Z += Shift.Z;
+
+        int noise = (int)(Noise * 1000);
+        value.X += ((float)rand.Next(-noise, noise)) / 1000;
+        value.Y += ((float)rand.Next(-noise, noise)) / 1000;
+        value.Z += ((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 Magnetometer
+    {
+
+    }
+
+    internal class Position
+    {
+      public static bool Enable;
+      public static uint Freq;
+      public static float Noise;
+      public static float Lateness;
+
+      private uint last = 0;
+
+      private Random rand = new Random();
+
+      private const int count = 1000;
+      private Vector3[] laten = new Vector3[count];
+      private uint index = 0;
+
+      public uint timer = 0;
+      public Vector3 result;
+
+      public void Update(Vector3 value, uint time)
+      {
+        if (!RealSimulation)
+        {
+          result = value;
+          timer = time;
+          return;
+        }
+
+        if (!Enable)
+        {
+          result = Vector3.NaN;
+          return;
+        }
+
+        int noise = (int)(Noise * 1000);
+        value.X += ((float)rand.Next(-noise, noise)) / 1000;
+        value.Y += ((float)rand.Next(-noise, noise)) / 1000;
+        value.Z += ((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 Barometer
+    {
+      public static bool Enable;
+      public static float Pressure;
+      public static uint Freq;
+      public static float Noise;
+      public static float Lateness;
+
+      private uint last = 0;
+
+      private Random rand = new Random();
+
+      private const int count = 1000;
+      private float[] laten = new float[count];
+      private uint index = 0;
+
+      public uint timer = 0;
+      public float result;
+
+      public void Update(float value, uint time)
+      {
+        value = Pressure - value;
+
+        if (!RealSimulation)
+        {
+          result = value;
+          timer = time;
+          return; 
+        }
+
+        if (!Enable)
+        {
+          result = float.NaN;
+          return;
+        }
+
+        int noise = (int)(Noise * 1000);
+        value += ((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 OpticalFlow
+    {
+
+    }
+
+    internal class Range
+    {
+      public static bool Enable;
+      public static float MaxHeight;
+      public static uint Freq;
+      public static float Noise;
+      public static float Lateness;
+
+      private uint last = 0;
+
+      private Random rand = new Random();
+
+      private const int count = 1000;
+      private float[] laten = new float[count];
+      private uint index = 0;
+
+      public uint timer = 0;
+      public float result;
+
+      public void Update(float value, uint time)
+      {
+        if (!RealSimulation)
+        {
+          result = value;
+          timer = time;
+          return;
+        }
+
+        if (!Enable)
+        {
+          result = float.NaN;
+          return;
+        }
+
+        if (value > MaxHeight) value = -1;
+        else
+        {
+          int noise = (int)(Noise * 1000);
+          value += ((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;
+        }
+      }
+    }
+  }
+}