OP-89

math complete but untested


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

ground/src/plugins/hitlil2/il2bridge.cpp

@@ -25,8 +25,43 @@
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
 
+/*
+ * Description of DeviceLink Protocol:
+ * A Request is initiated with R/ followed by id's of to be requested settings
+ * even id's indicate read only values, odd are write only
+ * (usually id =get value id+1= set - for same setting)
+ * id's are separated by /
+ * requests can contain values to set, or to select a subsystem
+ * values are separated by \
+ * example: R/30/48/64\0/64\1/
+ * request read only settings 30,48 and 64 with parameters 0 and 1
+ * the answer consists of an A followed by id value pairs in the same format
+ * example: A/30\0/48\0/64\0\22/64\1\102/
+ *
+ * A full protocol description as well as a list of ID's and their meanings
+ * can be found shipped with IL2 in the file DeviceLink.txt
+ *
+ * id's used in this file:
+ * 30: IAS in km/h (float)
+ * 32: vario in m/s (float)
+ * 38: angular speed °/s (float) (which direction? azimuth?)
+ * 40: barometric alt in m (float)
+ * 42: flight course in ° (0-360) (float)
+ * 46: roll angle in ° (-180 - 180) (floatniguration)
+ * 48: pitch angle in ° (-90 - 90) (float)
+ * 80/81: engine power  (-1.0 (0%) - 1.0 (100%)) (float)
+ * 84/85: aileron servo (-1.0 - 1.0) (float)
+ * 86/87: elevator servo (-1.0 - 1.0) (float)
+ * 88/89: rudder servo (-1.0 - 1.0) (float)
+ *
+ * IL2 currently offers no useful way of providing GPS data
+ * therefore fake GPS data will be calculated using IMS
+ *
+ * unfortunately angular acceleration provided is very limited, too
+ */
 #include "il2bridge.h"
 #include "extensionsystem/pluginmanager.h"
+#include <math.h>
 
 Il2Bridge::Il2Bridge(QString il2HostName, int il2Port)
 {
@@ -76,6 +111,12 @@ Il2Bridge::Il2Bridge(QString il2HostName, int il2Port)
     connect(il2Timer, SIGNAL(timeout()), this, SLOT(onIl2ConnectionTimeout()));
     il2Timer->setInterval(il2Timeout);
     il2Timer->start();
+
+    // setup time
+    time = new QTime();
+    time->start();
+
+    current.T=0;
 }
 
 Il2Bridge::~Il2Bridge()
@@ -112,7 +153,11 @@ void Il2Bridge::transmitUpdate()
 //          .arg(rudder)
 //          .arg(throttle);
     QString cmd;
-    cmd=QString("R/30/32/34/40/42/46/48/64\\0/64\\1/74\\0/74\\1/164/");
+    cmd=QString("R/30/32/38/40/42/46/48/81\\%1/85\\%2/87\\%3/89\\%4/")
+    	.arg(throttle*2.0-1.0)
+	.arg(ailerons)
+	.arg(elevator)
+	.arg(rudder);
     QByteArray data = cmd.toAscii();
     outSocket->write(data);
 }
@@ -203,66 +248,82 @@ void Il2Bridge::onIl2ConnectionTimeout()
     }
 }
 
