[HITL] : - add X-Plane bridge;

- appropriate and needed changes in HITL core files;

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1724 ebee16cc-31ac-478f-84a7-5cbb03baadba

ground/src/plugins/hitlnew/fgsimulator.cpp

@@ -139,7 +139,6 @@ void FGSimulator::transmitUpdate()
 	float elevator = -actData.Pitch;
 	float rudder = actData.Yaw;
 	float throttle = actData.Throttle;
-
 	// Send update to FlightGear
 	QString cmd;
 	cmd = QString("%1,%2,%3,%4\n")
@@ -152,9 +151,10 @@ void FGSimulator::transmitUpdate()
 }
 
 
-void FGSimulator::processUpdate(QString& data)
+void FGSimulator::processUpdate(const QByteArray& inp)
 {
 	// Split
+	QString data(inp);
 	QStringList fields = data.split(",");
 	// Get xRate (deg/s)
 //	float xRate = fields[0].toFloat() * 180.0/M_PI;

ground/src/plugins/hitlnew/fgsimulator.h

@@ -51,7 +51,7 @@ private:
     static const float KT2MPS;
     static const float INHG2KPA;
 
-	void processUpdate(QString& data);
+	void processUpdate(const QByteArray& data);
 };
 
 class FGSimulatorCreator : public SimulatorCreator

ground/src/plugins/hitlnew/hitlnew.pro

@@ -11,7 +11,8 @@ HEADERS += hitlplugin.h \
     hitlgadget.h \
     simulator.h \
     fgsimulator.h \
-    il2simulator.h
+    il2simulator.h \
+    xplanesimulator.h
 SOURCES += hitlplugin.cpp \
     hitlwidget.cpp \
     hitloptionspage.cpp \
@@ -20,7 +21,8 @@ SOURCES += hitlplugin.cpp \
     hitlgadget.cpp \
     simulator.cpp \
     il2simulator.cpp \
-    fgsimulator.cpp
+    fgsimulator.cpp \
+    xplanesimulator.cpp
 OTHER_FILES += hitlnew.pluginspec
 FORMS += hitloptionspage.ui \
     hitlwidget.ui

ground/src/plugins/hitlnew/hitlplugin.cpp

@@ -31,6 +31,7 @@
 #include <extensionsystem/pluginmanager.h>
 #include "fgsimulator.h"
 #include "il2simulator.h"
+#include "xplanesimulator.h"
 
 QList<SimulatorCreator* > HITLPlugin::typeSimulators;
 
@@ -54,6 +55,7 @@ bool HITLPlugin::initialize(const QStringList& args, QString *errMsg)
 
    addSimulator(new FGSimulatorCreator("FG","FlightGear"));
    addSimulator(new IL2SimulatorCreator("IL2","IL2"));
+   addSimulator(new XplaneSimulatorCreator("X-Plane","X-Plane"));
 
    return true;
 }

ground/src/plugins/hitlnew/il2simulator.cpp

@@ -59,6 +59,8 @@
  *
  * unfortunately angular acceleration provided is very limited, too
  */
+
+
 #include "il2simulator.h"
 #include "extensionsystem/pluginmanager.h"
 #include <coreplugin/icore.h>
