Add real mode (bar)
This commit is contained in:
317
DroneSimulator/RealMode.cs
Normal file
317
DroneSimulator/RealMode.cs
Normal file
@ -0,0 +1,317 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
using System.Text;
|
||||
using System.Numerics;
|
||||
using System.Reflection;
|
||||
|
||||
namespace DroneSimulator
|
||||
{
|
||||
internal class RealMode
|
||||
{
|
||||
public static bool RealSimulation;
|
||||
|
||||
internal class Accelerometer
|
||||
{
|
||||
public static uint Freq;
|
||||
public static float Noise;
|
||||
public static float ScaleLeft;
|
||||
public static float ScaleRight;
|
||||
public static float Lateness;
|
||||
|
||||
private uint last = 0;
|
||||
|
||||
private Random rand = new Random();
|
||||
|
||||
private const int count = 1000;
|
||||
private Vector3[] laten = new Vector3[count];
|
||||
private uint index = 0;
|
||||
|
||||
private uint timer = 0;
|
||||
private Vector3 result;
|
||||
|
||||
public void Update(ref Vector3 value, ref uint time)
|
||||
{
|
||||
if (!RealSimulation) 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;
|
||||
}
|
||||
else
|
||||
{
|
||||
value = result;
|
||||
time = timer;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
internal class Gyroscope
|
||||
{
|
||||
public static uint Freq;
|
||||
public static float Noise;
|
||||
public static Vector3 Shift;
|
||||
public static float Lateness;
|
||||
|
||||
private uint last = 0;
|
||||
|
||||
private Random rand = new Random();
|
||||
|
||||
private const int count = 1000;
|
||||
private Vector3[] laten = new Vector3[count];
|
||||
private uint index = 0;
|
||||
|
||||
private uint timer = 0;
|
||||
private Vector3 result;
|
||||
|
||||
public void Update(ref Vector3 value, ref uint time)
|
||||
{
|
||||
if (!RealSimulation) 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;
|
||||
}
|
||||
else
|
||||
{
|
||||
value = result;
|
||||
time = timer;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
internal class Magnetometer
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
internal class Position
|
||||
{
|
||||
public static bool Enable;
|
||||
public static uint Freq;
|
||||
public static float Noise;
|
||||
public static float Lateness;
|
||||
|
||||
private uint last = 0;
|
||||
|
||||
private Random rand = new Random();
|
||||
|
||||
private const int count = 1000;
|
||||
private Vector3[] laten = new Vector3[count];
|
||||
private uint index = 0;
|
||||
|
||||
private uint timer = 0;
|
||||
private Vector3 result;
|
||||
|
||||
public void Update(ref Vector3 value, ref uint time)
|
||||
{
|
||||
if (!RealSimulation) return;
|
||||
if (!Enable) { value = result; time = timer; return; }
|
||||
|
||||
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;
|
||||
}
|
||||
else
|
||||
{
|
||||
value = result;
|
||||
time = timer;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
internal class Barometer
|
||||
{
|
||||
public static bool Enable;
|
||||
public static float Pressure;
|
||||
public static uint Freq;
|
||||
public static float Noise;
|
||||
public static float Lateness;
|
||||
|
||||
private uint last = 0;
|
||||
|
||||
private Random rand = new Random();
|
||||
|
||||
private const int count = 1000;
|
||||
private float[] laten = new float[count];
|
||||
private uint index = 0;
|
||||
|
||||
private uint timer = 0;
|
||||
private float result;
|
||||
|
||||
public void Update(ref float value, ref uint time)
|
||||
{
|
||||
if (!Enable) { value = result; time = timer; return; }
|
||||
|
||||
value = Pressure - value;
|
||||
|
||||
if (!RealSimulation) return;
|
||||
|
||||
int noise = (int)(Noise * 1000);
|
||||
value += ((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;
|
||||
}
|
||||
else
|
||||
{
|
||||
value = result;
|
||||
time = timer;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
internal class OpticalFlow
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
internal class Range
|
||||
{
|
||||
public static bool Enable;
|
||||
public static float MaxHeight;
|
||||
public static uint Freq;
|
||||
public static float Noise;
|
||||
public static float Lateness;
|
||||
|
||||
private uint last = 0;
|
||||
|
||||
private Random rand = new Random();
|
||||
|
||||
private const int count = 1000;
|
||||
private float[] laten = new float[count];
|
||||
private uint index = 0;
|
||||
|
||||
private uint timer = 0;
|
||||
private float result;
|
||||
|
||||
public void Update(ref float value, ref uint time)
|
||||
{
|
||||
if (!RealSimulation) return;
|
||||
if (!Enable) { value = result; time = timer; return; }
|
||||
|
||||
if (value > MaxHeight) value = -1;
|
||||
|
||||
int noise = (int)(Noise * 1000);
|
||||
value += ((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;
|
||||
}
|
||||
else
|
||||
{
|
||||
value = result;
|
||||
time = timer;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user