From 8df3fa95df6e3148b535d7c9521e6d9ee30f0108 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 3 Apr 2013 23:11:09 +0200
Subject: [PATCH] Finished sensor HIL, pending testing

---
 src/comm/QGCXPlaneLink.cc | 43 +++++++++++++++++++++++++++++++++++----
 src/uas/UAS.cc            |  2 +-
 2 files changed, 40 insertions(+), 5 deletions(-)

diff --git a/src/comm/QGCXPlaneLink.cc b/src/comm/QGCXPlaneLink.cc
index 28586877c..c45a67e27 100644
--- a/src/comm/QGCXPlaneLink.cc
+++ b/src/comm/QGCXPlaneLink.cc
@@ -354,6 +354,35 @@ void QGCXPlaneLink::updateControls(uint64_t time, float rollAilerons, float pitc
     writeBytes((const char*)&p, sizeof(p));
 }
 
+Eigen::Matrix3f euler_to_wRo(double yaw, double pitch, double roll) {
+  double c__ = cos(yaw);
+  double _c_ = cos(pitch);
+  double __c = cos(roll);
+  double s__ = sin(yaw);
+  double _s_ = sin(pitch);
+  double __s = sin(roll);
+  double cc_ = c__ * _c_;
+  double cs_ = c__ * _s_;
+  double sc_ = s__ * _c_;
+  double ss_ = s__ * _s_;
+  double c_c = c__ * __c;
+  double c_s = c__ * __s;
+  double s_c = s__ * __c;
+  double s_s = s__ * __s;
+  double _cc = _c_ * __c;
+  double _cs = _c_ * __s;
+  double csc = cs_ * __c;
+  double css = cs_ * __s;
+  double ssc = ss_ * __c;
+  double sss = ss_ * __s;
+  Eigen::Matrix3f wRo;
+  wRo <<
+    cc_  , css-s_c,  csc+s_s,
+    sc_  , sss+c_c,  ssc-c_s,
+    -_s_  ,     _cs,      _cc;
+  return wRo;
+}
+
 void QGCXPlaneLink::writeBytes(const char* data, qint64 size)
 {
     if (!data) return;
@@ -442,6 +471,12 @@ void QGCXPlaneLink::readBytes()
 
                 //qDebug() << "SPEEDS:" << "airspeed" << airspeed << "m/s, groundspeed" << groundspeed << "m/s";
             }
+            if (p.index == 4)
+            {
+                xacc = p.f[5] * 9.81f;
+                xacc = p.f[6] * 9.81f;
+                zacc = -p.f[4] * 9.81f;
+            }
             else if (p.index == 6 && xPlaneVersion == 10)
             {
                 // inHg to kPa
@@ -518,8 +553,8 @@ void QGCXPlaneLink::readBytes()
             else if (p.index == 21 && xPlaneVersion == 10)
             {
                 vx = p.f[3];
-                vy = p.f[4];
-                vz = p.f[5];
+                vy = -p.f[5];
+                vz = p.f[4];
             }
             else if (p.index == 12)
             {
@@ -593,8 +628,8 @@ void QGCXPlaneLink::readBytes()
             emit sensorHilGpsChanged(QGC::groundTimeUsecs(), lat, lon, alt, gps_fix_type, eph, epv, vel, cog, satellites);
         } else {
             emit hilStateChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed,
-                         pitchspeed, yawspeed, lat*1E7, lon*1E7, alt*1E3,
-                         vx, vy, vz, xacc*1000, yacc*1000, zacc*1000);
+                         pitchspeed, yawspeed, lat, lon, alt,
+                         vx, vy, vz, xacc, yacc, zacc);
         }
     }
 
diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index 115069288..9c2d6addc 100644
--- a/src/uas/UAS.cc
+++ b/src/uas/UAS.cc
@@ -2693,7 +2693,7 @@ void UAS::sendHilState(uint64_t time_us, float roll, float pitch, float yaw, flo
         mavlink_message_t msg;
         mavlink_msg_hil_state_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg,
                                    time_us, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed,
-                                   lat, lon, alt, vx, vy, vz, xacc, yacc, zacc);
+                                   lat*1e7f, lon*1e7f, alt*1000, vx*100, vy*100, vz*100, xacc*1000, yacc*1000, zacc*1000);
         sendMessage(msg);
     }
     else
-- 
2.22.0