CPL/Simulator
6
0
Fork 2
Code Issues 1 Pull Requests Actions Packages Projects Releases Activity

Update Drone.cs

Browse Source
This commit is contained in:
Sergey Sklyarov
2025-06-27 12:12:24 +03:00
parent 39c81a227b
commit 12e518af0e
1 changed files with 6 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
DroneSimulator/Drone.cs
View File

@ -293,8 +293,9 @@ namespace DroneSimulator
Vector4 ori = GetOrientation(); Vector4 ori = GetOrientation();
Orientation = ori; Orientation = ori;
float range = 0;
if(PosXYZ.Z <= 0) if (PosXYZ.Z <= 0)
{ {
PosXYZ.Z = 0; PosXYZ.Z = 0;
SpdXYZ.Z = 0; SpdXYZ.Z = 0;
@ -336,19 +337,19 @@ namespace DroneSimulator
float tilt = MathF.Sqrt((ori.X * ori.X) + (ori.Y * ori.Y)) * TO_RADI; float tilt = MathF.Sqrt((ori.X * ori.X) + (ori.Y * ori.Y)) * TO_RADI;
if (tilt < 90 && ori.W > 0) LaserRange = PosXYZ.Z / MathF.Cos(tilt); range = PosXYZ.Z / MathF.Cos(tilt);
if (tilt < 90 && ori.W > 0) LaserRange = range;
else LaserRange = float.MaxValue; else LaserRange = float.MaxValue;
} }
RealAcc.Update(Acc, tick); RealAcc.Update(Acc, tick);
RealGyr.Update(Gyr, tick); RealGyr.Update(Gyr, tick);
RealRange.Update(LaserRange, tick); RealRange.Update(LaserRange, tick);
RealBar.Update(PosXYZ.Z, tick); RealBar.Update(PosXYZ.Z, tick);
RealPos.Update(PosXYZ, tick); RealPos.Update(PosXYZ, tick);
bool of = RealOF.Update(new Vector2(SpdXYZ.X - Gyr.Y, SpdXYZ.Y + Gyr.X), LaserRange, tick); bool of = RealOF.Update(new Vector2(SpdXYZ.X * range - Gyr.Y, SpdXYZ.Y * range + Gyr.X), LaserRange, tick);
if(of) lock (this) if(of) lock (this)
{ {