Merge branch 'master' into integration_merge_connstatus

nd an empty message aborts

libs/mavlink/include/mavlink/v1.0/ardupilotmega/version.h

@@ -5,7 +5,7 @@
 #ifndef MAVLINK_VERSION_H
 #define MAVLINK_VERSION_H
 
-#define MAVLINK_BUILD_DATE "Fri Jun  7 22:41:53 2013"
+#define MAVLINK_BUILD_DATE "Mon Jul  1 09:49:34 2013"
 #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
 #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 254
  

libs/mavlink/include/mavlink/v1.0/autoquad/mavlink.h

+/** @file
+ *	@brief MAVLink comm protocol built from autoquad.xml
+ *	@see http://pixhawk.ethz.ch/software/mavlink
+ */
+#ifndef MAVLINK_H
+#define MAVLINK_H
+
+#ifndef MAVLINK_STX
+#define MAVLINK_STX 254
+#endif
+
+#ifndef MAVLINK_ENDIAN
+#define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN
+#endif
+
+#ifndef MAVLINK_ALIGNED_FIELDS
+#define MAVLINK_ALIGNED_FIELDS 1
+#endif
+
+#ifndef MAVLINK_CRC_EXTRA
+#define MAVLINK_CRC_EXTRA 1
+#endif
+
+#include "version.h"
+#include "autoquad.h"
+
+#endif // MAVLINK_H

libs/mavlink/include/mavlink/v1.0/autoquad/testsuite.h

