rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-10 18:24:11 +01:00
Code Issues Releases Activity

OP-223 Basic support for X-Plane implemented. New Wiki documentation should be written, but basically: use 127.0.0.2 as the local IP, 127.0.0.1 as the destination IP, choose a (free) port as the input port which you will setup in the inet3 tab with dest address 127.0.0.2, and output port as 49000. Enjoy!

Browse Source 
One potential issue: X-Plane expects 4-byte floats, the code is potentially not 64bit compliant...



git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2281 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
edouard 2010-12-24 18:35:34 +00:00 committed by edouard
parent 7ab6dfeeba
commit d96a9aace9
6 changed files with 192 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
ground/src/plugins/hitlnew/fgsimulator.cpp
View File

@ -34,11 +34,6 @@
#define M_PI 3.14159265358979323846 #define M_PI 3.14159265358979323846
#endif #endif
const float FGSimulator::FT2M = 0.3048;
const float FGSimulator::KT2MPS = 0.514444444;
const float FGSimulator::INHG2KPA = 3.386;
const float FGSimulator::FPS2CMPS = 30.48;
const float FGSimulator::DEG2RAD = (M_PI/180.0);

5
ground/src/plugins/hitlnew/fgsimulator.h
View File

@ -47,11 +47,6 @@ private slots:
void processReadyRead(); void processReadyRead();
private: private:
static const float FT2M;
static const float KT2MPS;
static const float INHG2KPA;
static const float FPS2CMPS;
static const float DEG2RAD;
int udpCounterGCSsend; //keeps track of udp packets sent to FG int udpCounterGCSsend; //keeps track of udp packets sent to FG
int udpCounterFGrecv; //keeps track of udp packets received by FG int udpCounterFGrecv; //keeps track of udp packets received by FG

7
ground/src/plugins/hitlnew/simulator.cpp
View File

@ -34,6 +34,13 @@
volatile bool Simulator::isStarted = false; volatile bool Simulator::isStarted = false;
const float Simulator::FT2M = 0.3048;
const float Simulator::KT2MPS = 0.514444444;
const float Simulator::INHG2KPA = 3.386;
const float Simulator::FPS2CMPS = 30.48;
const float Simulator::DEG2RAD = (M_PI/180.0);
Simulator::Simulator(const SimulatorSettings& params) : Simulator::Simulator(const SimulatorSettings& params) :
simProcess(NULL), simProcess(NULL),
time(NULL), time(NULL),

7
ground/src/plugins/hitlnew/simulator.h
View File

@ -156,6 +156,12 @@ private slots:
virtual void processUpdate(const QByteArray& data) = 0; virtual void processUpdate(const QByteArray& data) = 0;
protected: protected:
static const float FT2M;
static const float KT2MPS;
static const float INHG2KPA;
static const float FPS2CMPS;
static const float DEG2RAD;
QProcess* simProcess; QProcess* simProcess;
QTime* time; QTime* time;
QUdpSocket* inSocket;//(new QUdpSocket()); QUdpSocket* inSocket;//(new QUdpSocket());
@ -179,6 +185,7 @@ protected:
QMutex lock; QMutex lock;
private: private:
int updatePeriod; int updatePeriod;
int simTimeout; int simTimeout;
volatile bool autopilotConnectionStatus; volatile bool autopilotConnectionStatus;

190
ground/src/plugins/hitlnew/xplanesimulator.cpp
View File

