CPL/Simulator
6
0
Fork 2
Code Issues 1 Pull Requests Actions Packages Projects Releases Activity

add WMM magnetometer

Browse Source
This commit is contained in:
Dmitriy
2025-07-26 00:51:14 +03:00
parent d3c3b013ff
commit 65011e65ee
2 changed files with 460 additions and 419 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
DroneSimulator/Drone.cs
View File

@ -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;

876
DroneSimulator/RealMode.cs
View File

@ -9,426 +9,464 @@ using static System.Windows.Forms.VisualStyles.VisualStyleElement.TaskbarClock;
namespace DroneSimulator
{
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 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
{
/**
* 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;
}
}
}
}
}