@@ -150,11 +152,11 @@ float IL2Simulator::TAS(float IAS, float alt) {
 /**
  * process data string from flight simulator
  */
-void IL2Simulator::processUpdate(QString& data)
+void IL2Simulator::processUpdate(const QByteArray& inp)
 {
 	// save old flight data to calculate delta's later
 	old=current;
-
+	QString data(inp);
 	// Split
 	QStringList fields = data.split("/");
 
@@ -208,17 +210,17 @@ void IL2Simulator::processUpdate(QString& data)
 	current.Y = old.Y + (current.dY*current.dT);
 
 	// Update AltitudeActual object
-        BaroAltitude::DataFields altActualData;
-        memset(&altActualData, 0, sizeof(BaroAltitude::DataFields));
-        altActualData.Altitude = current.Z;
+	BaroAltitude::DataFields altActualData;
+	memset(&altActualData, 0, sizeof(BaroAltitude::DataFields));
+	altActualData.Altitude = current.Z;
 	altActualData.Temperature = TEMP_GROUND + (current.Z * TEMP_LAPSE_RATE) - 273.0;
 	altActualData.Pressure = PRESSURE(current.Z)/1000.0; // kpa
 	altActual->setData(altActualData);
 
 	// Update attActual object
 	AttitudeActual::DataFields attActualData;
-        memset(&attActualData, 0, sizeof(AttitudeActual::DataFields));
-        attActualData.Roll = current.roll;
+	memset(&attActualData, 0, sizeof(AttitudeActual::DataFields));
+	attActualData.Roll = current.roll;
 	attActualData.Pitch = current.pitch;
 	attActualData.Yaw = current.azimuth;
 	attActualData.q1 = 0;
@@ -229,8 +231,8 @@ void IL2Simulator::processUpdate(QString& data)
 
 	// Update gps objects
 	PositionActual::DataFields gpsData;
-        memset(&gpsData, 0, sizeof(PositionActual::DataFields));
-        gpsData.Altitude = current.Z;
+	memset(&gpsData, 0, sizeof(PositionActual::DataFields));
+	gpsData.Altitude = current.Z;
 	gpsData.Heading = current.azimuth;
 	gpsData.Groundspeed = current.groundspeed;
 	gpsData.Latitude = settings.latitude.toFloat() + current.Y * DEG2M;

ground/src/plugins/hitlnew/il2simulator.h

@@ -63,7 +63,8 @@ private:
 	float DENSITY(float pressure);
 	float PRESSURE(float alt);
 	float TAS(float ias,float alt);
-	void processUpdate(QString& data);
+
+	void processUpdate(const QByteArray& data);
 
 };
 

ground/src/plugins/hitlnew/simulator.cpp

@@ -191,9 +191,9 @@ void Simulator::receiveUpdate()
 		quint16 senderPort;
 		inSocket->readDatagram(datagram.data(), datagram.size(),
 							   &sender, &senderPort);
-		QString datastr(datagram);
+		//QString datastr(datagram);
 		// Process incomming data
-		processUpdate(datastr);
+		processUpdate(datagram);
 	 }
 }
 

ground/src/plugins/hitlnew/simulator.h

@@ -134,7 +134,7 @@ private slots:
 	Q_INVOKABLE void onDeleteSimulator(void);
 
 	virtual void transmitUpdate() = 0;
-	virtual void processUpdate(QString& data) = 0;
+	virtual void processUpdate(const QByteArray& data) = 0;
 
 protected:
 	QProcess* simProcess;

ground/src/plugins/hitlnew/xplanesimulator.cpp

