From d96a9aace92b1a903cd730273a82fa79d3647b0b Mon Sep 17 00:00:00 2001 From: edouard Date: Fri, 24 Dec 2010 18:35:34 +0000 Subject: [PATCH] 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! 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 --- ground/src/plugins/hitlnew/fgsimulator.cpp | 5 - ground/src/plugins/hitlnew/fgsimulator.h | 5 - ground/src/plugins/hitlnew/simulator.cpp | 7 + ground/src/plugins/hitlnew/simulator.h | 9 +- .../src/plugins/hitlnew/xplanesimulator.cpp | 202 +++++++++++++++--- ground/src/plugins/hitlnew/xplanesimulator.h | 6 +- 6 files changed, 192 insertions(+), 42 deletions(-) diff --git a/ground/src/plugins/hitlnew/fgsimulator.cpp b/ground/src/plugins/hitlnew/fgsimulator.cpp index 101ceaf2c..ec6510a75 100644 --- a/ground/src/plugins/hitlnew/fgsimulator.cpp +++ b/ground/src/plugins/hitlnew/fgsimulator.cpp @@ -34,11 +34,6 @@ #define M_PI 3.14159265358979323846 #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); diff --git a/ground/src/plugins/hitlnew/fgsimulator.h b/ground/src/plugins/hitlnew/fgsimulator.h index f4b8213aa..859b712d0 100644 --- a/ground/src/plugins/hitlnew/fgsimulator.h +++ b/ground/src/plugins/hitlnew/fgsimulator.h @@ -47,11 +47,6 @@ private slots: void processReadyRead(); 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 udpCounterFGrecv; //keeps track of udp packets received by FG diff --git a/ground/src/plugins/hitlnew/simulator.cpp b/ground/src/plugins/hitlnew/simulator.cpp index 6bb12b015..8a9662ce6 100644 --- a/ground/src/plugins/hitlnew/simulator.cpp +++ b/ground/src/plugins/hitlnew/simulator.cpp @@ -34,6 +34,13 @@ 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) : simProcess(NULL), time(NULL), diff --git a/ground/src/plugins/hitlnew/simulator.h b/ground/src/plugins/hitlnew/simulator.h index f3e4b8883..c0d88ef6c 100644 --- a/ground/src/plugins/hitlnew/simulator.h +++ b/ground/src/plugins/hitlnew/simulator.h @@ -156,7 +156,13 @@ private slots: virtual void processUpdate(const QByteArray& data) = 0; protected: - QProcess* simProcess; + static const float FT2M; + static const float KT2MPS; + static const float INHG2KPA; + static const float FPS2CMPS; + static const float DEG2RAD; + + QProcess* simProcess; QTime* time; QUdpSocket* inSocket;//(new QUdpSocket()); QUdpSocket* outSocket; @@ -179,6 +185,7 @@ protected: QMutex lock; private: + int updatePeriod; int simTimeout; volatile bool autopilotConnectionStatus; diff --git a/ground/src/plugins/hitlnew/xplanesimulator.cpp b/ground/src/plugins/hitlnew/xplanesimulator.cpp index df13acccc..d2ee56d9f 100644 --- a/ground/src/plugins/hitlnew/xplanesimulator.cpp +++ b/ground/src/plugins/hitlnew/xplanesimulator.cpp @@ -68,6 +68,7 @@ void TraceBuf(const char* buf,int len); XplaneSimulator::XplaneSimulator(const SimulatorSettings& params) : Simulator(params) { + once = false; } @@ -78,7 +79,9 @@ XplaneSimulator::~XplaneSimulator() void XplaneSimulator::setupUdpPorts(const QString& host, int inPort, int outPort) { 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 rudder = actData.Yaw; float throttle = actData.Throttle*2-1.0; - float tmp = -999; - quint32 none = *((quint32*)&tmp); // get float as 4 bytes + float none = -999; + //quint32 none = *((quint32*)&tmp); // get float as 4 bytes - quint32 code; + quint32 code; QByteArray buf; QDataStream stream(&buf,QIODevice::ReadWrite); -// !!! LAN byte order - Big Endian -//#if Q_BYTE_ORDER == Q_LITTLE_ENDIAN -// stream.setByteOrder(QDataStream::LittleEndian); -//#endif + // !!! LAN byte order - Big Endian + #if Q_BYTE_ORDER == Q_LITTLE_ENDIAN + stream.setByteOrder(QDataStream::LittleEndian); + #endif // 11th data settings (flight con: ail/elv/rud) buf.clear(); - code = 11; - quint8 header[] = "DATA"; - stream << *((quint32*)header) << (quint8)0x30 << code << *((quint32*)&elevator) << *((quint32*)&ailerons) << *((quint32*)&rudder) - << none << *((quint32*)&ailerons) << none << none << none; - TraceBuf(buf.data(),41); - outSocket->write(buf); + code = 11; + //quint8 header[] = "DATA"; + /* + 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(&code), sizeof(code)); + buf.append(reinterpret_cast(&elevator), sizeof(elevator)); + buf.append(reinterpret_cast(&ailerons), sizeof(ailerons)); + buf.append(reinterpret_cast(&rudder), sizeof(rudder)); + buf.append(reinterpret_cast(&none), sizeof(none)); + buf.append(reinterpret_cast(&rudder), sizeof(rudder)); + buf.append(reinterpret_cast(&none), sizeof(none)); + buf.append(reinterpret_cast(&none), sizeof(none)); + buf.append(reinterpret_cast(&none), sizeof(none)); + TraceBuf(buf.data(),41); + + 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) buf.clear(); code = 25; - stream << *((quint32*)header) << (quint8)0x30 << code << *((quint32*)&throttle) << none << none - << none << none << none << none << none; - outSocket->write(buf); + //stream << *((quint32*)header) << (quint8)0x30 << code << *((quint32*)&throttle) << none << none + // << none << none << none << none << none; + buf.append("DATA0"); + buf.append(reinterpret_cast(&code), sizeof(code)); + buf.append(reinterpret_cast(&throttle), sizeof(throttle)); + buf.append(reinterpret_cast(&throttle), sizeof(throttle)); + buf.append(reinterpret_cast(&none), sizeof(none)); + buf.append(reinterpret_cast(&none), sizeof(none)); + buf.append(reinterpret_cast(&none), sizeof(none)); + buf.append(reinterpret_cast(&none), sizeof(none)); + buf.append(reinterpret_cast(&none), sizeof(none)); + buf.append(reinterpret_cast(&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 heading = 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; QByteArray& buf = const_cast(dataBuf); @@ -170,12 +224,12 @@ void XplaneSimulator::processUpdate(const QByteArray& dataBuf) case XplaneSimulator::LatitudeLongitude: latitude = *((float*)(buf.data()+4*1)); longitude = *((float*)(buf.data()+4*2)); - altitude = *((float*)(buf.data()+4*3)) /* * 3.048 */; + altitude = *((float*)(buf.data()+4*3))* FT2M; break; case XplaneSimulator::Speed: - airspeed = *((float*)(buf.data()+4*6)); - speed = *((float*)(buf.data()+4*7)); + airspeed = *((float*)(buf.data()+4*7)); + speed = *((float*)(buf.data()+4*8)); break; case XplaneSimulator::PitchRollHeading: @@ -184,14 +238,32 @@ void XplaneSimulator::processUpdate(const QByteArray& dataBuf) heading = *((float*)(buf.data()+4*3)); break; - case XplaneSimulator::SystemPressures: + /* + case XplaneSimulator::SystemPressures: pressure = *((float*)(buf.data()+4*1)); break; + */ 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; + default: break; } @@ -199,11 +271,50 @@ void XplaneSimulator::processUpdate(const QByteArray& dataBuf) buf.remove(0,36); } 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 BaroAltitude::DataFields altActualData; memset(&altActualData, 0, sizeof(BaroAltitude::DataFields)); altActualData.Altitude = altitude; - altActualData.Temperature = weather; + altActualData.Temperature = temperature; altActualData.Pressure = pressure; altActual->setData(altActualData); @@ -212,7 +323,7 @@ void XplaneSimulator::processUpdate(const QByteArray& dataBuf) memset(&attActualData, 0, sizeof(AttitudeActual::DataFields)); attActualData.Roll = roll; //roll; attActualData.Pitch = pitch; // pitch - // attActualData.Yaw = yaw; + attActualData.Yaw = heading; // Yaw // attActualData.q1 = 0; // attActualData.q2 = 0; // attActualData.q3 = 0; @@ -222,12 +333,43 @@ void XplaneSimulator::processUpdate(const QByteArray& dataBuf) // Update gps objects GPSPosition::DataFields gpsData; memset(&gpsData, 0, sizeof(GPSPosition::DataFields)); - gpsData.Altitude = altitude; -// gpsData.Heading = pitch[2]; -// gpsData.Groundspeed = speed[0]; - gpsData.Latitude = latitude; - gpsData.Longitude = longitude; + gpsData.Altitude = altitude; + gpsData.Heading = heading; + gpsData.Groundspeed = speed; + gpsData.Latitude = latitude*1e7; + gpsData.Longitude = longitude*1e7; + gpsData.Satellites = 10; + gpsData.Status = GPSPosition::STATUS_FIX3D; 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 //attActual->updated(); diff --git a/ground/src/plugins/hitlnew/xplanesimulator.h b/ground/src/plugins/hitlnew/xplanesimulator.h index c5c468fa3..9d2e27c85 100644 --- a/ground/src/plugins/hitlnew/xplanesimulator.h +++ b/ground/src/plugins/hitlnew/xplanesimulator.h @@ -44,6 +44,10 @@ private slots: void transmitUpdate(); private: + bool once; + float initX; + float initY; + float initZ; enum XplaneOutputData { FramRate, @@ -63,7 +67,7 @@ private: Brakes, AngularMoments, AngularAccelerations, - AngularVelociies, + AngularVelocities, PitchRollHeading, AoA, LatitudeLongitude,