+/** @file
+ *	@brief MAVLink comm protocol testsuite generated from autoquad.xml
+ *	@see http://qgroundcontrol.org/mavlink/
+ */
+#ifndef AUTOQUAD_TESTSUITE_H
+#define AUTOQUAD_TESTSUITE_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#ifndef MAVLINK_TEST_ALL
+#define MAVLINK_TEST_ALL
+static void mavlink_test_common(uint8_t, uint8_t, mavlink_message_t *last_msg);
+static void mavlink_test_autoquad(uint8_t, uint8_t, mavlink_message_t *last_msg);
+
+static void mavlink_test_all(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
+{
+	mavlink_test_common(system_id, component_id, last_msg);
+	mavlink_test_autoquad(system_id, component_id, last_msg);
+}
+#endif
+
+#include "../common/testsuite.h"
+
+
+static void mavlink_test_aq_telemetry_f(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
+{
+	mavlink_message_t msg;
+        uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
+        uint16_t i;
+	mavlink_aq_telemetry_f_t packet_in = {
+		17.0,
+	45.0,
+	73.0,
+	101.0,
+	129.0,
+	157.0,
+	185.0,
+	213.0,
+	241.0,
+	269.0,
+	297.0,
+	325.0,
+	353.0,
+	381.0,
+	409.0,
+	437.0,
+	465.0,
+	493.0,
+	521.0,
+	549.0,
+	21395,
+	};
+	mavlink_aq_telemetry_f_t packet1, packet2;
+        memset(&packet1, 0, sizeof(packet1));
+        	packet1.value1 = packet_in.value1;
+        	packet1.value2 = packet_in.value2;
+        	packet1.value3 = packet_in.value3;
+        	packet1.value4 = packet_in.value4;
+        	packet1.value5 = packet_in.value5;
+        	packet1.value6 = packet_in.value6;
+        	packet1.value7 = packet_in.value7;
+        	packet1.value8 = packet_in.value8;
+        	packet1.value9 = packet_in.value9;
+        	packet1.value10 = packet_in.value10;
+        	packet1.value11 = packet_in.value11;
+        	packet1.value12 = packet_in.value12;
+        	packet1.value13 = packet_in.value13;
+        	packet1.value14 = packet_in.value14;
+        	packet1.value15 = packet_in.value15;
+        	packet1.value16 = packet_in.value16;
+        	packet1.value17 = packet_in.value17;
+        	packet1.value18 = packet_in.value18;
+        	packet1.value19 = packet_in.value19;
+        	packet1.value20 = packet_in.value20;
+        	packet1.Index = packet_in.Index;
+        
+        
+
+        memset(&packet2, 0, sizeof(packet2));
+	mavlink_msg_aq_telemetry_f_encode(system_id, component_id, &msg, &packet1);
+	mavlink_msg_aq_telemetry_f_decode(&msg, &packet2);
+        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+
+        memset(&packet2, 0, sizeof(packet2));
+	mavlink_msg_aq_telemetry_f_pack(system_id, component_id, &msg , packet1.Index , packet1.value1 , packet1.value2 , packet1.value3 , packet1.value4 , packet1.value5 , packet1.value6 , packet1.value7 , packet1.value8 , packet1.value9 , packet1.value10 , packet1.value11 , packet1.value12 , packet1.value13 , packet1.value14 , packet1.value15 , packet1.value16 , packet1.value17 , packet1.value18 , packet1.value19 , packet1.value20 );
+	mavlink_msg_aq_telemetry_f_decode(&msg, &packet2);
+        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+
+        memset(&packet2, 0, sizeof(packet2));
+	mavlink_msg_aq_telemetry_f_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.Index , packet1.value1 , packet1.value2 , packet1.value3 , packet1.value4 , packet1.value5 , packet1.value6 , packet1.value7 , packet1.value8 , packet1.value9 , packet1.value10 , packet1.value11 , packet1.value12 , packet1.value13 , packet1.value14 , packet1.value15 , packet1.value16 , packet1.value17 , packet1.value18 , packet1.value19 , packet1.value20 );
+	mavlink_msg_aq_telemetry_f_decode(&msg, &packet2);
+        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+
+        memset(&packet2, 0, sizeof(packet2));
+        mavlink_msg_to_send_buffer(buffer, &msg);
+        for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) {
+        	comm_send_ch(MAVLINK_COMM_0, buffer[i]);
+        }
+	mavlink_msg_aq_telemetry_f_decode(last_msg, &packet2);
+        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+        
+        memset(&packet2, 0, sizeof(packet2));
+	mavlink_msg_aq_telemetry_f_send(MAVLINK_COMM_1 , packet1.Index , packet1.value1 , packet1.value2 , packet1.value3 , packet1.value4 , packet1.value5 , packet1.value6 , packet1.value7 , packet1.value8 , packet1.value9 , packet1.value10 , packet1.value11 , packet1.value12 , packet1.value13 , packet1.value14 , packet1.value15 , packet1.value16 , packet1.value17 , packet1.value18 , packet1.value19 , packet1.value20 );
+	mavlink_msg_aq_telemetry_f_decode(last_msg, &packet2);
+        MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
+}
+
+static void mavlink_test_autoquad(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
+{
+	mavlink_test_aq_telemetry_f(system_id, component_id, last_msg);
+}
+
+#ifdef __cplusplus
+}
+#endif // __cplusplus
+#endif // AUTOQUAD_TESTSUITE_H

libs/mavlink/include/mavlink/v1.0/autoquad/version.h

+/** @file
+ *	@brief MAVLink comm protocol built from autoquad.xml
+ *	@see http://pixhawk.ethz.ch/software/mavlink
+ */
+#ifndef MAVLINK_VERSION_H
+#define MAVLINK_VERSION_H
+
+#define MAVLINK_BUILD_DATE "Mon Jul  1 09:49:48 2013"
+#define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
+#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 254
+ 
+#endif // MAVLINK_VERSION_H

libs/mavlink/include/mavlink/v1.0/common/version.h

@@ -5,7 +5,7 @@
 #ifndef MAVLINK_VERSION_H
 #define MAVLINK_VERSION_H
 
-#define MAVLINK_BUILD_DATE "Fri Jun  7 22:44:13 2013"
+#define MAVLINK_BUILD_DATE "Mon Jul  1 09:51:07 2013"
 #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
 #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 254
  

libs/mavlink/include/mavlink/v1.0/matrixpilot/version.h

@@ -5,7 +5,7 @@
 #ifndef MAVLINK_VERSION_H
 #define MAVLINK_VERSION_H
 