@@ -0,0 +1,260 @@
+/**
+ ******************************************************************************
+ *
+ * @file       xplanesimulator.cpp
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief
+ * @see        The GNU Public License (GPL) Version 3
+ * @defgroup   hitlplugin
+ * @{
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+ /**
+ * Description of X-Plane Protocol:
+ *
+ * To see what data can be sended/recieved to/from X-Plane, launch X-Plane -> goto main menu
+ * (cursor at top of main X-Plane window) -> Settings -> Data Input and Output -> Data Set.
+ * Data Set shown all X-Plane params,
+ * each row has four checkbox: 1st check - out to UDP; 4 check - show on screen
+ * All the UDP messages for X-Plane have the same format, which is:
+ * 5-character MESSAGE PROLOUGE (to indicate the type of message)
+ * and then a DATA INPUT STRUCTURE (containing the message data that you want to send or receive)
+ *
+ * DATA INPUT/OUTPUT STRUCTURE is the following stuct:
+ *
+ *  struct data_struct
+ *  {
+ *      int index;     // data index, the index into the list of variables
+					   // you can output from the Data Output screen in X-Plane.
+ *      float data[8]; // the up to 8 numbers you see in the data output screen associated with that selection..
+					   // many outputs do not use all 8, though.
+ * };
+ *
+ * For Example, update of aileron/elevon/rudder in X-Plane (11 row in Data Set)
+ * bytes     value     description
+ * [0-3]     DATA      message type
+ * [4]       none      no matter
+ * [5-8]     11        code of setting param(row in Data Set)
+ * [9-41]    data      message data (8 float values)
+ * total size: 41 byte
+ *
+ */
+
+#include "xplanesimulator.h"
+#include "extensionsystem/pluginmanager.h"
+#include <coreplugin/icore.h>
+#include <coreplugin/threadmanager.h>
+#include <math.h>
+#include <qxtlogger.h>
+
+void TraceBuf(const char* buf,int len);
+
+XplaneSimulator::XplaneSimulator(const SimulatorSettings& params) :
+		Simulator(params)
+{
+}
+
+
+XplaneSimulator::~XplaneSimulator()
+{
+}
+
+void XplaneSimulator::setupUdpPorts(const QString& host, int inPort, int outPort)
+{
+	inSocket->bind(QHostAddress(host), inPort);
+	outSocket->bind(QHostAddress(host), outPort);
+}
+
+/**
+ * update data in X-Plane simulator
+ */
+void XplaneSimulator::transmitUpdate()
+{
+	//Read ActuatorDesired from autopilot
+	ActuatorDesired::DataFields actData = actDesired->getData();
+	float ailerons = actData.Roll;
+	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
+
+	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
+
+	// 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);
+
+	// 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);
+
+
+
+	/** !!! this settings was given from ardupilot X-Plane.pl, I comment them
+	   but if it needed comment should be removed !!!
+
+	// 8th data settings (joystick 1 ail/elv/rud)
+	stream << "DATA0" << quint32(11) << elevator << ailerons << rudder
+			<< float(-999) << float(-999) << float(-999) << float(-999) << float(-999);
+	outSocket->write(buf);
+	*/
+
+}
+
+/**
+ * process data string from X-Plane simulator
+ */
+void XplaneSimulator::processUpdate(const QByteArray& dataBuf)
+{
+	float altitude = 0;
+	float latitude = 0;
+	float longitude = 0;
+	float airspeed = 0;
+	float speed = 0;
+	float pitch = 0;
+	float roll = 0;
+	float heading = 0;
+	float pressure = 0;
+	float weather = 0;
+
+	QString str;
+	QByteArray& buf = const_cast<QByteArray&>(dataBuf);
+	QString data(buf);
+
+	if(data.left(4) == "DATA") // check type of packet
+	{
+		buf.remove(0,5);
+		if(dataBuf.size() % 36)
+		{
+			qxtLog->info("incorrect length of UDP packet: ",buf);
+			return; // incorrect length of struct
+		}
+		// check correctness of data length, length must be multiple of (id_size+8*float_size)=4+8*4=36
+		int channelCounter = dataBuf.size() / 36;
+		do
+		{
+			switch(buf[0]) // switch by id
+			{
+			case XplaneSimulator::LatitudeLongitude:
+				latitude = *((float*)(buf.data()+4*1));
+				longitude = *((float*)(buf.data()+4*2));
+				altitude = *((float*)(buf.data()+4*3)) /* * 3.048 */;
+				break;
+
+			case XplaneSimulator::Speed:
+				airspeed = *((float*)(buf.data()+4*6));
+				speed = *((float*)(buf.data()+4*7));
+				break;
+
+			case XplaneSimulator::PitchRollHeading:
+				pitch = *((float*)(buf.data()+4*1));
+				roll = *((float*)(buf.data()+4*2));
+				heading = *((float*)(buf.data()+4*3));
+				break;
+
+			case XplaneSimulator::SystemPressures:
+				pressure = *((float*)(buf.data()+4*1));
+				break;
+
+			case XplaneSimulator::AtmosphereWeather:
+				weather = *((float*)(buf.data()+4*1));
+				break;
+
+			default:
+				break;
+			}
+			channelCounter--;
+			buf.remove(0,36);
+		} while (channelCounter);
+
+		// Update AltitudeActual object
+		BaroAltitude::DataFields altActualData;
+		memset(&altActualData, 0, sizeof(BaroAltitude::DataFields));
+		altActualData.Altitude = altitude;
+		altActualData.Temperature = weather;
+		altActualData.Pressure = pressure;
+		altActual->setData(altActualData);
+
+		// Update attActual object
+		AttitudeActual::DataFields attActualData;
+		memset(&attActualData, 0, sizeof(AttitudeActual::DataFields));
+		attActualData.Roll = roll;   //roll;
+		attActualData.Pitch = pitch;  // pitch
+		//		attActualData.Yaw = yaw;
+		//		attActualData.q1 = 0;
+		//		attActualData.q2 = 0;
+		//		attActualData.q3 = 0;
+		//		attActualData.q4 = 0;
+		attActual->setData(attActualData);
+
+		// Update gps objects
+		PositionActual::DataFields gpsData;
+		memset(&gpsData, 0, sizeof(PositionActual::DataFields));
+		gpsData.Altitude = altitude;
+//		gpsData.Heading = pitch[2];
+//		gpsData.Groundspeed = speed[0];
+		gpsData.Latitude = latitude;
+		gpsData.Longitude = longitude;
+		posActual->setData(gpsData);
+	}
+	// issue manual update
+	//attActual->updated();
+	//altActual->updated();
+	//posActual->updated();
+}
+
+
+void TraceBuf(const char* buf,int len)
+{
+	QString str;
+	bool reminder=true;
+	for(int i=0; i < len; i++)
+	{
+		if(!(i%16))
+		{
+			if(i>0)
+			{
+				qDebug() << str;
+				str.clear();
+				reminder=false;
+			}
+			reminder=true;
+		}
+		str+=QString(" 0x%1").arg((quint8)buf[i],2,16,QLatin1Char('0'));
+	}
+
+	if(reminder)
+		qDebug() << str;
+}

