150billionsteps/Simulator
1
0
Fork 0
forked from CPL/Simulator
Code Pull Requests Activity

Дофига исправлений

Browse Source
This commit is contained in:
Sergey Sklyarov 2025-04-04 02:12:55 +03:00
parent b8ff486e5a
commit 46e296b7c2
4 changed files with 57 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
DroneClient/FormMain.cs
View File

@ -112,37 +112,39 @@ namespace DroneSimulator
private void button_UU_MouseDown(object sender, MouseEventArgs e)
{
const float pow = 0.1f;
if (sender == button_UU)
{
sendDrone.MotorUL -= 0.1f; sendDrone.MotorUR -= 0.1f;
sendDrone.MotorDL += 0.1f; sendDrone.MotorDR += 0.1f;
sendDrone.MotorUL -= pow; sendDrone.MotorUR -= pow;
sendDrone.MotorDL += pow; sendDrone.MotorDR += pow;
}
if (sender == button_DD)
{
sendDrone.MotorUL += 0.1f; sendDrone.MotorUR += 0.1f;
sendDrone.MotorDL -= 0.1f; sendDrone.MotorDR -= 0.1f;
sendDrone.MotorUL += pow; sendDrone.MotorUR += pow;
sendDrone.MotorDL -= pow; sendDrone.MotorDR -= pow;
}
if (sender == button_LL)
{
sendDrone.MotorUL -= 0.1f; sendDrone.MotorUR += 0.1f;
sendDrone.MotorDL -= 0.1f; sendDrone.MotorDR += 0.1f;
sendDrone.MotorUL -= pow; sendDrone.MotorUR += pow;
sendDrone.MotorDL -= pow; sendDrone.MotorDR += pow;
}
if (sender == button_RR)
{
sendDrone.MotorUL += 0.1f; sendDrone.MotorUR -= 0.1f;
sendDrone.MotorDL += 0.1f; sendDrone.MotorDR -= 0.1f;
sendDrone.MotorUL += pow; sendDrone.MotorUR -= pow;
sendDrone.MotorDL += pow; sendDrone.MotorDR -= pow;
}
if (sender == button_ML)
{
sendDrone.MotorUL -= 0.1f; sendDrone.MotorUR += 0.1f;
sendDrone.MotorDL += 0.1f; sendDrone.MotorDR -= 0.1f;
sendDrone.MotorUL -= pow; sendDrone.MotorUR += pow;
sendDrone.MotorDL += pow; sendDrone.MotorDR -= pow;
}
if (sender == button_MR)
{
sendDrone.MotorUL += 0.1f; sendDrone.MotorUR -= 0.1f;
sendDrone.MotorDL -= 0.1f; sendDrone.MotorDR += 0.1f;
sendDrone.MotorUL += pow; sendDrone.MotorUR -= pow;
sendDrone.MotorDL -= pow; sendDrone.MotorDR += pow;
}
}

50
DroneSimulator/Drone.cs
View File