-#define MAVLINK_BUILD_DATE "Fri Jun  7 22:44:10 2013"
+#define MAVLINK_BUILD_DATE "Mon Jul  1 09:50:15 2013"
 #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
 #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 254
  

libs/mavlink/include/mavlink/v1.0/pixhawk/version.h

@@ -5,7 +5,7 @@
 #ifndef MAVLINK_VERSION_H
 #define MAVLINK_VERSION_H
 
-#define MAVLINK_BUILD_DATE "Fri Jun  7 22:42:06 2013"
+#define MAVLINK_BUILD_DATE "Mon Jul  1 09:50:30 2013"
 #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
 #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255
  

libs/mavlink/include/mavlink/v1.0/sensesoar/version.h

@@ -5,7 +5,7 @@
 #ifndef MAVLINK_VERSION_H
 #define MAVLINK_VERSION_H
 
-#define MAVLINK_BUILD_DATE "Fri Jun  7 22:42:18 2013"
+#define MAVLINK_BUILD_DATE "Mon Jul  1 09:50:52 2013"
 #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
 #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 254
  

src/comm/MAVLinkSimulationMAV.cc

@@ -410,9 +410,47 @@ void MAVLinkSimulationMAV::handleMessage(const mavlink_message_t& msg)
     {
         mavlink_hil_state_t state;
         mavlink_msg_hil_state_decode(&msg, &state);
-        roll = state.roll;
-        pitch = state.pitch;
-        yaw = state.yaw;
+
+        double a = state.attitude_quaternion[0];
+        double b = state.attitude_quaternion[1];
+        double c = state.attitude_quaternion[2];
+        double d = state.attitude_quaternion[3];
+        double aSq = a * a;
+        double bSq = b * b;
+        double cSq = c * c;
+        double dSq = d * d;
+        float dcm[3][3];
+        dcm[0][0] = aSq + bSq - cSq - dSq;
+        dcm[0][1] = 2.0 * (b * c - a * d);
+        dcm[0][2] = 2.0 * (a * c + b * d);
+        dcm[1][0] = 2.0 * (b * c + a * d);
+        dcm[1][1] = aSq - bSq + cSq - dSq;
+        dcm[1][2] = 2.0 * (c * d - a * b);
+        dcm[2][0] = 2.0 * (b * d - a * c);
+        dcm[2][1] = 2.0 * (a * b + c * d);
+        dcm[2][2] = aSq - bSq - cSq + dSq;
+
+        float phi, theta, psi;
+        theta = asin(-dcm[2][0]);
+
+        if (fabs(theta - M_PI_2) < 1.0e-3f) {
+            phi = 0.0f;
+            psi = (atan2(dcm[1][2] - dcm[0][1],
+                    dcm[0][2] + dcm[1][1]) + phi);
+
+        } else if (fabs(theta + M_PI_2) < 1.0e-3f) {
+            phi = 0.0f;
+            psi = atan2f(dcm[1][2] - dcm[0][1],
+                      dcm[0][2] + dcm[1][1] - phi);
+
+        } else {
+            phi = atan2f(dcm[2][1], dcm[2][2]);
+            psi = atan2f(dcm[1][0], dcm[0][0]);
+        }
+
+        roll = phi;
+        pitch = theta;
+        yaw = psi;
         rollspeed = state.rollspeed;
         pitchspeed = state.pitchspeed;
         yawspeed = state.yawspeed;

src/comm/QGCFlightGearLink.cc

@@ -242,6 +242,10 @@ void QGCFlightGearLink::readBytes()
     // Parse string
     float roll, pitch, yaw, rollspeed, pitchspeed, yawspeed;
     double lat, lon, alt;
+
+    // XXX add
+    float ind_airspeed = 0.0f;
+    float true_airspeed = 0.0f;
     double vx, vy, vz, xacc, yacc, zacc;
 
     lat = values.at(1).toDouble();
@@ -265,7 +269,7 @@ void QGCFlightGearLink::readBytes()
     // Send updated state
     emit hilStateChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed,
                          pitchspeed, yawspeed, lat, lon, alt,
-                         vx, vy, vz, xacc, yacc, zacc);
+                         vx, vy, vz, ind_airspeed, true_airspeed, xacc, yacc, zacc);
 
     //    // Echo data for debugging purposes
     //    std::cerr << __FILE__ << __LINE__ << "Received datagram:" << std::endl;