ground/src/plugins/hitlnew/xplanesimulator.h

@@ -0,0 +1,90 @@
+/**
+ ******************************************************************************
+ *
+ * @file       xplanesimulator.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief
+ * @see        The GNU Public License (GPL) Version 3
+ * @defgroup   hitlplugin
+ * @{
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#ifndef XPLANESIMULATOR_H
+#define XPLANESIMULATOR_H
+
+#include <QObject>
+#include <simulator.h>
+
+class XplaneSimulator: public Simulator
+{
+	Q_OBJECT
+public:
+	XplaneSimulator(const SimulatorSettings& params);
+	~XplaneSimulator();
+
+	void setupUdpPorts(const QString& host, int inPort, int outPort);
+
+private slots:
+	void transmitUpdate();
+
+private:
+	enum XplaneOutputData
+	{
+		FramRate,
+		Times,
+		SimStats,
+		Speed,
+		Gload,
+		AtmosphereWeather,
+		AtmosphereAircraft,
+		SystemPressures,
+		Joystick1,
+		Joystick2,
+		ArtStab,
+		FlightCon,
+		WingSweep,
+		Trim,
+		Brakes,
+		AngularMoments,
+		AngularAccelerations,
+		AngularVelociies,
+		PitchRollHeading,
+		AoA,
+		LatitudeLongitude,
+		LocVelDistTraveled
+	};
+
+	void processUpdate(const QByteArray& data);
+
+};
+
+class XplaneSimulatorCreator : public SimulatorCreator
+{
+public:
+	XplaneSimulatorCreator(const QString& classId, const QString& description)
+	:  SimulatorCreator (classId,description)
+	{}
+
+	Simulator* createSimulator(const SimulatorSettings& params)
+	{
+		return new XplaneSimulator(params);
+	}
+};
+
+#endif // XPLANESIMULATOR_H