add WMM magnetometer

DroneSimulator/Drone.cs

@@ -62,6 +62,7 @@ namespace DroneSimulator
     private RealMode.Barometer RealBar = new RealMode.Barometer();
     private RealMode.Range RealRange = new RealMode.Range();
     private RealMode.OpticalFlow RealOF = new RealMode.OpticalFlow();
+    private RealMode.Magnetometer RealMagnetometer = new RealMode.Magnetometer();
 
     public static byte[] getBytes(object data)
     {
@@ -372,6 +373,8 @@ namespace DroneSimulator
 
       bool of = RealOF.Update(of_xy, LaserRange, tick);
 
+            RealMagnetometer.Update(Quat, tick);
+
       lock (this)
       {
         MoveOF += RealOF.result * time;

DroneSimulator/RealMode.cs

@@ -9,426 +9,464 @@ using static System.Windows.Forms.VisualStyles.VisualStyleElement.TaskbarClock;
 
 namespace DroneSimulator
 {
-  internal class RealMode
+    internal class RealMode
-  {
-    
-
-    internal class Accelerometer
-    {
-      public static uint Freq;
-      public static float Noise;
-      public static float ScaleLeft;
-      public static float ScaleRight;
-      public static float Lateness;
-      public static bool RealSimulation;
-
-      private uint last = 0;
-
-      private Random rand = new Random();
-
-      private const int count = 1000;
-      private Vector3[] laten = new Vector3[count];
-      private int 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 = time - last;
-        while (true)
-        {
-          laten[index] = value;
-          clock--;
-          if (clock == 0) break;
-          index++;
-          if (index >= count) index = 0;
-        }
-        last = time;
-
-        int move = (int)(Lateness * count);
-        move = index - move;
-        while (move < 0) move += count;
-        value = laten[move];
-
-        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;
-      public static bool RealSimulation;
-
-      private uint last = 0;
-
-      private Random rand = new Random();
-
-      private const int count = 1000;
-      private Vector3[] laten = new Vector3[count];
-      private int 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 = time - last;
-        while (true)
-        {
-          laten[index] = value;
-          clock--;
-          if (clock == 0) break;
-          index++;
-          if (index >= count) index = 0;
-        }
-        last = time;
-
-        int move = (int)(Lateness * count);
-        move = index - move;
-        while (move < 0) move += count;
-        value = laten[move];
-
-        uint freq = 1000 / Freq;
-
-        if (timer + freq <= time)
-        {
-          result = value;
-          timer = time;
-        }
-      }
-    }
-
-    internal class Magnetometer
     {
 
+
+        internal class Accelerometer
+        {
+            public static uint Freq;
+            public static float Noise;
+            public static float ScaleLeft;
+            public static float ScaleRight;
+            public static float Lateness;
+            public static bool RealSimulation;
+
+            private uint last = 0;
+
+            private Random rand = new Random();
+
+            private const int count = 1000;
+            private Vector3[] laten = new Vector3[count];
+            private int 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 = time - last;
+                while (true)
+                {
+                    laten[index] = value;
+                    clock--;
+                    if (clock == 0) break;
+                    index++;
+                    if (index >= count) index = 0;
+                }
+                last = time;
+
+                int move = (int)(Lateness * count);
+                move = index - move;
+                while (move < 0) move += count;
+                value = laten[move];
+
+                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;
+            public static bool RealSimulation;
+
+            private uint last = 0;
+
+            private Random rand = new Random();
+
+            private const int count = 1000;
+            private Vector3[] laten = new Vector3[count];
+            private int 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 = time - last;
+                while (true)
+                {
+                    laten[index] = value;
+                    clock--;
+                    if (clock == 0) break;
+                    index++;
+                    if (index >= count) index = 0;
+                }
+                last = time;
+
+                int move = (int)(Lateness * count);
+                move = index - move;
+                while (move < 0) move += count;
+                value = laten[move];
+
+                uint freq = 1000 / Freq;
+
+                if (timer + freq <= time)
+                {
+                    result = value;
+                    timer = time;
+                }
+            }
+        }
+
+        internal class Magnetometer
+        {
+            /**
+             * The model is produced by the United States’ National Geospatial-Intelligence Agency (NGA)
+             * and the United Kingdom’s Defence Geographic Centre (DGC)
+             * NCEI and the British Geological Survey (BGS) jointly developed the WMM.
+             */
+            /* Taganrog
+             * 47° 12' 32" N
+             * 38° 56' 10" E
+             * Declination: 8° 32' 28"
+             * Inclination: 65° 34' 9"
+             * Total Field: 51,120.8 nT
+             */
+            public static float fieldStrength = 51.1208F; // uT
+            public static float fieldDeclination = (8 + 32/60 + 28/3600) * (MathF.PI / 180);
+            public static float fieldInclination = (65 + 34/60 + 9/3600) * (MathF.PI / 180);
+
+            private static Vector3 InitializeMagneticField()
+            {
+                float horizontalComponent = fieldStrength * MathF.Cos(fieldInclination);
+
+                float northComponent = horizontalComponent * MathF.Cos(fieldDeclination); // X
+                float eastComponent = horizontalComponent * MathF.Sin(fieldDeclination);  // Y
+                float downComponent = fieldStrength * MathF.Sin(fieldInclination);        // Z
+
+                return new Vector3(northComponent, eastComponent, downComponent);
+            }
+
+            private static Vector3 magneticField = InitializeMagneticField();
+
+            //TODO: noise and delay(?)
+
+            public uint timer = 0;
+            public Vector3 result;
+
+            public void Update(Quaternion oreintantion, uint time)
+            {
+                result = Vector3.Transform(magneticField, oreintantion);
+                timer = time;
+            }
+        }
+
+        internal class Position
+        {
+            public static bool Enable;
+            public static uint Freq;
+            public static float Noise;
+            public static float Lateness;
+            public static bool RealSimulation;
+
+            private uint last = 0;
+
+            private Random rand = new Random();
+
+            private const int count = 1000;
+            private Vector3[] laten = new Vector3[count];
+            private int index = 0;
+
+            public uint timer = 0;
+            public Vector3 result;
+
+            public void Update(Vector3 value, uint time)
+            {
+                Vector3 v = value;
+
+                int noise = (int)(Noise * 1000);
+                v.X += ((float)rand.Next(-noise, noise)) / 1000;
+                v.Y += ((float)rand.Next(-noise, noise)) / 1000;
+                v.Z += ((float)rand.Next(-noise, noise)) / 1000;
+
+                uint clock = time - last;
+                while (true)
+                {
+                    laten[index] = v;
+                    clock--;
+                    if (clock == 0) break;
+                    index++;
+                    if (index >= count) index = 0;
+                }
+                last = time;
+
+                if (!Enable)
+                {
+                    result = Vector3.NaN;
+                    timer = time;
+                    return;
+                }
+
+                if (!RealSimulation)
+                {
+                    result = value;
+                    timer = time;
+                    return;
+                }
+
+                int move = (int)(Lateness * count);
+                move = index - move;
+                while (move < 0) move += count;
+                v = laten[move];
+
+                uint freq = 1000 / Freq;
+                if (timer + freq <= time)
+                {
+                    result = v;
+                    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;
+            public static bool RealSimulation;
+            public static float Temperature = 25.0f;
+
+            private uint last = 0;
+
+            private Random rand = new Random();
+
+            private const int count = 1000;
+            private float[] laten = new float[count];
+            private int index = 0;
+
+            public uint timer = 0;
+            public float result;
+
+            public void Update(float value, uint time)
+            {
+                value = Pressure * MathF.Exp(-0.02896f * 9.81f * value / (8.314f * (Temperature + 273.15f)));
+
+                float v = value;
+
+                int noise = (int)(Noise * 1000);
+                v += ((float)rand.Next(-noise, noise)) / 1000;
+
+                uint clock = time - last;
+                while (true)
+                {
+                    laten[index] = v;
+                    clock--;
+                    if (clock == 0) break;
+                    index++;
+                    if (index >= count) index = 0;
+                }
+                last = time;
+
+                if (!Enable)
+                {
+                    result = float.NaN;
+                    timer = time;
+                    return;
+                }
+
+                if (!RealSimulation)
+                {
+                    result = value;
+                    timer = time;
+                    return;
+                }
+
+                int move = (int)(Lateness * count);
+                move = index - move;
+                while (move < 0) move += count;
+                v = laten[move];
+
+                uint freq = 1000 / Freq;
+                if (timer + freq <= time)
+                {
+                    result = v;
+                    timer = time;
+                }
+            }
+        }
+
+        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 Lens;
+            public static bool RealSimulation;
+
+            private uint last = 0;
+
+            private Random rand = new Random();
+
+            private const int count = 1000;
+            private Vector2[] laten = new Vector2[count];
+            private int index = 0;
+
+            public uint delay = 0;
+
+            public uint timer = 0;
+            public Vector2 result;
+            public bool Update(Vector2 value, float Range, uint time)
+            {
+                value *= Lens;
+
+                Vector2 v = value;
+
+                if (Range > MaxHeight) v = Vector2.Zero;
+                else
+                {
+                    int noise = (int)(Noise * 1000);
+                    v.X += ((float)rand.Next(-noise, noise)) / 1000;
+                    v.Y += ((float)rand.Next(-noise, noise)) / 1000;
+                }
+
+                uint clock = time - last;
+                while (true)
+                {
+                    laten[index] = v;
+                    clock--;
+                    if (clock == 0) break;
+                    index++;
+                    if (index >= count) index = 0;
+                }
+                last = time;
+
+                if (!Enable)
+                {
+                    result = Vector2.NaN;
+                    timer = time;
+                    return true;
+                }
+
+                if (!RealSimulation)
+                {
+                    result = value;
+                    timer = time;
+                    return true;
+                }
+
+                int move = (int)(Lateness * count);
+                move = index - move;
+                while (move < 0) move += count;
+                result = laten[move];
+
+                uint freq = count / Freq;
+                if (timer + freq <= time)
+                {
+                    timer = time;
+                    return true;
+                }
+
+                return false;
+            }
+        }
+
+        internal class Range
+        {
+            public static bool Enable;
+            public static float MaxHeight;
+            public static uint Freq;
+            public static float Noise;
+            public static float Lateness;
+            public static bool RealSimulation;
+
+            private uint last = 0;
+
+            private Random rand = new Random();
+
+            private const int count = 1000;
+            private float[] laten = new float[count];
+            private int index = 0;
+
+            public uint timer = 0;
+            public float result;
+
+            public void Update(float value, uint time)
+            {
+                float v = value;
+
+                if (v > MaxHeight) v = -1;
+                else
+                {
+                    int noise = (int)(Noise * 1000);
+                    v += ((float)rand.Next(-noise, noise)) / 1000;
+                }
+
+                uint clock = time - last;
+                while (true)
+                {
+                    laten[index] = v;
+                    clock--;
+                    if (clock == 0) break;
+                    index++;
+                    if (index >= count) index = 0;
+                }
+                last = time;
+
+                if (!Enable)
+                {
+                    result = float.NaN;
+                    timer = time;
+                    return;
+                }
+
+                if (!RealSimulation)
+                {
+                    result = value;
+                    timer = time;
+                    return;
+                }
+
+                int move = (int)(Lateness * count);
+                move = index - move;
+                while (move < 0) move += count;
+                v = laten[move];
+
+                uint freq = 1000 / Freq;
+                if (timer + freq <= time)
+                {
+                    result = v;
+                    timer = time;
+                }
+            }
+        }
     }
-
-    internal class Position
-    {
-      public static bool Enable;
-      public static uint Freq;
-      public static float Noise;
-      public static float Lateness;
-      public static bool RealSimulation;
-
-      private uint last = 0;
-
-      private Random rand = new Random();
-
-      private const int count = 1000;
-      private Vector3[] laten = new Vector3[count];
-      private int index = 0;
-
-      public uint timer = 0;
-      public Vector3 result;
-
-      public void Update(Vector3 value, uint time)
-      {
-        Vector3 v = value;
-
-        int noise = (int)(Noise * 1000);
-        v.X += ((float)rand.Next(-noise, noise)) / 1000;
-        v.Y += ((float)rand.Next(-noise, noise)) / 1000;
-        v.Z += ((float)rand.Next(-noise, noise)) / 1000;
-
-        uint clock = time - last;
-        while (true)
-        {
-          laten[index] = v;
-          clock--;
-          if (clock == 0) break;
-          index++;
-          if (index >= count) index = 0;
-        }
-        last = time;
-
-        if (!Enable)
-        {
-          result = Vector3.NaN;
-          timer = time;
-          return;
-        }
-
-        if (!RealSimulation)
-        {
-          result = value;
-          timer = time;
-          return;
-        }
-
-        int move = (int)(Lateness * count);
-        move = index - move;
-        while (move < 0) move += count;
-        v = laten[move];
-
-        uint freq = 1000 / Freq;
-        if (timer + freq <= time)
-        {
-          result = v;
-          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;
-      public static bool RealSimulation;
-      public static float Temperature = 25.0f;
-
-      private uint last = 0;
-
-      private Random rand = new Random();
-
-      private const int count = 1000;
-      private float[] laten = new float[count];
-      private int index = 0;
-
-      public uint timer = 0;
-      public float result;
-
-      public void Update(float value, uint time)
-      {
-        value = Pressure * MathF.Exp(-0.02896f * 9.81f * value / (8.314f * (Temperature + 273.15f)));
-
-        float v = value;
-
-        int noise = (int)(Noise * 1000);
-        v += ((float)rand.Next(-noise, noise)) / 1000;
-
-        uint clock = time - last;
-        while (true)
-        {
-          laten[index] = v;
-          clock--;
-          if (clock == 0) break;
-          index++;
-          if (index >= count) index = 0;
-        }
-        last = time;
-
-        if (!Enable)
-        {
-          result = float.NaN;
-          timer = time;
-          return;
-        }
-
-        if (!RealSimulation)
-        {
-          result = value;
-          timer = time;
-          return;
-        }
-
-        int move = (int)(Lateness * count);
-        move = index - move;
-        while (move < 0) move += count;
-        v = laten[move];
-
-        uint freq = 1000 / Freq;
-        if (timer + freq <= time)
-        {
-          result = v;
-          timer = time;
-        }
-      }
-    }
-
-    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 Lens;
-      public static bool RealSimulation;
-
-      private uint last = 0;
-
-      private Random rand = new Random();
-
-      private const int count = 1000;
-      private Vector2[] laten = new Vector2[count];
-      private int index = 0;
-
-      public uint delay = 0;
-
-      public uint timer = 0;
-      public Vector2 result;
-      public bool Update(Vector2 value, float Range, uint time)
-      {
-        value *= Lens;
-
-        Vector2 v = value;
-
-        if (Range > MaxHeight) v = Vector2.Zero;
-        else
-        {
-          int noise = (int)(Noise * 1000);
-          v.X += ((float)rand.Next(-noise, noise)) / 1000;
-          v.Y += ((float)rand.Next(-noise, noise)) / 1000;
-        }
-
-        uint clock = time - last;
-        while (true)
-        {
-          laten[index] = v;
-          clock--;
-          if (clock == 0) break;
-          index++;
-          if (index >= count) index = 0;
-        }
-        last = time;
-
-        if (!Enable)
-        {
-          result = Vector2.NaN;
-          timer = time;
-          return true;
-        }
-
-        if (!RealSimulation)
-        {
-          result = value;
-          timer = time;
-          return true;
-        }
-
-        int move = (int)(Lateness * count);
-        move = index - move;
-        while (move < 0) move += count;
-        result = laten[move];
-
-        uint freq = count / Freq;
-        if (timer + freq <= time)
-        {
-          timer = time;
-          return true;
-        }
-
-        return false;
-      }
-    }
-
-    internal class Range
-    {
-      public static bool Enable;
-      public static float MaxHeight;
-      public static uint Freq;
-      public static float Noise;
-      public static float Lateness;
-      public static bool RealSimulation;
-
-      private uint last = 0;
-
-      private Random rand = new Random();
-
-      private const int count = 1000;
-      private float[] laten = new float[count];
-      private int index = 0;
-
-      public uint timer = 0;
-      public float result;
-
-      public void Update(float value, uint time)
-      {
-        float v = value;
-
-        if (v > MaxHeight) v = -1;
-        else
-        {
-          int noise = (int)(Noise * 1000);
-          v += ((float)rand.Next(-noise, noise)) / 1000;
-        }
-
-        uint clock = time - last;
-        while (true)
-        {
-          laten[index] = v;
-          clock--;
-          if (clock == 0) break;
-          index++;
-          if (index >= count) index = 0;
-        }
-        last = time;
-
-        if (!Enable)
-        {
-          result = float.NaN;
-          timer = time;
-          return;
-        }
-
-        if (!RealSimulation)
-        {
-          result = value;
-          timer = time;
-          return;
-        }
-
-        int move = (int)(Lateness * count);
-        move = index - move;
-        while (move < 0) move += count;
-        v = laten[move];
-
-        uint freq = 1000 / Freq;
-        if (timer + freq <= time)
-        {
-          result = v;
-          timer = time;
-        }
-      }
-    }
-  }
 }