dmarkova/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:53:00 +03:00
parent 46e296b7c2
commit bf037b0870
2 changed files with 12 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
DroneSimulator/Drone.cs
View File

@ -9,16 +9,17 @@ namespace DroneSimulator
public float Mass; // Масса public float Mass; // Масса
public bool Active; // Живой? public bool Active; // Живой?
public float Length; // Длинна лучей public float Length; // Длинна лучей
public float Dynamic; // Физика поведения public const float Dynamic = 10; // Динамика вращения
public Vector3 PosXYZ, SpdXYZ, AccXYZ; // Положение в пространстве: Позиция, Скорость, Ускорение public Vector3 PosXYZ, SpdXYZ, AccXYZ; // Положение в пространстве: Позиция, Скорость, Ускорение
public Quaternion Quat; // Основной кватернион public Quaternion Quat; // Основной кватернион
public float Power = 0; // Тяга всех двигателей public float Power = 0; // Тяга всех двигателей
public float MaxPower; // Тяга всех двигателей
public Vector3 SpdPRY, AccPRY; // Поворот в пространстве: pitch roll yaw public Vector3 SpdPRY, AccPRY; // Поворот в пространстве: pitch roll yaw
public Vector3 Acc, Gyr; // Имитация: Акселерометр, Гироскоп public Vector3 Acc, Gyr; // Имитация: Акселерометр, Гироскоп
public float LaserRange; // Имитация: Дальномер public float LaserRange; // Имитация: Дальномер
private const float Gravity = 1.0f; private const float Gravity = 9.8f;
private const float TO_GRAD = 180 / MathF.PI; private const float TO_GRAD = 180 / MathF.PI;
private const float TO_RADI = MathF.PI / 180; private const float TO_RADI = MathF.PI / 180;
@ -133,11 +134,11 @@ namespace DroneSimulator
DroneThread.Start(); DroneThread.Start();
} }
public int Create(float mass, float len, float dynamic) public int Create(float power, float mass, float len)
{ {
Mass = Range(mass); Mass = mass;
Dynamic = dynamic * 100;
Length = len; Length = len;
MaxPower = power;
Active = true; Active = true;
@ -201,17 +202,15 @@ namespace DroneSimulator
flow += flow * 0.1f; // Воздушная подушка flow += flow * 0.1f; // Воздушная подушка
} }
SpdPRY += AccPRY * ((time * Dynamic) / (Mass * Length)); SpdPRY += AccPRY * (Dynamic * time / (Mass * Length));
float dyn = Dynamic / 10;
Quaternion pow = Quaternion.Inverse(Quat) * new Quaternion(0, 0, flow, 0) * Quat; Quaternion pow = Quaternion.Inverse(Quat) * new Quaternion(0, 0, flow, 0) * Quat;
AccXYZ = new Vector3(pow.X, pow.Y, pow.Z) * (dyn / Mass); AccXYZ = new Vector3(pow.X, pow.Y, pow.Z) * (Gravity / Mass);
SpdXYZ += (AccXYZ + new Vector3(0, 0, -Gravity * dyn)) * time; SpdXYZ += (AccXYZ + new Vector3(0, 0, -Gravity)) * time;
PosXYZ += SpdXYZ * time; PosXYZ += SpdXYZ * time;
AccXYZ /= dyn; AccXYZ /= Gravity; // Вернуть измерения в G
if (PosXYZ.Z < 0) if (PosXYZ.Z < 0)
{ {
@ -269,7 +268,7 @@ namespace DroneSimulator
if (pow > 1) pow = 1; if (pow > 1) pow = 1;
if (pow < 0) pow = 0; if (pow < 0) pow = 0;
return pow; return pow * MaxPower;
} }
public void SetQadroPow(float ul, float ur, float dl, float dr) public void SetQadroPow(float ul, float ur, float dl, float dr)

2
DroneSimulator/FormMain.cs
View File

@ -33,7 +33,7 @@ namespace DroneSimulator
if (data.Connect) if (data.Connect)
{ {
Drone drone = new Drone(data.ID); Drone drone = new Drone(data.ID);
drone.Create(0.5f, 1.0f, 1.0f); drone.Create(1.0f, 0.5f, 1.0f);
screen2D.CreateDrone(Color.Red, data.ID); screen2D.CreateDrone(Color.Red, data.ID);