+float Il2Bridge::DENSITY(float alt) {
+	return (GROUNDDENSITY * pow(
+				(TEMP_GROUND+(TEMP_LAPSE_RATE*alt)/TEMP_GROUND),
+				((AIR_CONST_FACTOR/TEMP_LAPSE_RATE)-1) )
+				);
+}
+
+float Il2Bridge::PRESSURE(float alt) {
+	return DENSITY(alt)*alt*TEMP_LAPSE_RATE*TEMP_GROUND*AIR_CONST;
+	
+}
+
+float Il2Bridge::TAS(float IAS, float alt) {
+	return (IAS*sqrt(GROUNDDENSITY/DENSITY(alt)));
+}
+
 void Il2Bridge::processUpdate(QString& data)
 {
 return;
     // Split
-    QStringList fields = data.split(",");
+    QStringList fields = data.split("/");
-    // Get xRate (deg/s)
+
-    float xRate = fields[0].toFloat() * 180.0/M_PI;
+    old=current;
-    // Get yRate (deg/s)
+
-    float yRate = fields[1].toFloat() * 180.0/M_PI;
+    int t;
-    // Get zRate (deg/s)
+    for (t=0; t<fields->length(); t++) {
-    float zRate = fields[2].toFloat() * 180.0/M_PI;
+    	QStringList values = fields[t].split("\\");
-    // Get xAccel (m/s^2)
+	int id = values[0].toInt();
-    float xAccel = fields[3].toFloat() * FT2M;
+	float value = values[1].toFloat();
-    // Get yAccel (m/s^2)
+	switch (id) {
-    float yAccel = fields[4].toFloat() * FT2M;
+	    case 30:
-    // Get xAccel (m/s^2)
+	        current.ias=value * KMH2MPS;
-    float zAccel = fields[5].toFloat() * FT2M;
+		break;
-    // Get pitch (deg)
+	    case 32:
-    float pitch = fields[6].toFloat();
+	        current.dZ=value;
-    // Get pitchRate (deg/s)
+		break;
-    float pitchRate = fields[7].toFloat();
+	    case 38:
-    // Get roll (deg)
+	        //angular_speed=value;
-    float roll = fields[8].toFloat();
+		break;
-    // Get rollRate (deg/s)
+	    case 40:
-    float rollRate = fields[9].toFloat();
+	        current.Z=value;
-    // Get yaw (deg)
+		break;
-    float yaw = fields[10].toFloat();
+	    case 42:
-    // Get yawRate (deg/s)
+	        current.azimuth=value;
-    float yawRate = fields[11].toFloat();
+		break;
-    // Get latitude (deg)
+	    case 46:
-    float latitude = fields[12].toFloat();
+	        current.roll=value;
-    // Get longitude (deg)
+		break;
-    float longitude = fields[13].toFloat();
+	    case 48:
-    // Get heading (deg)
+	        current.pitch=value;
-    float heading = fields[14].toFloat();
+		break;
-    // Get altitude (m)
+    }
-    float altitude = fields[15].toFloat() * FT2M;
+
-    // Get altitudeAGL (m)
+    current.dT = ((float)time->restart()) / 1000.0;
-    float altitudeAGL = fields[16].toFloat() * FT2M;
+    current.T = old.T+current.dT;
-    // Get groundspeed (m/s)
+
-    float groundspeed = fields[17].toFloat() * KT2MPS;
+    current.tas = TAS(current.ias,current.Z);
-    // Get airspeed (m/s)
+    current.groundspeed = current.tas*cos(current.pitch*DEG2RAD);
-    float airspeed = fields[18].toFloat() * KT2MPS;
+    current.dX = current.groundspeed*sin(current.azimuth*DEG2RAD);
-    // Get temperature (degC)
+    current.dY = current.groundspeed*cos(current.azimuth*DEG2RAD);
-    float temperature = fields[19].toFloat();
+
-    // Get pressure (kpa)
+    current.X = old.X + (current.dX/current.dT);
-    float pressure = fields[20].toFloat() * INHG2KPA;
+    current.Y = old.Y + (current.dY/current.dT);
 
     // Update AltitudeActual object
     AltitudeActual::DataFields altActualData;
-    altActualData.Altitude = altitudeAGL;
+    altActualData.Altitude = current.Z;
-    altActualData.Temperature = temperature;
+    altActualData.Temperature = (TEMP_GROUND * current.Z * TEMP_LAPSE_RATE)+273.0;
-    altActualData.Pressure = pressure;
+    altActualData.Pressure = PRESSURE(current.Z)*1000.0; // kpa
     altActual->setData(altActualData);
 
     // Update attActual object
     AttitudeActual::DataFields attActualData;
-    attActualData.Roll = roll;
+    attActualData.Roll = current.roll;
-    attActualData.Pitch = pitch;
+    attActualData.Pitch = current.pitch;
-    attActualData.Yaw = yaw;
+    attActualData.Yaw = current.yaw;
     attActualData.q1 = 0;
     attActualData.q2 = 0;
     attActualData.q3 = 0;
@@ -272,12 +333,12 @@ return;
 
     // Update gps objects
     PositionActual::DataFields gpsData;
-    gpsData.Altitude = altitude;
+    gpsData.Altitude = current.Z;
-    gpsData.Heading = heading;
+    gpsData.Heading = current.azimuth;
-    gpsData.Groundspeed = groundspeed;
+    gpsData.Groundspeed = current.groundspeed;
-    gpsData.Latitude = latitude;
+    gpsData.Latitude = current.Y * DEG2M;
-    gpsData.Longitude = longitude;
+    gpsData.Longitude = current.X * cos(gpsData.Latitude*DEG2RAD) * DEG2M;
-    gpsData.Satellites = 10;
+    gpsData.Satellites = 7;
     gpsData.Status = PositionActual::STATUS_FIX3D;
     posActual->setData(gpsData);
 }

ground/src/plugins/hitlil2/il2bridge.h

@@ -31,6 +31,7 @@
 #include <QObject>
 #include <QUdpSocket>
 #include <QTimer>
+#include <QTime>
 #include <QMessageBox>
 #include <math.h>
 #include "uavtalk/telemetrymanager.h"
@@ -41,6 +42,36 @@
 #include "uavobjects/positionactual.h"
 #include "uavobjects/gcstelemetrystats.h"
 
+
+struct flightParams {
+
+	// time
+	float T;
+	float dT;
+
+	// speed (relative)
+	float ias;
+	float tas;
+	float groundspeed;
+
+	// position (absolute)
+	float X;
+	float Y;
+	float Z;
+
+	// speed (absolute)
+	float dX;
+	float dY;
+	float dZ;
+
+	//angle
+	float azimuth;
+	float pitch;
+	float roll;
+
+};
+
+
 class Il2Bridge: public QObject
 {
     Q_OBJECT
@@ -69,8 +100,22 @@ private slots:
 private:
     static const float FT2M = 0.3048;
     static const float KT2MPS = 0.514444444;
+    static const float MPS2KMH = 3.6;
+    static const float KMH2MPS = (1.0/MPS2KMH);
     static const float INHG2KPA = 3.386;
+    static const float RAD2DEG = (180.0/M_PI);
+    static const float DEG2RAD = (1.0/RAD2DEG);
+    static const float M2DEG =  60.*1852.; // 60 miles per degree times 1852 meters per mile
+    static const float DEG2M = (1.0/M2DEG);
 
+    static const float AIR_CONST = 8314.32; // N*m/(Kmol*K) 
+    static const float GROUNDDENSITY = 1.225; // kg/m³ ;)
+    static const float TEMP_GROUND = (15.0 + 273.0); // 15°C in Kelvin
+    static const float TEMP_LAPSE_RATE = -0.0065; //degrees per meter
+    static const float AIR_CONST_FACTOR = -0.0341631947363104; //several nature constants calculated into one
+
+    struct FlightParams current;
+    struct FlightParams old;
     QUdpSocket* outSocket;
     ActuatorDesired* actDesired;
     AltitudeActual* altActual;
@@ -82,6 +127,7 @@ private:
     int updatePeriod;
     QTimer* txTimer;
     QTimer* il2Timer;
+    QTime* time;
     bool autopilotConnectionStatus;
     bool il2ConnectionStatus;
     int il2Timeout;