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