@@ -299,7 +303,7 @@ bool QGCFlightGearLink::disconnectSimulation()
     disconnect(process, SIGNAL(error(QProcess::ProcessError)),
                this, SLOT(processError(QProcess::ProcessError)));
     disconnect(mav, SIGNAL(hilControlsChanged(uint64_t, float, float, float, float, uint8_t, uint8_t)), this, SLOT(updateControls(uint64_t,float,float,float,float,uint8_t,uint8_t)));
-    disconnect(this, SIGNAL(hilStateChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float)), mav, SLOT(sendHilState(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float)));
+    disconnect(this, SIGNAL(hilStateChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), mav, SLOT(sendHilState(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)));
 
     if (process)
     {
@@ -346,7 +350,7 @@ bool QGCFlightGearLink::connectSimulation()
     terraSync = new QProcess(this);
 
     connect(mav, SIGNAL(hilControlsChanged(uint64_t, float, float, float, float, uint8_t, uint8_t)), this, SLOT(updateControls(uint64_t,float,float,float,float,uint8_t,uint8_t)));
-    connect(this, SIGNAL(hilStateChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float)), mav, SLOT(sendHilState(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float)));
+    connect(this, SIGNAL(hilStateChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), mav, SLOT(sendHilState(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)));
 
 
     UAS* uas = dynamic_cast<UAS*>(mav);

src/comm/QGCHilLink.h

@@ -91,16 +91,20 @@ signals:
     /** @brief State update from simulation */
     void hilStateChanged(quint64 time_us, float roll, float pitch, float yaw, float rollspeed,
                                           float pitchspeed, float yawspeed, double lat, double lon, double alt,
-                                          float vx, float vy, float vz, float xacc, float yacc, float zacc);
+                                          float vx, float vy, float vz, float ind_airspeed, float true_airspeed, float xacc, float yacc, float zacc);
 
-    void sensorHilGpsChanged(quint64 time_us, double lat, double lon, double alt, int fix_type, float eph, float epv, float vel, float cog, int satellites);
+    void hilGroundTruthChanged(quint64 time_us, float roll, float pitch, float yaw, float rollspeed,
+                              float pitchspeed, float yawspeed, double lat, double lon, double alt,
+                              float vx, float vy, float vz, float ind_airspeed, float true_airspeed, float xacc, float yacc, float zacc);
+
+    void sensorHilGpsChanged(quint64 time_us, double lat, double lon, double alt, int fix_type, float eph, float epv, float vel, float vn, float ve, float vd, float cog, int satellites);
 
     void sensorHilRawImuChanged(quint64 time_us, float xacc, float yacc, float zacc,
                                                   float xgyro, float ygyro, float zgyro,
                                                   float xmag, float ymag, float zmag,
                                                   float abs_pressure, float diff_pressure,
                                                   float pressure_alt, float temperature,
-                                                  quint16 fields_updated);
+                                                  quint32 fields_updated);
     
     /** @brief Remote host and port changed */
     void remoteChanged(const QString& hostPort);

src/comm/QGCJSBSimLink.cc

@@ -271,7 +271,7 @@ bool QGCJSBSimLink::disconnectSimulation()
     disconnect(process, SIGNAL(error(QProcess::ProcessError)),
                this, SLOT(processError(QProcess::ProcessError)));
     disconnect(mav, SIGNAL(hilControlsChanged(uint64_t, float, float, float, float, uint8_t, uint8_t)), this, SLOT(updateControls(uint64_t,float,float,float,float,uint8_t,uint8_t)));
