diff --git a/src/comm/QGCXPlaneLink.cc b/src/comm/QGCXPlaneLink.cc
index 28586877ca01b90903a3cb3bac7292731a01f38f..c2078ed7c098d00183a5b41b0410340f53306caf 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);
         }
     }
 
@@ -643,8 +678,8 @@ 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(uint64_t,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t)), mav, SLOT(sendHilState(quint64,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t)));
-        disconnect(this, SIGNAL(sensorHilGpsChanged(quint64,double,double,double,int,float,float,float,float,int)), mav, SLOT(sendHilGps(uint64_t, double, double, double, int, float, float, float, float, int)));
+        disconnect(this, SIGNAL(hilStateChanged(uint64_t,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t)), mav, SLOT(sendHilState(uint64_t,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t)));
+        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)));
 
         UAS* uas = dynamic_cast<UAS*>(mav);
@@ -827,8 +862,8 @@ 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(uint64_t,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t)), mav, SLOT(sendHilState(quint64,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t)));
-    connect(this, SIGNAL(sensorHilGpsChanged(quint64,double,double,double,int,float,float,float,float,int)), mav, SLOT(sendHilGps(uint64_t, double, double, double, int, float, float, float, float, int)));
+    connect(this, SIGNAL(hilStateChanged(uint64_t,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t)), mav, SLOT(sendHilState(uint64_t,float,float,float,float,float,float,int32_t,int32_t,int32_t,int16_t,int16_t,int16_t,int16_t,int16_t,int16_t)));
+    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)));
 
     UAS* uas = dynamic_cast<UAS*>(mav);
diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index b40fed4a4b369df54fa0d1eef087c92e4c66c7b0..8f8661aeec287c5894004b34c4e51b2ef29217a6 100644
--- a/src/uas/UAS.cc
+++ b/src/uas/UAS.cc
@@ -1691,10 +1691,22 @@ QList<int> UAS::getComponentIds()
 void UAS::setMode(int mode)
 {
     //this->mode = mode; //no call assignament, update receive message from UAS
+
+    // Strip armed / disarmed call, this is not relevant for setting the mode
+    uint8_t newMode = mode;
+    newMode &= (~(uint8_t)MAV_MODE_FLAG_SAFETY_ARMED);
+    // Now set current state (request no change)
+    newMode |= (uint8_t)(this->mode) & (uint8_t)(MAV_MODE_FLAG_SAFETY_ARMED);
+
+    // Strip HIL part, replace it with current system state
+    newMode &= (~(uint8_t)MAV_MODE_FLAG_HIL_ENABLED);
+    // Now set current state (request no change)
+    newMode |= (uint8_t)(this->mode) & (uint8_t)(MAV_MODE_FLAG_HIL_ENABLED);
+
     mavlink_message_t msg;
-    mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, (uint8_t)uasId, (uint8_t)mode, (uint16_t)navMode);
+    mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, (uint8_t)uasId, newMode, (uint16_t)navMode);
     sendMessage(msg);
-    qDebug() << "SENDING REQUEST TO SET MODE TO SYSTEM" << uasId << ", REQUEST TO SET MODE " << (uint8_t)mode;
+    qDebug() << "SENDING REQUEST TO SET MODE TO SYSTEM" << uasId << ", REQUEST TO SET MODE " << newMode;
 }
 
 /**
@@ -2720,7 +2732,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
diff --git a/src/ui/uas/UASControlWidget.cc b/src/ui/uas/UASControlWidget.cc
index 9d970ee05868a2f36024e79c8ce519dbb1e80970..6632aa3276b936518a54e0344e6e6bd8865d9d60 100644
--- a/src/ui/uas/UASControlWidget.cc
+++ b/src/ui/uas/UASControlWidget.cc
@@ -51,12 +51,12 @@ UASControlWidget::UASControlWidget(QWidget *parent) : QWidget(parent),
 
     connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setUAS(UASInterface*)));
     ui.modeComboBox->clear();
-    ui.modeComboBox->insertItem(0, UAS::getShortModeTextFor(MAV_MODE_PREFLIGHT), MAV_MODE_PREFLIGHT);
-    ui.modeComboBox->insertItem(1, UAS::getShortModeTextFor((MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED)), (MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED));
-    ui.modeComboBox->insertItem(2, UAS::getShortModeTextFor(MAV_MODE_FLAG_MANUAL_INPUT_ENABLED), MAV_MODE_FLAG_MANUAL_INPUT_ENABLED);
-    ui.modeComboBox->insertItem(3, UAS::getShortModeTextFor((MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED)), (MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED));
-    ui.modeComboBox->insertItem(4, UAS::getShortModeTextFor((MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED | MAV_MODE_FLAG_AUTO_ENABLED)), (MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED | MAV_MODE_FLAG_AUTO_ENABLED));
-    ui.modeComboBox->insertItem(5, UAS::getShortModeTextFor((MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_TEST_ENABLED)), (MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_TEST_ENABLED));
+    ui.modeComboBox->insertItem(0, UAS::getShortModeTextFor(MAV_MODE_PREFLIGHT).remove(0, 2), MAV_MODE_PREFLIGHT);
+    ui.modeComboBox->insertItem(1, UAS::getShortModeTextFor((MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED)).remove(0, 2), (MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED));
+    ui.modeComboBox->insertItem(2, UAS::getShortModeTextFor(MAV_MODE_FLAG_MANUAL_INPUT_ENABLED).remove(0, 2), MAV_MODE_FLAG_MANUAL_INPUT_ENABLED);
+    ui.modeComboBox->insertItem(3, UAS::getShortModeTextFor((MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED)).remove(0, 2), (MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED));
+    ui.modeComboBox->insertItem(4, UAS::getShortModeTextFor((MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED | MAV_MODE_FLAG_AUTO_ENABLED)).remove(0, 2), (MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED | MAV_MODE_FLAG_AUTO_ENABLED));
+    ui.modeComboBox->insertItem(5, UAS::getShortModeTextFor((MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_TEST_ENABLED)).remove(0, 2), (MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_TEST_ENABLED));
     connect(ui.modeComboBox, SIGNAL(activated(int)), this, SLOT(setMode(int)));
     connect(ui.setModeButton, SIGNAL(clicked()), this, SLOT(transmitMode()));