From 52dc821ee2594f3493a962dfc4ecf8b67e7d7804 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 2 Jul 2013 00:47:20 +0200
Subject: [PATCH] Added rotation of mag vector into body frame in HIL

---
 src/comm/QGCXPlaneLink.cc | 59 +++++++++++++++++++++++++++++++++------
 1 file changed, 50 insertions(+), 9 deletions(-)

diff --git a/src/comm/QGCXPlaneLink.cc b/src/comm/QGCXPlaneLink.cc
index c71859b9f6..5841a5b0a5 100644
--- a/src/comm/QGCXPlaneLink.cc
+++ b/src/comm/QGCXPlaneLink.cc
@@ -524,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;
@@ -552,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;
             }
 
@@ -570,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)
             {
@@ -624,7 +661,6 @@ void QGCXPlaneLink::readBytes()
             // Set state
             simUpdateLastText = QGC::groundTimeMilliseconds();
         }
-        simUpdateLast = QGC::groundTimeMilliseconds();
 
         if (_sensorHilEnabled)
         {
@@ -647,13 +683,18 @@ void QGCXPlaneLink::readBytes()
             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);
+                                 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);
         }
 
-        emit hilGroundTruthChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed,
-                 pitchspeed, yawspeed, lat, lon, alt,
-                 vx, vy, vz, ind_airspeed, true_airspeed, xacc, yacc, zacc);
+        simUpdateLast = QGC::groundTimeMilliseconds();
     }
 
     if (!oldConnectionState && xPlaneConnected)
-- 
GitLab