-    disconnect(this, SIGNAL(hilStateChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float)), mav, SLOT(sendHilState(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float)));
+    disconnect(this, SIGNAL(hilStateChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), mav, SLOT(sendHilState(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)));
 
     if (process)
     {
@@ -311,7 +311,7 @@ bool QGCJSBSimLink::connectSimulation()
     process = new QProcess(this);
 
     connect(mav, SIGNAL(hilControlsChanged(uint64_t, float, float, float, float, uint8_t, uint8_t)), this, SLOT(updateControls(uint64_t,float,float,float,float,uint8_t,uint8_t)));
-    connect(this, SIGNAL(hilStateChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float)), mav, SLOT(sendHilState(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float)));
+    connect(this, SIGNAL(hilStateChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), mav, SLOT(sendHilState(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)));
 
 
     UAS* uas = dynamic_cast<UAS*>(mav);

src/comm/QGCXPlaneLink.cc

@@ -54,7 +54,7 @@ QGCXPlaneLink::QGCXPlaneLink(UASInterface* mav, QString remoteHost, QHostAddress
     simUpdateLast(QGC::groundTimeMilliseconds()),
     simUpdateLastText(QGC::groundTimeMilliseconds()),
     simUpdateHz(0),
-    _sensorHilEnabled(false)
+    _sensorHilEnabled(true)
 {
     this->localHost = localHost;
     this->localPort = localPort/*+mav->getUASID()*/;
@@ -468,7 +468,8 @@ void QGCXPlaneLink::readBytes()
 
             if (p.index == 3)
             {
-                airspeed = p.f[6] * 0.44704f;
+                ind_airspeed = p.f[5] * 0.44704f;
+                true_airspeed = p.f[6] * 0.44704f;
                 groundspeed = p.f[7] * 0.44704;
 
                 //qDebug() << "SPEEDS:" << "airspeed" << airspeed << "m/s, groundspeed" << groundspeed << "m/s";
@@ -523,8 +524,6 @@ void QGCXPlaneLink::readBytes()
                 roll = p.f[1] / 180.0f * M_PI;
                 yaw = p.f[2] / 180.0f * M_PI;
 
-                yaw = yaw;
-
                 // X-Plane expresses yaw as 0..2 PI
                 if (yaw > M_PI) {
                     yaw -= 2.0 * M_PI;
@@ -551,6 +550,43 @@ void QGCXPlaneLink::readBytes()
                 zmag = 0.45f;
                 fields_changed |= (1 << 6) | (1 << 7) | (1 << 8);
 
+                double cosPhi = cos(roll);
+                double sinPhi = sin(roll);
+                double cosThe = cos(pitch);
+                double sinThe = sin(pitch);
+                double cosPsi = cos(0);
+                double sinPsi = sin(0);
+
+                float dcm[3][3];
+
+                dcm[0][0] = cosThe * cosPsi;
+                dcm[0][1] = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi;
+                dcm[0][2] = sinPhi * sinPsi + cosPhi * sinThe * cosPsi;
+
+                dcm[1][0] = cosThe * sinPsi;
+                dcm[1][1] = cosPhi * cosPsi + sinPhi * sinThe * sinPsi;
+                dcm[1][2] = -sinPhi * cosPsi + cosPhi * sinThe * sinPsi;
+
+                dcm[2][0] = -sinThe;
+                dcm[2][1] = sinPhi * cosThe;
+                dcm[2][2] = cosPhi * cosThe;
+
+                Eigen::Matrix3f m = Eigen::Map<Eigen::Matrix3f>((float*)dcm).eval();
+
+                Eigen::Vector3f mag(xmag, ymag, zmag);
+
+                Eigen::Vector3f magbody = m * mag;
+
+                qDebug() << "yaw mag:" << p.f[2] << "x" << xmag << "y" << ymag;
+                qDebug() << "yaw mag in body:" << magbody(0) << magbody(1) << magbody(2);
+
+                xmag = magbody(0);
+                ymag = magbody(1);
+                zmag = magbody(2);
+
+                // Rotate the measurement vector into the body frame using roll and pitch
+
+
                 emitUpdate = true;
             }
 
@@ -569,7 +605,9 @@ void QGCXPlaneLink::readBytes()
             {
                 vy = p.f[3];
                 vx = -p.f[5];
-                vz = p.f[4];
+                // moving 'up' in XPlane is positive, but its negative in NED
+                // for us.
+                vz = -p.f[4];
             }
             else if (p.index == 12)
             {
@@ -623,7 +661,6 @@ void QGCXPlaneLink::readBytes()
             // Set state
             simUpdateLastText = QGC::groundTimeMilliseconds();
         }
-        simUpdateLast = QGC::groundTimeMilliseconds();
 
         if (_sensorHilEnabled)
         {
@@ -634,19 +671,30 @@ void QGCXPlaneLink::readBytes()
 
             emit sensorHilRawImuChanged(QGC::groundTimeUsecs(), xacc, yacc, zacc, rollspeed, pitchspeed, yawspeed,
                                         xmag, ymag, zmag, abs_pressure, diff_pressure, pressure_alt, temperature, fields_changed);
+
+            // XXX make these GUI-configurable and add randomness
+            int gps_fix_type = 3;
+            float eph = 0.3;
+            float epv = 0.6;
+            float vel = sqrt(vx*vx + vy*vy + vz*vz);
+            float cog = atan2(vy, vx);
+            int satellites = 8;
+
+            emit sensorHilGpsChanged(QGC::groundTimeUsecs(), lat, lon, alt, gps_fix_type, eph, epv, vel, vx, vy, vz, cog, satellites);
+        } else {
+            emit hilStateChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed,
+                                 pitchspeed, yawspeed, lat, lon, alt,
+                                 vx, vy, vz, ind_airspeed, true_airspeed, xacc, yacc, zacc);
+        }
+
+        // Limit ground truth to 25 Hz
+        if (QGC::groundTimeMilliseconds() - simUpdateLast > 40) {
+            emit hilGroundTruthChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed,
+                                       pitchspeed, yawspeed, lat, lon, alt,
+                                       vx, vy, vz, ind_airspeed, true_airspeed, xacc, yacc, zacc);
         }
 
-        int gps_fix_type = 3;
-        float eph = 0.3;
-        float epv = 0.6;
-        float vel = sqrt(vx*vx + vy*vy + vz*vz);
-        float cog = atan2(vy, vx);
-        int satellites = 8;
-
-        emit sensorHilGpsChanged(QGC::groundTimeUsecs(), lat, lon, alt, gps_fix_type, eph, epv, vel, cog, satellites);
-        emit hilStateChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed,
-                     pitchspeed, yawspeed, lat, lon, alt,
-                     vx, vy, vz, xacc, yacc, zacc);
+        simUpdateLast = QGC::groundTimeMilliseconds();
     }
 
     if (!oldConnectionState && xPlaneConnected)
