forked from CPL/Simulator
535 lines
12 KiB
C#
535 lines
12 KiB
C#
using Microsoft.VisualBasic.Devices;
|
||
using System;
|
||
using System.Collections.Generic;
|
||
using System.Linq;
|
||
using System.Numerics;
|
||
using System.Reflection;
|
||
using System.Text;
|
||
using static System.Windows.Forms.VisualStyles.VisualStyleElement.Rebar;
|
||
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
|
||
{
|
||
/**
|
||
* 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 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);
|
||
|
||
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();
|
||
|
||
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)
|
||
{
|
||
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;
|
||
}
|
||
}
|
||
}
|
||
|
||
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;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|