@ -68,6 +68,7 @@ void TraceBuf(const char* buf,int len);
XplaneSimulator::XplaneSimulator(const SimulatorSettings& params) : XplaneSimulator::XplaneSimulator(const SimulatorSettings& params) :
Simulator(params) Simulator(params)
{ {
once = false;
} }
@ -78,7 +79,9 @@ XplaneSimulator::~XplaneSimulator()
void XplaneSimulator::setupUdpPorts(const QString& host, int inPort, int outPort) void XplaneSimulator::setupUdpPorts(const QString& host, int inPort, int outPort)
{ {
inSocket->bind(QHostAddress(host), inPort); inSocket->bind(QHostAddress(host), inPort);
outSocket->bind(QHostAddress(host), outPort); //outSocket->bind(QHostAddress(host), outPort);
once = false;
} }
/** /**
@ -92,33 +95,75 @@ void XplaneSimulator::transmitUpdate()
float elevator = actData.Pitch; float elevator = actData.Pitch;
float rudder = actData.Yaw; float rudder = actData.Yaw;
float throttle = actData.Throttle*2-1.0; float throttle = actData.Throttle*2-1.0;
float tmp = -999; float none = -999;
quint32 none = *((quint32*)&tmp); // get float as 4 bytes //quint32 none = *((quint32*)&tmp); // get float as 4 bytes
quint32 code; quint32 code;
QByteArray buf; QByteArray buf;
QDataStream stream(&buf,QIODevice::ReadWrite); QDataStream stream(&buf,QIODevice::ReadWrite);
// !!! LAN byte order - Big Endian // !!! LAN byte order - Big Endian
//#if Q_BYTE_ORDER == Q_LITTLE_ENDIAN #if Q_BYTE_ORDER == Q_LITTLE_ENDIAN
// stream.setByteOrder(QDataStream::LittleEndian); stream.setByteOrder(QDataStream::LittleEndian);
//#endif #endif
// 11th data settings (flight con: ail/elv/rud) // 11th data settings (flight con: ail/elv/rud)
buf.clear(); buf.clear();
code = 11; code = 11;
quint8 header[] = "DATA"; //quint8 header[] = "DATA";
stream << *((quint32*)header) << (quint8)0x30 << code << *((quint32*)&elevator) << *((quint32*)&ailerons) << *((quint32*)&rudder) /*
<< none << *((quint32*)&ailerons) << none << none << none; stream << *((quint32*)header) <<
(quint8)0x30 <<
code <<
*((quint32*)&elevator) <<
*((quint32*)&ailerons) <<
*((quint32*)&rudder) <<
none <<
*((quint32*)&ailerons) <<
none <<
none <<
none;
*/
buf.append("DATA0");
buf.append(reinterpret_cast<const char*>(&code), sizeof(code));
buf.append(reinterpret_cast<const char*>(&elevator), sizeof(elevator));
buf.append(reinterpret_cast<const char*>(&ailerons), sizeof(ailerons));
buf.append(reinterpret_cast<const char*>(&rudder), sizeof(rudder));
buf.append(reinterpret_cast<const char*>(&none), sizeof(none));
buf.append(reinterpret_cast<const char*>(&rudder), sizeof(rudder));
buf.append(reinterpret_cast<const char*>(&none), sizeof(none));
buf.append(reinterpret_cast<const char*>(&none), sizeof(none));
buf.append(reinterpret_cast<const char*>(&none), sizeof(none));
TraceBuf(buf.data(),41); TraceBuf(buf.data(),41);
outSocket->write(buf);
if(outSocket->writeDatagram(buf, QHostAddress(settings.remoteHostAddress), settings.outPort) == -1)
{
emit processOutput("Error sending UDP packet to XPlane: " + outSocket->errorString() + "\n");
}
//outSocket->write(buf);
// 25th data settings (throttle command) // 25th data settings (throttle command)
buf.clear(); buf.clear();
code = 25; code = 25;
stream << *((quint32*)header) << (quint8)0x30 << code << *((quint32*)&throttle) << none << none //stream << *((quint32*)header) << (quint8)0x30 << code << *((quint32*)&throttle) << none << none
<< none << none << none << none << none; // << none << none << none << none << none;
outSocket->write(buf); buf.append("DATA0");
buf.append(reinterpret_cast<const char*>(&code), sizeof(code));
buf.append(reinterpret_cast<const char*>(&throttle), sizeof(throttle));
buf.append(reinterpret_cast<const char*>(&throttle), sizeof(throttle));
buf.append(reinterpret_cast<const char*>(&none), sizeof(none));
buf.append(reinterpret_cast<const char*>(&none), sizeof(none));
buf.append(reinterpret_cast<const char*>(&none), sizeof(none));
buf.append(reinterpret_cast<const char*>(&none), sizeof(none));
buf.append(reinterpret_cast<const char*>(&none), sizeof(none));
buf.append(reinterpret_cast<const char*>(&none), sizeof(none));
if(outSocket->writeDatagram(buf, QHostAddress(settings.remoteHostAddress), settings.outPort) == -1)
{
emit processOutput("Error sending UDP packet to XPlane: " + outSocket->errorString() + "\n");
}
//outSocket->write(buf);
@ -147,7 +192,16 @@ void XplaneSimulator::processUpdate(const QByteArray& dataBuf)
float roll = 0; float roll = 0;
float heading = 0; float heading = 0;
float pressure = 0; float pressure = 0;
float weather = 0; float temperature = 0;
float velX = 0;
float velY = 0;
float velZ = 0;
float dstX = 0;
float dstY = 0;
float dstZ = 0;
float rollRate=0;
float pitchRate=0;
float yawRate=0;
QString str; QString str;
QByteArray& buf = const_cast<QByteArray&>(dataBuf); QByteArray& buf = const_cast<QByteArray&>(dataBuf);
@ -170,12 +224,12 @@ void XplaneSimulator::processUpdate(const QByteArray& dataBuf)
case XplaneSimulator::LatitudeLongitude: case XplaneSimulator::LatitudeLongitude:
latitude = *((float*)(buf.data()+4*1)); latitude = *((float*)(buf.data()+4*1));
longitude = *((float*)(buf.data()+4*2)); longitude = *((float*)(buf.data()+4*2));
altitude = *((float*)(buf.data()+4*3)) /* * 3.048 */; altitude = *((float*)(buf.data()+4*3))* FT2M;
break; break;
case XplaneSimulator::Speed: case XplaneSimulator::Speed:
airspeed = *((float*)(buf.data()+4*6)); airspeed = *((float*)(buf.data()+4*7));
speed = *((float*)(buf.data()+4*7)); speed = *((float*)(buf.data()+4*8));
break; break;
case XplaneSimulator::PitchRollHeading: case XplaneSimulator::PitchRollHeading:
@ -184,12 +238,30 @@ void XplaneSimulator::processUpdate(const QByteArray& dataBuf)
heading = *((float*)(buf.data()+4*3)); heading = *((float*)(buf.data()+4*3));
break; break;
/*
case XplaneSimulator::SystemPressures: case XplaneSimulator::SystemPressures:
pressure = *((float*)(buf.data()+4*1)); pressure = *((float*)(buf.data()+4*1));
break; break;
*/
case XplaneSimulator::AtmosphereWeather: case XplaneSimulator::AtmosphereWeather:
weather = *((float*)(buf.data()+4*1)); pressure = *((float*)(buf.data()+4*1)) * INHG2KPA;
temperature = *((float*)(buf.data()+4*2));
break;
case XplaneSimulator::LocVelDistTraveled:
dstX = *((float*)(buf.data()+4*1));
dstY = *((float*)(buf.data()+4*2));
dstZ = *((float*)(buf.data()+4*3));
velX = *((float*)(buf.data()+4*4));
velY = *((float*)(buf.data()+4*5));
velZ = *((float*)(buf.data()+4*6));
break;
case XplaneSimulator::AngularVelocities:
pitchRate = *((float*)(buf.data()+4*1));
rollRate = *((float*)(buf.data()+4*2));
yawRate = *((float*)(buf.data()+4*3));
break; break;
default: default:
@ -199,11 +271,50 @@ void XplaneSimulator::processUpdate(const QByteArray& dataBuf)
buf.remove(0,36); buf.remove(0,36);
} while (channelCounter); } while (channelCounter);
HomeLocation::DataFields homeData = posHome->getData();
if(!once)
{
// Upon startup, we reset the HomeLocation object to
// the plane's location:
memset(&homeData, 0, sizeof(HomeLocation::DataFields));
// Update homelocation
homeData.Latitude = latitude * 1e7;
homeData.Longitude = longitude * 1e7;
homeData.Altitude = altitude;
double LLA[3];
LLA[0]=latitude;
LLA[1]=longitude;
LLA[2]=altitude;
double ECEF[3];
double RNE[9];
Utils::CoordinateConversions().RneFromLLA(LLA,(double (*)[3])RNE);
for (int t=0;t<9;t++) {
homeData.RNE[t]=RNE[t];
}
Utils::CoordinateConversions().LLA2ECEF(LLA,ECEF);
homeData.ECEF[0]=ECEF[0]*100;
homeData.ECEF[1]=ECEF[1]*100;
homeData.ECEF[2]=ECEF[2]*100;
homeData.Be[0]=0;
homeData.Be[1]=0;
homeData.Be[2]=0;
posHome->setData(homeData);
posHome->updated();
// Initialize the initial distance
initX = dstX;
initY = dstY;
initZ = dstZ;
once=1;
}
// Update AltitudeActual object // Update AltitudeActual object
BaroAltitude::DataFields altActualData; BaroAltitude::DataFields altActualData;
memset(&altActualData, 0, sizeof(BaroAltitude::DataFields)); memset(&altActualData, 0, sizeof(BaroAltitude::DataFields));
altActualData.Altitude = altitude; altActualData.Altitude = altitude;
altActualData.Temperature = weather; altActualData.Temperature = temperature;
altActualData.Pressure = pressure; altActualData.Pressure = pressure;
altActual->setData(altActualData); altActual->setData(altActualData);
@ -212,7 +323,7 @@ void XplaneSimulator::processUpdate(const QByteArray& dataBuf)
memset(&attActualData, 0, sizeof(AttitudeActual::DataFields)); memset(&attActualData, 0, sizeof(AttitudeActual::DataFields));
attActualData.Roll = roll; //roll; attActualData.Roll = roll; //roll;
attActualData.Pitch = pitch; // pitch attActualData.Pitch = pitch; // pitch
// attActualData.Yaw = yaw; attActualData.Yaw = heading; // Yaw
// attActualData.q1 = 0; // attActualData.q1 = 0;
// attActualData.q2 = 0; // attActualData.q2 = 0;
// attActualData.q3 = 0; // attActualData.q3 = 0;
@ -223,11 +334,42 @@ void XplaneSimulator::processUpdate(const QByteArray& dataBuf)
GPSPosition::DataFields gpsData; GPSPosition::DataFields gpsData;
memset(&gpsData, 0, sizeof(GPSPosition::DataFields)); memset(&gpsData, 0, sizeof(GPSPosition::DataFields));
gpsData.Altitude = altitude; gpsData.Altitude = altitude;
// gpsData.Heading = pitch[2]; gpsData.Heading = heading;
// gpsData.Groundspeed = speed[0]; gpsData.Groundspeed = speed;
gpsData.Latitude = latitude; gpsData.Latitude = latitude*1e7;
gpsData.Longitude = longitude; gpsData.Longitude = longitude*1e7;
gpsData.Satellites = 10;
gpsData.Status = GPSPosition::STATUS_FIX3D;
gpsPos->setData(gpsData); gpsPos->setData(gpsData);
// Update VelocityActual.{Nort,East,Down}
VelocityActual::DataFields velocityActualData;
memset(&velocityActualData, 0, sizeof(VelocityActual::DataFields));
velocityActualData.North = velX*100;
velocityActualData.East = velY*100;
velocityActualData.Down = velZ*100;
velActual->setData(velocityActualData);
// Update PositionActual.{Nort,East,Down}
PositionActual::DataFields positionActualData;
memset(&positionActualData, 0, sizeof(PositionActual::DataFields));
positionActualData.North = (dstX-initX)*100;
positionActualData.East = (dstY-initY)*100;
positionActualData.Down = (dstZ-initZ)*100;
posActual->setData(positionActualData);
// Update AttitudeRaw object (filtered gyros only for now)
AttitudeRaw::DataFields rawData;
memset(&rawData, 0, sizeof(AttitudeRaw::DataFields));
rawData = attRaw->getData();
rawData.gyros_filtered[0] = rollRate;
//rawData.gyros_filtered[1] = cos(DEG2RAD * roll) * pitchRate + sin(DEG2RAD * roll) * yawRate;
//rawData.gyros_filtered[2] = cos(DEG2RAD * roll) * yawRate - sin(DEG2RAD * roll) * pitchRate;
rawData.gyros_filtered[1] = pitchRate;
rawData.gyros_filtered[2] = yawRate;
attRaw->setData(rawData);
} }
// issue manual update // issue manual update
//attActual->updated(); //attActual->updated();

6
ground/src/plugins/hitlnew/xplanesimulator.h
View File

@ -44,6 +44,10 @@ private slots:
void transmitUpdate(); void transmitUpdate();
private: private:
bool once;
float initX;
float initY;
float initZ;
enum XplaneOutputData enum XplaneOutputData
{ {
FramRate, FramRate,
@ -63,7 +67,7 @@ private:
Brakes, Brakes,
AngularMoments, AngularMoments,
AngularAccelerations, AngularAccelerations,
AngularVelociies, AngularVelocities,
PitchRollHeading, PitchRollHeading,
AoA, AoA,
LatitudeLongitude, LatitudeLongitude,