@@ -694,9 +742,10 @@ bool QGCXPlaneLink::disconnectSimulation()
         disconnect(mav, SIGNAL(hilControlsChanged(uint64_t, float, float, float, float, uint8_t, uint8_t)), this, SLOT(updateControls(uint64_t,float,float,float,float,uint8_t,uint8_t)));
         disconnect(mav, SIGNAL(hilActuatorsChanged(uint64_t, float, float, float, float, float, float, float, float)), this, SLOT(updateActuators(uint64_t,float,float,float,float,float,float,float,float)));
 
-        disconnect(this, SIGNAL(hilStateChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float)), mav, SLOT(sendHilState(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float)));
-        disconnect(this, SIGNAL(sensorHilGpsChanged(quint64,double,double,double,int,float,float,float,float,int)), mav, SLOT(sendHilGps(quint64, double, double, double, int, float, float, float, float, int)));
-        disconnect(this, SIGNAL(sensorHilRawImuChanged(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint16)), mav, SLOT(sendHilSensors(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint16)));
+        disconnect(this, SIGNAL(hilGroundTruthChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), mav, SLOT(sendHilGroundTruth(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)));
+        disconnect(this, SIGNAL(hilStateChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), mav, SLOT(sendHilState(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)));
+        disconnect(this, SIGNAL(sensorHilGpsChanged(quint64,double,double,double,int,float,float,float,float,float,float,float,int)), mav, SLOT(sendHilGps(quint64,double,double,double,int,float,float,float,float,float,float,float,int)));
+        disconnect(this, SIGNAL(sensorHilRawImuChanged(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32)), mav, SLOT(sendHilSensors(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32)));
 
         UAS* uas = dynamic_cast<UAS*>(mav);
         if (uas)