@ -9,6 +9,7 @@ namespace DroneSimulator
public float Mass; // Масса
public bool Active; // Живой?
public float Length; // Длинна лучей
public float Dynamic; // Физика поведения
public Vector3 PosXYZ, SpdXYZ, AccXYZ; // Положение в пространстве: Позиция, Скорость, Ускорение
public Quaternion Quat; // Основной кватернион
public float Power = 0; // Тяга всех двигателей
@ -76,6 +77,20 @@ namespace DroneSimulator
public float LaserRange;
}
public DataOut GetDataOut()
{
DataOut data = new DataOut();
data.AccX = Acc.X; data.AccY = Acc.Y; data.AccZ = Acc.Z;
data.GyrX = Gyr.X; data.GyrY = Gyr.Y; data.GyrZ = Gyr.Z;
data.PosX = PosXYZ.X; data.PosY = PosXYZ.Y;
data.LaserRange = LaserRange;
return data;
}
public struct DataIn
{
public float MotorUL, MotorUR, MotorDL, MotorDR;
@ -99,7 +114,7 @@ namespace DroneSimulator
{
float time = Environment.TickCount - Timer;
Timer = Environment.TickCount;
Action(time / 100);
Action(time / 1000);
Thread.Sleep(1);
}
}
@ -108,7 +123,7 @@ namespace DroneSimulator
{
ID = id;
Active = false;
PosXYZ = new Vector3(2000, 1000, 0);
PosXYZ = Vector3.Zero;
SpdXYZ = Vector3.Zero;
AccXYZ = Vector3.Zero;
Quat = Quaternion.Identity;
@ -118,9 +133,10 @@ namespace DroneSimulator
DroneThread.Start();
}
public int Create(float mass, float len)
public int Create(float mass, float len, float dynamic)
{
Mass = Range(mass);
Dynamic = dynamic * 100;
Length = len;
Active = true;
@ -180,19 +196,23 @@ namespace DroneSimulator
float flow = Power;
if (PosXYZ.Z < 100)
if (PosXYZ.Z < 0.3f)
{
flow += flow * 0.1f; // Воздушная подушка
}
SpdPRY += AccPRY * ((time * Dynamic) / (Mass * Length));
float dyn = Dynamic / 10;
Quaternion pow = Quaternion.Inverse(Quat) * new Quaternion(0, 0, flow, 0) * Quat;
AccXYZ = new Vector3(pow.X, pow.Y, pow.Z) / Mass;
SpdPRY += AccPRY * time / (Mass * Length);
SpdXYZ += (AccXYZ + new Vector3(0, 0, -Gravity)) * time;
AccXYZ = new Vector3(pow.X, pow.Y, pow.Z) * (dyn / Mass);
SpdXYZ += (AccXYZ + new Vector3(0, 0, -Gravity * dyn)) * time;
PosXYZ += SpdXYZ * time;
AccXYZ /= dyn;
if (PosXYZ.Z < 0)
{
SpdPRY = Vector3.Zero;
@ -200,7 +220,7 @@ namespace DroneSimulator
SpdXYZ.Y = 0;
Quat = Quaternion.Identity;
}
else Rotate(SpdPRY.X, SpdPRY.Y, SpdPRY.Z);
else Rotate(SpdPRY.X * time, SpdPRY.Y * time, SpdPRY.Z * time);
Vector4 ori = GetOrientation();
@ -213,7 +233,7 @@ namespace DroneSimulator
Active = false; // Сильно ударился о землю
}*/
/*if (MathF.Abs(ori.X) > 45 || MathF.Abs(ori.Y) > 45)
/*if (MathF.Abs(ori.X) > 20 || MathF.Abs(ori.Y) > 20)
{
Active = false; // Повредил винты при посадке
}*/
@ -226,13 +246,13 @@ namespace DroneSimulator
}
else
{
/*if (ori.W < 0)
if (ori.W < 0)
{
Active = false; // Перевернулся вверх ногами
}*/
//Active = false; // Перевернулся вверх ногами
}
Quaternion grav = new Quaternion(AccXYZ.X, AccXYZ.Y, AccXYZ.Z, 0);
grav = (Quat * grav) * Quaternion.Inverse(Quat);
//grav = (Quat * grav) * Quaternion.Inverse(Quat); // Инерциальный акселерометр (тест)
Acc = new Vector3(grav.X, grav.Y, grav.Z);
Gyr = SpdPRY;

9
DroneSimulator/FormMain.cs
View File

@ -33,7 +33,7 @@ namespace DroneSimulator
if (data.Connect)
{
Drone drone = new Drone(data.ID);
drone.Create(0.5f, 10.0f);
drone.Create(0.5f, 1.0f, 1.0f);
screen2D.CreateDrone(Color.Red, data.ID);
@ -73,12 +73,7 @@ namespace DroneSimulator
drone.SetQadroPow(id.MotorUL, id.MotorUR, id.MotorDL, id.MotorDR);
Drone.DataOut od = new Drone.DataOut();
od.AccX = drone.Acc.X; od.AccY = drone.Acc.Y; od.AccZ = drone.Acc.Z;
od.GyrX = drone.Gyr.X; od.GyrY = drone.Gyr.Y; od.GyrZ = drone.Gyr.Z;
od.PosX = drone.PosXYZ.X; od.PosY = drone.PosXYZ.Y;
od.LaserRange = drone.LaserRange;
Drone.DataOut od = drone.GetDataOut();
try { data.Client.Send(Drone.getBytes(od)); }
catch { }

6
DroneSimulator/Screen2D.cs
View File

@ -24,6 +24,7 @@ namespace DroneSimulator
public int Azimuth = 0;
}
private float Scale = 100;
private DrawCallback drawCallback;
private Bitmap MainArea = new Bitmap(4096, 2560);
private List<DroneInfo> DroneList = new List<DroneInfo>();
@ -161,6 +162,11 @@ namespace DroneSimulator
const float TO_GRAD = 180 / MathF.PI;
const float TO_RADI = MathF.PI / 180;
pos *= Scale;
pos.X += MainArea.Width / 2;
pos.Y += MainArea.Height / 2;
foreach (var d in DroneList)
{
if (d.ID != id) continue;