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

update

Browse Source
This commit is contained in:
Sergey Sklyarov
2025-05-22 01:47:54 +03:00
parent e0f3e3db19
commit 90cec037eb
9 changed files with 553 additions and 176 deletions
Download Patch File Download Diff File Expand all files Collapse all files

40
DroneSimulator/Drone.cs
View File

@ -26,6 +26,8 @@ namespace DroneSimulator
public Vector4 Orientation;
public static bool[] Freeze = new bool[3];
private uint DataTimer;
private const float Gravity = 9.8f;
@ -36,13 +38,14 @@ namespace DroneSimulator
private Thread DroneThread;
private int Timer;
private Random MainRandom = new Random();
private Vector2 MoveOF = Vector2.Zero;
RealMode.Accelerometer RealAcc = new RealMode.Accelerometer();
RealMode.Gyroscope RealGyr = new RealMode.Gyroscope();
RealMode.Position RealPos = new RealMode.Position();
RealMode.Barometer RealBar = new RealMode.Barometer();
RealMode.Range RealRange = new RealMode.Range();
RealMode.OpticalFlow RealOF = new RealMode.OpticalFlow();
public static byte[] getBytes(object data)
{
@ -169,7 +172,7 @@ namespace DroneSimulator
{
Quaternion grav = new Quaternion(0, 0, 1, 0);
grav = (Quat * grav) * Quaternion.Inverse(Quat);
grav = Quat * grav * Quaternion.Inverse(Quat);
float yaw = 2 * MathF.Atan2(Quat.Z, Quat.W) * TO_GRAD;
if (yaw < 0.0f) yaw = 360.0f + yaw;
@ -201,6 +204,10 @@ namespace DroneSimulator
AccXYZ /= Gravity; // Вернуть измерения в G
if (Freeze[0]) { SpdXYZ.X = 0; PosXYZ.X = 0; }
if (Freeze[1]) { SpdXYZ.Y = 0; PosXYZ.Y = 0; }
if (Freeze[2]) { SpdXYZ.Z = 0; PosXYZ.Z = 5; }
if (PosXYZ.Z < 0)
{
SpdPRY = Vector3.Zero;
@ -241,23 +248,26 @@ namespace DroneSimulator
//Active = false; // Перевернулся вверх ногами
}
Quaternion grav = new Quaternion(AccXYZ.X, AccXYZ.Y, AccXYZ.Z, 0);
//grav = (Quat * grav) * Quaternion.Inverse(Quat); // Инерциальный акселерометр (тест)
Quaternion grav = Quat * new Quaternion(0, 0, flow/Mass, 0) * Quaternion.Inverse(Quat);
Acc = new Vector3(grav.X, grav.Y, grav.Z);
Gyr = SpdPRY;
float tilt = (MathF.Abs(ori.X) + MathF.Abs(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);
else LaserRange = float.MaxValue;
}
MoveOF.X += SpdXYZ.X - Gyr.Y;
MoveOF.Y += SpdXYZ.Y + Gyr.X;
RealAcc.Update(Acc, (uint)tick);
RealGyr.Update(Gyr, (uint)tick);
RealRange.Update(LaserRange, (uint)tick);
RealBar.Update(PosXYZ.Z * 11, (uint)tick);
RealPos.Update(PosXYZ, (uint)tick);
RealOF.Update(MoveOF, LaserRange, (uint)tick);
DataTimer = (uint)tick;
}
@ -378,6 +388,24 @@ namespace DroneSimulator
return getBytes(bar);
}
private byte[] SendDataOF()
{
DroneData.DataOF of = new DroneData.DataOF();
of.Head.Size = Marshal.SizeOf(typeof(DroneData.DataOF));
of.Head.Mode = DroneData.DataMode.Response;
of.Head.Type = DroneData.DataType.DataOF;
of.Head.Time = (uint)Environment.TickCount;
of.X = RealOF.result.X;
of.Y = RealOF.result.Y;
of.Time = RealBar.timer;
MoveOF = Vector2.Zero;
return getBytes(of);
}
private byte[] SendDataQuaternion()
{
DroneData.DataQuat quat = new DroneData.DataQuat();
@ -424,6 +452,8 @@ namespace DroneSimulator
case DroneData.DataType.DataBar: if (head.Mode == DroneData.DataMode.Request) return SendDataBarometer(); else return zero;
case DroneData.DataType.DataOF: if (head.Mode == DroneData.DataMode.Request) return SendDataOF(); else return zero;
case DroneData.DataType.DataQuat: if (head.Mode == DroneData.DataMode.Request) return SendDataQuaternion(); else return zero;
case DroneData.DataType.DataMotor4: if (head.Mode == DroneData.DataMode.Response) RecvDataMotor4(body); return zero;