Mag active

DroneSimulator/Drone.cs

@@ -62,7 +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();
+    private RealMode.Magnetometer RealMag = new RealMode.Magnetometer();
 
     public static byte[] getBytes(object data)
     {
@@ -373,7 +373,7 @@ namespace DroneSimulator
 
       bool of = RealOF.Update(of_xy, LaserRange, tick);
 
-            RealMagnetometer.Update(Quat, tick);
+      RealMag.Update(Quat, tick);
 
       lock (this)
       {
@@ -481,8 +481,8 @@ namespace DroneSimulator
       mag.Head.Type = DroneData.DataType.DataMag;
       mag.Head.Time = (uint)(DateTime.Now.Ticks / Stopwatch.Frequency / 1000);
 
-      mag.Mag.X = 0; mag.Mag.Y = 0; mag.Mag.Z = 0;
-      mag.Time = Timer;
+      mag.Mag.X = RealMag.result.X; mag.Mag.Y = RealMag.result.Y; mag.Mag.Z = RealMag.result.Z;
+      mag.Time = RealMag.timer;
 
       return getBytes(mag);
     }

DroneSimulator/RealMode.cs

@@ -1,4 +1,5 @@
-using System;
+using Microsoft.VisualBasic.Devices;
+using System;
 using System.Collections.Generic;
 using System.Linq;
 using System.Numerics;
@@ -157,9 +158,20 @@ namespace DroneSimulator
        * Inclination: 65° 34' 9"
        * Total Field: 51,120.8 nT
        */
+      public static bool Enable;
+      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();
+
       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);
+      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()
       {
@@ -174,15 +186,65 @@ namespace DroneSimulator
 
       private static Vector3 magneticField = InitializeMagneticField();
 
-            //TODO: noise and delay(?)
+      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(Quaternion oreintantion, uint time)
       {
-                result = Vector3.Transform(magneticField, oreintantion);
+        Vector3 value = Vector3.Transform(magneticField, oreintantion);
+
+        Vector3 v = value;
+
+        v.X += Shift.X;
+        v.Y += Shift.Y;
+        v.Z += Shift.Z;
+
+        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;
+        }
       }
     }