@@ -878,9 +927,10 @@ bool QGCXPlaneLink::connectSimulation()
     connect(mav, SIGNAL(hilControlsChanged(uint64_t, float, float, float, float, uint8_t, uint8_t)), this, SLOT(updateControls(uint64_t,float,float,float,float,uint8_t,uint8_t)));
     connect(mav, SIGNAL(hilActuatorsChanged(uint64_t, float, float, float, float, float, float, float, float)), this, SLOT(updateActuators(uint64_t,float,float,float,float,float,float,float,float)));
 
-    connect(this, SIGNAL(hilStateChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float)), mav, SLOT(sendHilState(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float)));
-    connect(this, SIGNAL(sensorHilGpsChanged(quint64,double,double,double,int,float,float,float,float,int)), mav, SLOT(sendHilGps(quint64, double, double, double, int, float, float, float, float, int)));
-    connect(this, SIGNAL(sensorHilRawImuChanged(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint16)), mav, SLOT(sendHilSensors(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint16)));
+    connect(this, SIGNAL(hilGroundTruthChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), mav, SLOT(sendHilGroundTruth(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)));
+    connect(this, SIGNAL(hilStateChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), mav, SLOT(sendHilState(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)));
+    connect(this, SIGNAL(sensorHilGpsChanged(quint64,double,double,double,int,float,float,float,float,float,float,float,int)), mav, SLOT(sendHilGps(quint64,double,double,double,int,float,float,float,float,float,float,float,int)));
+    connect(this, SIGNAL(sensorHilRawImuChanged(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32)), mav, SLOT(sendHilSensors(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32)));
 
     UAS* uas = dynamic_cast<UAS*>(mav);
     if (uas)

src/comm/QGCXPlaneLink.h

@@ -193,7 +193,8 @@ protected:
     float roll, pitch, yaw, rollspeed, pitchspeed, yawspeed;
     double lat, lon, alt;
     float vx, vy, vz, xacc, yacc, zacc;
-    float airspeed;
+    float ind_airspeed;
+    float true_airspeed;
     float groundspeed;
     float xmag, ymag, zmag, abs_pressure, diff_pressure, pressure_alt, temperature;
 

src/uas/UAS.h

@@ -696,11 +696,15 @@ public slots:
     /** @brief Send the full HIL state to the MAV */
     void sendHilState(quint64 time_us, float roll, float pitch, float yaw, float rollRotationRate,
                         float pitchRotationRate, float yawRotationRate, double lat, double lon, double alt,
-                        float vx, float vy, float vz, float xacc, float yacc, float zacc);
+                        float vx, float vy, float vz, float ind_airspeed, float true_airspeed, float xacc, float yacc, float zacc);
+
+    void sendHilGroundTruth(quint64 time_us, float roll, float pitch, float yaw, float rollRotationRate,
+                        float pitchRotationRate, float yawRotationRate, double lat, double lon, double alt,
+                        float vx, float vy, float vz, float ind_airspeed, float true_airspeed, float xacc, float yacc, float zacc);
 
     /** @brief RAW sensors for sensor HIL */
-    void sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, float rollRotationRate, float pitchRotationRate, float yawRotationRate,
-                                        float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, quint16 fields_changed);
+    void sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, float rollspeed, float pitchspeed, float yawspeed,
+                        float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, quint32 fields_changed);
 
     /**
      * @param time_us
@@ -714,7 +718,7 @@ public slots:
      * @param cog course over ground, in radians, -pi..pi
      * @param satellites
      */
-    void sendHilGps(quint64 time_us, double lat, double lon, double alt, int fix_type, float eph, float epv, float vel, float cog, int satellites);
+    void sendHilGps(quint64 time_us, double lat, double lon, double alt, int fix_type, float eph, float epv, float vel, float vn, float ve, float vd,  float cog, int satellites);
 
 
     /** @brief Places the UAV in Hardware-in-the-Loop simulation status **/

