mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 22:48:29 -04:00
25bcfdd1e7
add arduino chip detect fix apm2,2.5 dialog test add write timeout. this will stop planner hangs on bad serial devices. change quickview decimal places to 0.00 fix map clicking issue. fix wind direction wrapping add airspeed use modify firmware screen from Marooned major flightdata tab change. add save/load polygon from file add some error handling dialogs
360 lines
14 KiB
C#
360 lines
14 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Linq;
|
|
using System.Text;
|
|
using System.Net;
|
|
using System.Net.Sockets;
|
|
|
|
namespace ArdupilotMega.HIL
|
|
{
|
|
class XPlane : Hil
|
|
{
|
|
Socket SimulatorRECV;
|
|
UdpClient XplanesSEND;
|
|
EndPoint Remote = (EndPoint)(new IPEndPoint(IPAddress.Any, 0));
|
|
|
|
// place to store the xplane packet data
|
|
float[][] DATA = new float[113][];
|
|
|
|
public override void SetupSockets(int recvPort, int SendPort, string simIP)
|
|
{
|
|
// setup receiver
|
|
IPEndPoint ipep = new IPEndPoint(IPAddress.Any, recvPort);
|
|
|
|
SimulatorRECV = new Socket(AddressFamily.InterNetwork,
|
|
SocketType.Dgram, ProtocolType.Udp);
|
|
|
|
SimulatorRECV.Bind(ipep);
|
|
|
|
UpdateStatus(-1, "Listerning on port UDP " + recvPort + " (sim->planner)\n");
|
|
|
|
|
|
// setup sender
|
|
XplanesSEND = new UdpClient(simIP, SendPort);
|
|
|
|
UpdateStatus(-1, "Sending to port UDP " + SendPort + " (planner->sim)\n");
|
|
|
|
setupXplane();
|
|
|
|
UpdateStatus(-1, "Sent xplane settings\n");
|
|
}
|
|
public override void Shutdown()
|
|
{
|
|
try
|
|
{
|
|
SimulatorRECV.Close();
|
|
}
|
|
catch { }
|
|
try
|
|
{
|
|
XplanesSEND.Close();
|
|
}
|
|
catch { }
|
|
}
|
|
|
|
public override void GetFromSim(ref sitl_fdm sitldata)
|
|
{
|
|
if (SimulatorRECV.Available > 0)
|
|
{
|
|
byte[] udpdata = new byte[1500];
|
|
int receviedbytes = 0;
|
|
try
|
|
{
|
|
while (SimulatorRECV.Available > 0)
|
|
{
|
|
receviedbytes = SimulatorRECV.ReceiveFrom(udpdata, ref Remote);
|
|
}
|
|
}
|
|
catch { }
|
|
|
|
if (udpdata[0] == 'D' && udpdata[1] == 'A')
|
|
{
|
|
// Xplanes sends
|
|
// 5 byte header
|
|
// 1 int for the index - numbers on left of output
|
|
// 8 floats - might be useful. or 0 if not
|
|
int count = 5;
|
|
while (count < receviedbytes)
|
|
{
|
|
int index = BitConverter.ToInt32(udpdata, count);
|
|
|
|
DATA[index] = new float[8];
|
|
|
|
DATA[index][0] = BitConverter.ToSingle(udpdata, count + 1 * 4); ;
|
|
DATA[index][1] = BitConverter.ToSingle(udpdata, count + 2 * 4); ;
|
|
DATA[index][2] = BitConverter.ToSingle(udpdata, count + 3 * 4); ;
|
|
DATA[index][3] = BitConverter.ToSingle(udpdata, count + 4 * 4); ;
|
|
DATA[index][4] = BitConverter.ToSingle(udpdata, count + 5 * 4); ;
|
|
DATA[index][5] = BitConverter.ToSingle(udpdata, count + 6 * 4); ;
|
|
DATA[index][6] = BitConverter.ToSingle(udpdata, count + 7 * 4); ;
|
|
DATA[index][7] = BitConverter.ToSingle(udpdata, count + 8 * 4); ;
|
|
|
|
count += 36; // 8 * float
|
|
}
|
|
|
|
bool xplane9 = !xplane10;
|
|
|
|
if (xplane9)
|
|
{
|
|
sitldata.pitchDeg = (DATA[18][0]);
|
|
sitldata.rollDeg = (DATA[18][1]);
|
|
sitldata.yawDeg = (DATA[18][2]);
|
|
sitldata.pitchRate = (DATA[17][0] * rad2deg);
|
|
sitldata.rollRate = (DATA[17][1] * rad2deg);
|
|
sitldata.yawRate = (DATA[17][2] * rad2deg);
|
|
|
|
sitldata.heading = ((float)DATA[19][2]);
|
|
}
|
|
else
|
|
{
|
|
sitldata.pitchDeg = (DATA[17][0]);
|
|
sitldata.rollDeg = (DATA[17][1]);
|
|
sitldata.yawDeg = (DATA[17][2]);
|
|
sitldata.pitchRate = (DATA[16][0] * rad2deg);
|
|
sitldata.rollRate = (DATA[16][1] * rad2deg);
|
|
sitldata.yawRate = (DATA[16][2] * rad2deg);
|
|
|
|
sitldata.heading = (DATA[18][2]);
|
|
}
|
|
|
|
sitldata.airspeed = ((DATA[3][5] * .44704));
|
|
|
|
sitldata.latitude = (DATA[20][0]);
|
|
sitldata.longitude = (DATA[20][1]);
|
|
sitldata.altitude = (DATA[20][2] * ft2m);
|
|
|
|
sitldata.speedN = DATA[21][3];// (DATA[3][7] * 0.44704 * Math.Sin(sitldata.heading * deg2rad));
|
|
sitldata.speedE = -DATA[21][5];// (DATA[3][7] * 0.44704 * Math.Cos(sitldata.heading * deg2rad));
|
|
|
|
Matrix3 dcm = new Matrix3();
|
|
dcm.from_euler(sitldata.rollDeg * deg2rad, sitldata.pitchDeg * deg2rad, sitldata.yawDeg * deg2rad);
|
|
|
|
// rad = tas^2 / (tan(angle) * G)
|
|
float turnrad = (float)(((DATA[3][7] * 0.44704) * (DATA[3][7] * 0.44704)) / (float)(9.8f * Math.Tan(sitldata.rollDeg * deg2rad)));
|
|
|
|
float gload = (float)(1 / Math.Cos(sitldata.rollDeg * deg2rad)); // calculated Gs
|
|
|
|
// a = v^2/r
|
|
float centripaccel = (float)((DATA[3][7] * 0.44704) * (DATA[3][7] * 0.44704)) / turnrad;
|
|
|
|
Vector3 accel_body = dcm.transposed() * (new Vector3(0, 0, -9.8));
|
|
|
|
Vector3 centrip_accel = new Vector3(0, centripaccel * Math.Cos(sitldata.rollDeg * deg2rad), centripaccel * Math.Sin(sitldata.rollDeg * deg2rad));
|
|
|
|
accel_body -= centrip_accel;
|
|
|
|
sitldata.xAccel = DATA[4][5] * 9.8;
|
|
sitldata.yAccel = DATA[4][6] * 9.8;
|
|
sitldata.zAccel = (0 - DATA[4][4]) * 9.8;
|
|
|
|
// Console.WriteLine(accel_body.ToString());
|
|
// Console.WriteLine(" {0} {1} {2}",sitldata.xAccel, sitldata.yAccel, sitldata.zAccel);
|
|
|
|
}
|
|
}
|
|
}
|
|
public override void SendToSim()
|
|
{
|
|
roll_out = (float)MainV2.cs.hilch1 / rollgain;
|
|
pitch_out = (float)MainV2.cs.hilch2 / pitchgain;
|
|
throttle_out = ((float)MainV2.cs.hilch3) / throttlegain;
|
|
rudder_out = (float)MainV2.cs.hilch4 / ruddergain;
|
|
|
|
// Limit min and max
|
|
roll_out = Constrain(roll_out, -1, 1);
|
|
pitch_out = Constrain(pitch_out, -1, 1);
|
|
rudder_out = Constrain(rudder_out, -1, 1);
|
|
throttle_out = Constrain(throttle_out, 0, 1);
|
|
|
|
// sending only 1 packet instead of many.
|
|
|
|
byte[] Xplane = new byte[5 + 36 + 36];
|
|
|
|
if (heli)
|
|
{
|
|
Xplane = new byte[5 + 36 + 36 + 36];
|
|
}
|
|
|
|
Xplane[0] = (byte)'D';
|
|
Xplane[1] = (byte)'A';
|
|
Xplane[2] = (byte)'T';
|
|
Xplane[3] = (byte)'A';
|
|
Xplane[4] = 0;
|
|
|
|
Array.Copy(BitConverter.GetBytes((int)25), 0, Xplane, 5, 4); // packet index
|
|
|
|
Array.Copy(BitConverter.GetBytes((float)throttle_out), 0, Xplane, 9, 4); // start data
|
|
Array.Copy(BitConverter.GetBytes((float)throttle_out), 0, Xplane, 13, 4);
|
|
Array.Copy(BitConverter.GetBytes((float)throttle_out), 0, Xplane, 17, 4);
|
|
Array.Copy(BitConverter.GetBytes((float)throttle_out), 0, Xplane, 21, 4);
|
|
|
|
Array.Copy(BitConverter.GetBytes((int)-999), 0, Xplane, 25, 4);
|
|
Array.Copy(BitConverter.GetBytes((int)-999), 0, Xplane, 29, 4);
|
|
Array.Copy(BitConverter.GetBytes((int)-999), 0, Xplane, 33, 4);
|
|
Array.Copy(BitConverter.GetBytes((int)-999), 0, Xplane, 37, 4);
|
|
|
|
// NEXT ONE - control surfaces
|
|
|
|
Array.Copy(BitConverter.GetBytes((int)11), 0, Xplane, 41, 4); // packet index
|
|
|
|
Array.Copy(BitConverter.GetBytes((float)(pitch_out * REV_pitch)), 0, Xplane, 45, 4); // start data
|
|
Array.Copy(BitConverter.GetBytes((float)(roll_out * REV_roll)), 0, Xplane, 49, 4);
|
|
Array.Copy(BitConverter.GetBytes((float)(rudder_out * REV_rudder)), 0, Xplane, 53, 4);
|
|
Array.Copy(BitConverter.GetBytes((int)-999), 0, Xplane, 57, 4);
|
|
|
|
Array.Copy(BitConverter.GetBytes((float)(roll_out * REV_roll * 0.5)), 0, Xplane, 61, 4);
|
|
Array.Copy(BitConverter.GetBytes((int)-999), 0, Xplane, 65, 4);
|
|
Array.Copy(BitConverter.GetBytes((int)-999), 0, Xplane, 69, 4);
|
|
Array.Copy(BitConverter.GetBytes((int)-999), 0, Xplane, 73, 4);
|
|
|
|
if (heli)
|
|
{
|
|
Array.Copy(BitConverter.GetBytes((float)(0)), 0, Xplane, 53, 4);
|
|
|
|
|
|
int a = 73 + 4;
|
|
Array.Copy(BitConverter.GetBytes((int)39), 0, Xplane, a, 4); // packet index
|
|
a += 4;
|
|
Array.Copy(BitConverter.GetBytes((float)(12 * collective_out)), 0, Xplane, a, 4); // main rotor 0 - 12
|
|
a += 4;
|
|
Array.Copy(BitConverter.GetBytes((float)(12 * rudder_out)), 0, Xplane, a, 4); // tail rotor -12 - 12
|
|
a += 4;
|
|
Array.Copy(BitConverter.GetBytes((int)-999), 0, Xplane, a, 4);
|
|
a += 4;
|
|
Array.Copy(BitConverter.GetBytes((int)-999), 0, Xplane, a, 4);
|
|
a += 4;
|
|
Array.Copy(BitConverter.GetBytes((int)-999), 0, Xplane, a, 4);
|
|
a += 4;
|
|
Array.Copy(BitConverter.GetBytes((int)-999), 0, Xplane, a, 4);
|
|
a += 4;
|
|
Array.Copy(BitConverter.GetBytes((int)-999), 0, Xplane, a, 4);
|
|
a += 4;
|
|
Array.Copy(BitConverter.GetBytes((int)-999), 0, Xplane, a, 4);
|
|
}
|
|
|
|
try
|
|
{
|
|
XplanesSEND.Send(Xplane, Xplane.Length);
|
|
|
|
}
|
|
catch (Exception e) { log.Info("Xplanes udp send error " + e.Message); }
|
|
}
|
|
public override void SendToAP(sitl_fdm sitldata)
|
|
{
|
|
TimeSpan gpsspan = DateTime.Now - lastgpsupdate;
|
|
|
|
// add gps delay
|
|
if (gpsspan.TotalMilliseconds >= GPS_rate)
|
|
{
|
|
lastgpsupdate = DateTime.Now;
|
|
|
|
// save current fix = 3
|
|
sitl_fdmbuffer[gpsbufferindex % sitl_fdmbuffer.Length] = sitldata;
|
|
|
|
// Console.WriteLine((gpsbufferindex % gpsbuffer.Length) + " " + ((gpsbufferindex + (gpsbuffer.Length - 1)) % gpsbuffer.Length));
|
|
|
|
// return buffer index + 5 = (3 + 5) = 8 % 6 = 2
|
|
oldgps = sitl_fdmbuffer[(gpsbufferindex + (sitl_fdmbuffer.Length - 1)) % sitl_fdmbuffer.Length];
|
|
|
|
//comPort.sendPacket(oldgps);
|
|
|
|
gpsbufferindex++;
|
|
}
|
|
|
|
MAVLink.mavlink_hil_state_t hilstate = new MAVLink.mavlink_hil_state_t();
|
|
|
|
hilstate.time_usec = (UInt64)DateTime.Now.Ticks; // microsec
|
|
|
|
hilstate.lat = (int)(oldgps.latitude * 1e7); // * 1E7
|
|
hilstate.lon = (int)(oldgps.longitude * 1e7); // * 1E7
|
|
hilstate.alt = (int)(oldgps.altitude * 1000); // mm
|
|
|
|
// Console.WriteLine(hilstate.alt);
|
|
|
|
hilstate.pitch = (float)sitldata.pitchDeg * deg2rad; // (rad)
|
|
hilstate.pitchspeed = (float)sitldata.pitchRate * deg2rad; // (rad/s)
|
|
hilstate.roll = (float)sitldata.rollDeg * deg2rad; // (rad)
|
|
hilstate.rollspeed = (float)sitldata.rollRate * deg2rad; // (rad/s)
|
|
hilstate.yaw = (float)sitldata.yawDeg * deg2rad; // (rad)
|
|
hilstate.yawspeed = (float)sitldata.yawRate * deg2rad; // (rad/s)
|
|
|
|
hilstate.vx = (short)(sitldata.speedN * 100); // m/s * 100
|
|
hilstate.vy = (short)(sitldata.speedE * 100); // m/s * 100
|
|
hilstate.vz = 0; // m/s * 100
|
|
|
|
hilstate.xacc = (short)(sitldata.xAccel * 1000); // (mg)
|
|
hilstate.yacc = (short)(sitldata.yAccel * 1000); // (mg)
|
|
hilstate.zacc = (short)(sitldata.zAccel * 1000); // (mg)
|
|
|
|
MainV2.comPort.sendPacket(hilstate);
|
|
|
|
MainV2.comPort.sendPacket(new MAVLink.mavlink_vfr_hud_t()
|
|
{
|
|
airspeed = (float)sitldata.airspeed
|
|
});
|
|
}
|
|
public override void GetFromAP() { }
|
|
|
|
void setupXplane()
|
|
{
|
|
// sending only 1 packet instead of many.
|
|
|
|
byte[] Xplane = new byte[5 + 4 * 8];
|
|
|
|
Xplane[0] = (byte)'D';
|
|
Xplane[1] = (byte)'S';
|
|
Xplane[2] = (byte)'E';
|
|
Xplane[3] = (byte)'L';
|
|
Xplane[4] = 0;
|
|
|
|
if (xplane10)
|
|
{
|
|
int pos = 5;
|
|
Xplane[pos] = 0x3;
|
|
pos += 4;
|
|
Xplane[pos] = 0x4;
|
|
pos += 4;
|
|
Xplane[pos] = 0x6;
|
|
pos += 4;
|
|
Xplane[pos] = 0x10;
|
|
pos += 4;
|
|
Xplane[pos] = 0x11;
|
|
pos += 4;
|
|
Xplane[pos] = 0x12;
|
|
pos += 4;
|
|
Xplane[pos] = 0x14;
|
|
pos += 4;
|
|
Xplane[pos] = 0x15;
|
|
pos += 4;
|
|
}
|
|
else
|
|
{
|
|
int pos = 5;
|
|
Xplane[pos] = 0x3;
|
|
pos += 4;
|
|
Xplane[pos] = 0x4;
|
|
pos += 4;
|
|
Xplane[pos] = 0x6;
|
|
pos += 4;
|
|
Xplane[pos] = 0x11;
|
|
pos += 4;
|
|
Xplane[pos] = 0x12;
|
|
pos += 4;
|
|
Xplane[pos] = 0x13;
|
|
pos += 4;
|
|
Xplane[pos] = 0x14;
|
|
pos += 4;
|
|
Xplane[pos] = 0x15;
|
|
pos += 4;
|
|
}
|
|
|
|
try
|
|
{
|
|
XplanesSEND.Send(Xplane, Xplane.Length);
|
|
|
|
}
|
|
catch (Exception e) { log.Info("Xplanes udp send error " + e.Message); }
|
|
|
|
}
|
|
}
|
|
} |