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

@@ -145,7 +145,45 @@ namespace DroneSimulator
 
         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