src/uas/UASInterface.h

@@ -369,14 +369,14 @@ public slots:
     /** @brief Send the full HIL state to the MAV */
     virtual void sendHilState(quint64 time_us, float roll, float pitch, float yaw, float rollspeed,
                         float pitchspeed, float yawspeed, double lat, double lon, double alt,
-                        float vx, float vy, float vz, float xacc, float yacc, float zacc) = 0;
+                        float vx, float vy, float vz, float ind_airspeed, float true_airspeed, float xacc, float yacc, float zacc) = 0;
 
     /** @brief RAW sensors for sensor HIL */
     virtual void sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, float rollspeed, float pitchspeed, float yawspeed,
-                                        float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, quint16 fields_changed) = 0;
+                                float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, quint32 fields_changed) = 0;
 
     /** @brief Send raw GPS for sensor HIL */
-    virtual void sendHilGps(quint64 time_us, double lat, double lon, double alt, int fix_type, float eph, float epv, float vel, float cog, int satellites) = 0;
+    virtual void sendHilGps(quint64 time_us, double lat, double lon, double alt, int fix_type, float eph, float epv, float vel, float vn, float ve, float vd, float cog, int satellites) = 0;
 
 
 protected:

src/ui/MainWindow.cc

@@ -655,7 +655,7 @@ void MainWindow::buildCommonWidgets()
     }
 #endif
 
-#if (defined _MSC_VER) | (defined Q_OS_MAC)
+#if (defined _MSC_VER) /*| (defined Q_OS_MAC) mac os doesn't support gearth right now */
     if (!gEarthWidget)
     {
         gEarthWidget = new QGCGoogleEarthView(this);

src/ui/QGCHilXPlaneConfiguration.cc

@@ -29,9 +29,9 @@ QGCHilXPlaneConfiguration::QGCHilXPlaneConfiguration(QGCHilLink* link, QWidget *
         ui->airframeComboBox->setCurrentIndex(link->getAirFrameIndex());
         // XXX not implemented yet
         ui->airframeComboBox->hide();
-        ui->sensorHilCheckBox->setChecked(link->sensorHilEnabled());
-        connect(link, SIGNAL(sensorHilChanged(bool)), ui->sensorHilCheckBox, SLOT(setChecked(bool)));
-        connect(ui->sensorHilCheckBox, SIGNAL(clicked(bool)), link, SLOT(enableSensorHIL(bool)));
+        ui->sensorHilCheckBox->setChecked(xplane->sensorHilEnabled());
+        connect(xplane, SIGNAL(sensorHilChanged(bool)), ui->sensorHilCheckBox, SLOT(setChecked(bool)));
+        connect(ui->sensorHilCheckBox, SIGNAL(clicked(bool)), xplane, SLOT(enableSensorHIL(bool)));
 
         connect(link, SIGNAL(versionChanged(int)), this, SLOT(setVersion(int)));
     }

src/ui/WaypointViewOnlyView.cc

@@ -76,7 +76,6 @@ void WaypointViewOnlyView::setCurrent(bool state)
 
 void WaypointViewOnlyView::updateValues()
 {
-    qDebug() << "Trof: WaypointViewOnlyView::updateValues() ID:" << wp->getId();
     // Check if we just lost the wp, delete the widget
     // accordingly
     if (!wp)

src/ui/map/QGCMapWidget.cc

@@ -603,7 +603,7 @@ void QGCMapWidget::handleMapWaypointEdit(mapcontrol::WayPointItem* waypoint)
  */
 void QGCMapWidget::updateWaypoint(int uas, Waypoint* wp)
 {
-    qDebug() << __FILE__ << __LINE__ << "UPDATING WP FUNCTION CALLED";
+    //qDebug() << __FILE__ << __LINE__ << "UPDATING WP FUNCTION CALLED";
     // Source of the event was in this widget, do nothing
     if (firingWaypointChange == wp) {
         return;