417 lines
8.9 KiB
C#
417 lines
8.9 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Linq;
|
|
using System.Text;
|
|
using System.Numerics;
|
|
using System.Reflection;
|
|
using static System.Windows.Forms.VisualStyles.VisualStyleElement.Rebar;
|
|
|
|
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 uint 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 = (uint)(Lateness * 1000);
|
|
|
|
uint tick = time - last;
|
|
last = time;
|
|
while (tick != 0)
|
|
{
|
|
tick--;
|
|
laten[index++] = value;
|
|
if (index >= clock) index = 0;
|
|
}
|
|
|
|
value = laten[index];
|
|
|
|
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 uint 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 = (uint)(Lateness * 1000);
|
|
|
|
uint tick = time - last;
|
|
last = time;
|
|
while (tick != 0)
|
|
{
|
|
tick--;
|
|
laten[index++] = value;
|
|
if (index >= clock) index = 0;
|
|
}
|
|
|
|
value = laten[index];
|
|
|
|
uint freq = 1000 / Freq;
|
|
|
|
if (timer + freq <= time)
|
|
{
|
|
result = value;
|
|
timer = time;
|
|
}
|
|
}
|
|
}
|
|
|
|
internal class Magnetometer
|
|
{
|
|
|
|
}
|
|
|
|
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 uint 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 = (uint)(Lateness * 1000);
|
|
|
|
uint tick = time - last;
|
|
last = time;
|
|
while (tick != 0)
|
|
{
|
|
tick--;
|
|
laten[index++] = v;
|
|
if (index >= clock) index = 0;
|
|
}
|
|
|
|
if (!Enable)
|
|
{
|
|
result = Vector3.NaN;
|
|
timer = time;
|
|
return;
|
|
}
|
|
|
|
if (!RealSimulation)
|
|
{
|
|
result = value;
|
|
timer = time;
|
|
return;
|
|
}
|
|
|
|
v = laten[index];
|
|
|
|
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 uint 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 = (uint)(Lateness * 1000);
|
|
|
|
uint tick = time - last;
|
|
last = time;
|
|
while (tick != 0)
|
|
{
|
|
tick--;
|
|
laten[index++] = v;
|
|
if (index >= clock) index = 0;
|
|
}
|
|
|
|
if (!Enable)
|
|
{
|
|
result = float.NaN;
|
|
timer = time;
|
|
return;
|
|
}
|
|
|
|
if (!RealSimulation)
|
|
{
|
|
result = value;
|
|
timer = time;
|
|
return;
|
|
}
|
|
|
|
v = laten[index];
|
|
|
|
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 uint 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 = (uint)(Lateness * 1000);
|
|
|
|
uint tick = time - last;
|
|
last = time;
|
|
while (tick != 0)
|
|
{
|
|
tick--;
|
|
laten[index++] = v;
|
|
if (index >= clock) index = 0;
|
|
}
|
|
|
|
if (!Enable)
|
|
{
|
|
result = Vector2.NaN;
|
|
timer = time;
|
|
return true;
|
|
}
|
|
|
|
if (!RealSimulation)
|
|
{
|
|
result = value;
|
|
timer = time;
|
|
return true;
|
|
}
|
|
|
|
v = laten[index];
|
|
|
|
uint freq = 1000 / Freq;
|
|
if (timer + freq <= time)
|
|
{
|
|
result = v;
|
|
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 uint 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 = (uint)(Lateness * 1000);
|
|
|
|
uint tick = time - last;
|
|
last = time;
|
|
while (tick != 0)
|
|
{
|
|
tick--;
|
|
laten[index++] = v;
|
|
if (index >= clock) index = 0;
|
|
}
|
|
|
|
if (!Enable)
|
|
{
|
|
result = float.NaN;
|
|
timer = time;
|
|
return;
|
|
}
|
|
|
|
if (!RealSimulation)
|
|
{
|
|
result = value;
|
|
timer = time;
|
|
return;
|
|
}
|
|
|
|
v = laten[index];
|
|
|
|
uint freq = 1000 / Freq;
|
|
if (timer + freq <= time)
|
|
{
|
|
result = v;
|
|
timer = time;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|