Merge branch 'master' of github.com:mavlink/qgroundcontrol into sensor_hil

libs/opmapcontrol/src/mapwidget/uavitem.cpp

@@ -58,7 +58,10 @@ namespace mapcontrol
         Q_UNUSED(option);
         Q_UNUSED(widget);
        // painter->rotate(-90);
+        QPainter::RenderHints oldhints = painter->renderHints();
+        painter->setRenderHints(QPainter::Antialiasing | QPainter::SmoothPixmapTransform);
         painter->drawPixmap(-pic.width()/2,-pic.height()/2,pic);
+        painter->setRenderHints(oldhints);
        //   painter->drawRect(QRectF(-pic.width()/2,-pic.height()/2,pic.width()-1,pic.height()-1));
     }
     QRectF UAVItem::boundingRect()const

src/comm/LinkManager.cc

@@ -74,7 +74,7 @@ void LinkManager::add(LinkInterface* link)
     if (!links.contains(link))
     {
         if(!link) return;
-        connect(link, SIGNAL(destroyed(QObject*)), this, SLOT(removeLink(QObject*)));
+        connect(link, SIGNAL(destroyed(QObject*)), this, SLOT(removeObj(QObject*)));
         links.append(link);
         emit newLink(link);
     }
@@ -94,6 +94,8 @@ void LinkManager::addProtocol(LinkInterface* link, ProtocolInterface* protocol)
     {
         // Protocol is new, add
         connect(link, SIGNAL(bytesReceived(LinkInterface*, QByteArray)), protocol, SLOT(receiveBytes(LinkInterface*, QByteArray)));
+        // Add status
+        connect(link, SIGNAL(connected(bool)), protocol, SLOT(linkStatusChanged(bool)));
         // Store the connection information in the protocol links map
         protocolLinks.insertMulti(protocol, link);
     }
@@ -145,7 +147,7 @@ bool LinkManager::disconnectLink(LinkInterface* link)
     return link->disconnect();
 }
 
-void LinkManager::removeLink(QObject* link)
+void LinkManager::removeObj(QObject* link)
 {
     LinkInterface* linkInterface = dynamic_cast<LinkInterface*>(link);
     if (linkInterface)
@@ -171,6 +173,10 @@ bool LinkManager::removeLink(LinkInterface* link)
         {
             protocolLinks.remove(proto, link);
         }
+
+        // Emit removal of link
+        emit linkRemoved(link);
+
         return true;
     }
     return false;

src/comm/LinkManager.h

@@ -72,7 +72,7 @@ public slots:
     void add(LinkInterface* link);
     void addProtocol(LinkInterface* link, ProtocolInterface* protocol);
 
-    void removeLink(QObject* link);
+    void removeObj(QObject* obj);
     bool removeLink(LinkInterface* link);
 
     bool connectAll();
@@ -91,6 +91,7 @@ private:
 
 signals:
     void newLink(LinkInterface* link);
+    void linkRemoved(LinkInterface* link);
 
 };
 

src/comm/MAVLinkProtocol.cc

@@ -174,6 +174,33 @@ QString MAVLinkProtocol::getLogfileName()
     }
 }
 
+void MAVLinkProtocol::linkStatusChanged(bool connected)
+{
+    LinkInterface* link = qobject_cast<LinkInterface*>(QObject::sender());
+
+    if (link) {
+        if (connected) {
+            // Send command to start MAVLink
+            // XXX hacky but safe
+            // Start NSH
+            const char init[] = {0x0d, 0x0d, 0x0d};
+            link->writeBytes(init, sizeof(init));
+            QGC::SLEEP::msleep(500);
+
+            // Stop any running mavlink instance
+            char* cmd = "mavlink stop\n";
+            link->writeBytes(cmd, strlen(cmd));
+            link->writeBytes(init, 2);
+            cmd = "uorb start";
+            link->writeBytes(cmd, strlen(cmd));
+            link->writeBytes(init, 2);
+            cmd = "sh /etc/init.d/rc.usb\n";
+            link->writeBytes(cmd, strlen(cmd));
+            link->writeBytes(init, 4);
+        }
+    }
+}
+
 /**
  * The bytes are copied by calling the LinkInterface::readBytes() method.
  * This method parses all incoming bytes and constructs a MAVLink packet.

src/comm/MAVLinkProtocol.h

@@ -128,6 +128,7 @@ public:
 public slots:
     /** @brief Receive bytes from a communication interface */
     void receiveBytes(LinkInterface* link, QByteArray b);
+    void linkStatusChanged(bool connected);
     /** @brief Send MAVLink message through serial interface */
     void sendMessage(mavlink_message_t message);
     /** @brief Send MAVLink message */

src/comm/MAVLinkSimulationLink.cc

@@ -107,7 +107,6 @@ MAVLinkSimulationLink::MAVLinkSimulationLink(QString readFile, QString writeFile
     maxTimeNoise = 0;
     this->id = getNextLinkId();
     LinkManager::instance()->add(this);
-    QObject::connect(this, SIGNAL(destroyed(QObject*)), LinkManager::instance(), SLOT(removeLink(QObject*)));
 }
 
 MAVLinkSimulationLink::~MAVLinkSimulationLink()

src/comm/ProtocolInterface.h

@@ -55,6 +55,7 @@ public:
 
 public slots:
     virtual void receiveBytes(LinkInterface *link, QByteArray b) = 0;
+    virtual void linkStatusChanged(bool connected) = 0;
 
 signals:
     /** @brief Update the packet loss from one system */

src/comm/QGCXPlaneLink.cc

@@ -474,14 +474,29 @@ void QGCXPlaneLink::readBytes()
             if (p.index == 4)
             {
                 xacc = p.f[5] * 9.81f;
-                xacc = p.f[6] * 9.81f;
+                yacc = p.f[6] * 9.81f;
                 zacc = -p.f[4] * 9.81f;
+
+                Eigen::Vector3f g(0, 0, -9.81f);
+
+                Eigen::Matrix3f R = euler_to_wRo(yaw, pitch, roll);
+
+                Eigen::Vector3f gr = R.transpose().eval() * g;
+
+                // TODO Add centrip. accel
+
+                xacc = gr[0];
+                yacc = gr[1];
+                zacc = gr[2];
+
+                fields_changed |= (1 << 0) | (1 << 1) | (1 << 2);
             }
             else if (p.index == 6 && xPlaneVersion == 10)
             {
-                // inHg to kPa
-                abs_pressure = p.f[0] * 3.3863886666718317f;
+                // inHg to hPa (hecto Pascal / millibar)
+                abs_pressure = p.f[0] * 33.863886666718317f;
                 temperature = p.f[1];
+                fields_changed |= (1 << 9) | (1 << 12);
             }
             // Forward controls from X-Plane to MAV, not very useful
             // better: Connect Joystick to QGroundControl
@@ -494,13 +509,13 @@ void QGCXPlaneLink::readBytes()
 //                UAS* uas = dynamic_cast<UAS*>(mav);
 //                if (uas) uas->setManualControlCommands(man_roll, man_pitch, man_yaw, 0.6);
 //            }
-            else if (xPlaneVersion == 10 && p.index == 16)
+            else if ((xPlaneVersion == 10 && p.index == 16) || (xPlaneVersion == 9 && p.index == 17))
             {
-                //qDebug() << "ANG VEL:" << p.f[0] << p.f[3] << p.f[7];
-                rollspeed = p.f[2];
-                pitchspeed = p.f[1];
-                yawspeed = p.f[0];
-                fields_changed |= (1 << 0) | (1 << 1) | (1 << 2);
+                // Cross checked with XPlane flight
+                pitchspeed = p.f[0];
+                rollspeed = p.f[1];
+                yawspeed = p.f[2];
+                fields_changed |= (1 << 3) | (1 << 4) | (1 << 5);
             }
             else if ((xPlaneVersion == 10 && p.index == 17) || (xPlaneVersion == 9 && p.index == 18))
             {
@@ -508,6 +523,9 @@ void QGCXPlaneLink::readBytes()
                 pitch = p.f[0] / 180.0f * M_PI;
                 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;
@@ -516,7 +534,7 @@ void QGCXPlaneLink::readBytes()
                     yaw += 2.0 * M_PI;
                 }
 
-                float yawmag = p.f[3];
+                float yawmag = p.f[3] / 180.0f * M_PI;
 
                 if (yawmag > M_PI) {
                     yawmag -= 2.0 * M_PI;
@@ -525,19 +543,17 @@ void QGCXPlaneLink::readBytes()
                     yawmag += 2.0 * M_PI;
                 }
 
-                xmag = cos(yawmag) * 0.4f - sin(yawmag) * 0.0f + 0.0f;
-                ymag = sin(yawmag) * 0.4f - sin(yawmag) * 0.0f + 0.0f;
-                zmag = 0.0f            + 0.0f            + 1.0f * 0.4f;
+                // Normal rotation matrix, but since we rotate the
+                // vector [0.25 0 0.45]', we end up with these relevant
+                // matrix parts.
+
+                xmag = cos(-yawmag) * 0.25f;
+                ymag = sin(-yawmag) * 0.25f;
+                zmag = 0.45f;
+                fields_changed |= (1 << 6) | (1 << 7) | (1 << 8);
 
                 emitUpdate = true;
             }
-            else if ((xPlaneVersion == 9 && p.index == 17))
-            {
-                rollspeed = p.f[2];
-                pitchspeed = p.f[1];
-                yawspeed = p.f[0];
-                fields_changed |= (1 << 0) | (1 << 1) | (1 << 2);
-            }
 
 //            else if (p.index == 19)
 //            {
@@ -614,23 +630,24 @@ void QGCXPlaneLink::readBytes()
         {
             diff_pressure = 0.0f;
             pressure_alt = alt;
+            // set pressure alt to changed
+            fields_changed |= (1 << 11);
 
             emit sensorHilRawImuChanged(QGC::groundTimeUsecs(), xacc, yacc, zacc, rollspeed, pitchspeed, yawspeed,
                                         xmag, ymag, zmag, abs_pressure, diff_pressure, pressure_alt, temperature, fields_changed);
-
-            int gps_fix_type = 3;
-            float eph = 0.3;
-            float epv = 0.6;
-            float vel = sqrt(vx*vx + vy*vy + vz*vz);
-            float cog = yaw;
-            int satellites = 8;
-
-            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, lon, alt,
-                         vx, vy, vz, 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 = yaw;
+        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);
     }
 
     if (!oldConnectionState && xPlaneConnected)
@@ -678,8 +695,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);
@@ -862,8 +879,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);

src/uas/UAS.cc

@@ -80,6 +80,10 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(),
     latitude(0.0),
     longitude(0.0),
     altitude(0.0),
+    globalEstimatorActive(false),
+    latitude_gps(0.0),
+    longitude_gps(0.0),
+    altitude_gps(0.0),
     roll(0.0),
     pitch(0.0),
     yaw(0.0),
@@ -733,6 +737,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
             latitude = pos.lat/(double)1E7;
             longitude = pos.lon/(double)1E7;
             altitude = pos.alt/1000.0;
+            globalEstimatorActive = true;
             speedX = pos.vx/100.0;
             speedY = pos.vy/100.0;
             speedZ = pos.vz/100.0;
@@ -772,34 +777,33 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
 
             if (pos.fix_type > 2)
             {
-                emit globalPositionChanged(this, pos.lat/(double)1E7, pos.lon/(double)1E7, pos.alt/1000.0, time);
-                latitude = pos.lat/(double)1E7;
-                longitude = pos.lon/(double)1E7;
-                altitude = pos.alt/1000.0;
+                latitude_gps = pos.lat/(double)1E7;
+                longitude_gps = pos.lon/(double)1E7;
+                altitude_gps = pos.alt/1000.0;
+
+                if (!globalEstimatorActive) {
+                    latitude = latitude_gps;
+                    longitude = longitude_gps;
+                    altitude = altitude_gps;
+                    emit globalPositionChanged(this, latitude, longitude, altitude, time);
+                }
+
                 positionLock = true;
                 isGlobalPositionKnown = true;
 
-                // Check for NaN
-                int alt = pos.alt;
-                if (!isnan(alt) && !isinf(alt))
-                {
-                    alt = 0;
-                    //emit textMessageReceived(uasId, message.compid, 255, "GCS ERROR: RECEIVED NaN or Inf FOR ALTITUDE");
-                }
-                // FIXME REMOVE LATER emit valueChanged(uasId, "altitude", "m", pos.alt/(double)1E3, time);
                 // Smaller than threshold and not NaN
 
                 float vel = pos.vel/100.0f;
 
-                if (vel < 1000000 && !isnan(vel) && !isinf(vel))
-                {
-                    // FIXME REMOVE LATER emit valueChanged(uasId, "speed", "m/s", vel, time);
-                    //qDebug() << "GOT GPS RAW";
-                    // emit speedChanged(this, (double)pos.v, 0.0, 0.0, time);
-                }
-                else
-                {
-                    emit textMessageReceived(uasId, message.compid, 255, QString("GCS ERROR: RECEIVED INVALID SPEED OF %1 m/s").arg(vel));
+                if (!globalEstimatorActive) {
+                    if ((vel < 1000000) && !isnan(vel) && !isinf(vel))
+                    {
+                        emit speedChanged(this, vel, 0.0, 0.0, time);
+                    }
+                    else
+                    {
+                        emit textMessageReceived(uasId, message.compid, 255, QString("GCS ERROR: RECEIVED INVALID SPEED OF %1 m/s").arg(vel));
+                    }
                 }
             }
         }
@@ -905,7 +909,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 // Emit change
                 emit parameterChanged(uasId, message.compid, parameterName, param);
                 emit parameterChanged(uasId, message.compid, value.param_count, value.param_index, parameterName, param);
-//                qDebug() << "RECEIVED PARAM:" << param;
             }
                 break;
             case MAV_PARAM_TYPE_INT32:
@@ -1691,10 +1694,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;
 }
 
 /**
@@ -2717,11 +2732,23 @@ void UAS::sendHilState(uint64_t time_us, float roll, float pitch, float yaw, flo
 {
     if (this->mode & MAV_MODE_FLAG_HIL_ENABLED)
     {
-        mavlink_message_t msg;
-        mavlink_msg_hil_state_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg,
-                                   time_us, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed,
-                                   lat*1e7f, lon*1e7f, alt*1000, vx*100, vy*100, vz*100, xacc*1000, yacc*1000, zacc*1000);
-        sendMessage(msg);
+        if (sensorHil) {
+            // Emit attitude for cross-check
+            emit attitudeChanged(this, 201, roll, pitch, yaw, getUnixTime());
+            emit valueChanged(uasId, "roll sim", "rad", roll, getUnixTime());
+            emit valueChanged(uasId, "pitch sim", "rad", pitch, getUnixTime());
+            emit valueChanged(uasId, "yaw sim", "rad", yaw, getUnixTime());
+
+            emit valueChanged(uasId, "roll rate sim", "rad/s", rollspeed, getUnixTime());
+            emit valueChanged(uasId, "pitch rate sim", "rad/s", pitchspeed, getUnixTime());
+            emit valueChanged(uasId, "yaw rate sim", "rad/s", yawspeed, getUnixTime());
+        } else {
+            mavlink_message_t msg;
+            mavlink_msg_hil_state_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg,
+                                       time_us, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed,
+                                       lat*1e7f, lon*1e7f, alt*1000, vx*100, vy*100, vz*100, xacc*1000, yacc*1000, zacc*1000);
+            sendMessage(msg);
+        }
     }
     else
     {
@@ -2744,6 +2771,7 @@ void UAS::sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, fl
                                      xmag, ymag, zmag, abs_pressure, diff_pressure, pressure_alt, temperature,
                                      fields_changed);
         sendMessage(msg);
+        sensorHil = true;
     }
     else
     {
@@ -2782,6 +2810,7 @@ void UAS::startHil()
 {
     if (hilEnabled) return;
     hilEnabled = true;
+    sensorHil = false;
     mavlink_message_t msg;
     mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), mode | MAV_MODE_FLAG_HIL_ENABLED, navMode);
     sendMessage(msg);
@@ -2799,6 +2828,7 @@ void UAS::stopHil()
     mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), mode & !MAV_MODE_FLAG_HIL_ENABLED, navMode);
     sendMessage(msg);
     hilEnabled = false;
+    sensorHil = false;
 }
 
 void UAS::shutdown()

src/uas/UAS.h

@@ -278,9 +278,13 @@ protected: //COMMENTS FOR TEST UNIT
     double localX;
     double localY;
     double localZ;
-    double latitude;
-    double longitude;
-    double altitude;
+    double latitude;            ///< Global latitude as estimated by position estimator
+    double longitude;           ///< Global longitude as estimated by position estimator
+    double altitude;            ///< Global altitude as estimated by position estimator
+    bool globalEstimatorActive; ///< Global position estimator present, do not fall back to GPS raw for position
+    double latitude_gps;        ///< Global latitude as estimated by raw GPS
+    double longitude_gps;       ///< Global longitude as estimated by raw GPS
+    double altitude_gps;        ///< Global altitude as estimated by raw GPS
     double speedX;              ///< True speed in X axis
     double speedY;              ///< True speed in Y axis
     double speedZ;              ///< True speed in Z axis
@@ -699,6 +703,7 @@ protected:
     quint64 lastNonNullTime;    ///< The last timestamp from the MAV that was not null
     unsigned int onboardTimeOffsetInvalidCount;     ///< Count when the offboard time offset estimation seemed wrong
     bool hilEnabled;            ///< Set to true if HIL mode is enabled from GCS (UAS might be in HIL even if this flag is not set, this defines the GCS HIL setting)
+    bool sensorHil;             ///< True if sensor HIL is enabled
 
 protected slots:
     /** @brief Write settings to disk */

src/ui/CommConfigurationWindow.cc

@@ -274,6 +274,10 @@ void CommConfigurationWindow::setConnection()
 {
     if(!link->isConnected()) {
         link->connect();
+        QGC::SLEEP::msleep(100);
+        if (link->isConnected())
+            // Auto-close window on connect
+            this->window()->close();
     } else {
         link->disconnect();
     }

src/ui/HSIDisplay.cc

@@ -1310,9 +1310,13 @@ void HSIDisplay::drawSafetyArea(const QPointF &topLeft, const QPointF &bottomRig
 void HSIDisplay::drawGPS(QPainter &painter)
 {
     float xCenter = xCenterPos;
-    float yCenter = xCenterPos;
-    // Max satellite circle radius
+    float yCenter = yCenterPos;
 
+    const float yawDeg = ((yaw/M_PI)*180.0f);
+    int yawRotate = static_cast<int>(yawDeg) % 360;
+    // XXX check rotation direction
+
+    // Max satellite circle radius
     const float margin = 0.15f;  // 20% margin of total width on each side
     float radius = (vwidth - vwidth * 2.0f * margin) / 2.0f;
     quint64 currTime = MG::TIME::getGroundTimeNowUsecs();
@@ -1353,8 +1357,8 @@ void HSIDisplay::drawGPS(QPainter &painter)
             painter.setPen(color);
             painter.setBrush(brush);
 
-            float xPos = xCenter + (sin(((sat->azimuth/255.0f)*360.0f)/180.0f * M_PI) * cos(sat->elevation/180.0f * M_PI)) * radius;
-            float yPos = yCenter - (cos(((sat->azimuth/255.0f)*360.0f)/180.0f * M_PI) * cos(sat->elevation/180.0f * M_PI)) * radius;
+            float xPos = xCenter + (sin(((sat->azimuth/255.0f)*360.0f-yawRotate)/180.0f * M_PI) * cos(sat->elevation/180.0f * M_PI)) * radius;
+            float yPos = yCenter - (cos(((sat->azimuth/255.0f)*360.0f-yawRotate)/180.0f * M_PI) * cos(sat->elevation/180.0f * M_PI)) * radius;
 
             // Draw circle for satellite, filled for used satellites
             drawCircle(xPos, yPos, vwidth*0.02f, 1.0f, color, &painter);

src/ui/QGCMAVLinkLogPlayer.cc

@@ -22,6 +22,7 @@ QGCMAVLinkLogPlayer::QGCMAVLinkLogPlayer(MAVLinkProtocol* mavlink, QWidget *pare
     binaryBaudRate(57600),
     isPlaying(false),
     currPacketCount(0),
+    lastLogDirectory(QDesktopServices::storageLocation(QDesktopServices::DesktopLocation)),
     ui(new Ui::QGCMAVLinkLogPlayer)
 {
     ui->setupUi(this);
@@ -43,10 +44,12 @@ QGCMAVLinkLogPlayer::QGCMAVLinkLogPlayer(MAVLinkProtocol* mavlink, QWidget *pare
     setAccelerationFactorInt(49);
     ui->speedSlider->setValue(49);
     ui->positionSlider->setValue(ui->positionSlider->minimum());
+    loadSettings();
 }
 
 QGCMAVLinkLogPlayer::~QGCMAVLinkLogPlayer()
 {
+    storeSettings();
     delete ui;
 }
 
@@ -167,9 +170,41 @@ bool QGCMAVLinkLogPlayer::reset(int packetIndex)
     }
 }
 
+void QGCMAVLinkLogPlayer::loadSettings()
+{
+    QSettings settings;
+    settings.beginGroup("QGC_MAVLINKLOGPLAYER");
+    lastLogDirectory = settings.value("LAST_LOG_DIRECTORY", lastLogDirectory).toString();
+    settings.endGroup();
+}
+
+void QGCMAVLinkLogPlayer::storeSettings()
+{
+    QSettings settings;
+    settings.beginGroup("QGC_MAVLINKLOGPLAYER");
+    settings.setValue("LAST_LOG_DIRECTORY", lastLogDirectory);
+    settings.endGroup();
+    settings.sync();
+}
+
+/**
+ * @brief Select a log file
+ * @param startDirectory Directory where the file dialog will be opened
+ * @return filename of the logFile
+ */
 bool QGCMAVLinkLogPlayer::selectLogFile()
 {
-    QString fileName = QFileDialog::getOpenFileName(this, tr("Specify MAVLink log file name to replay"), QDesktopServices::storageLocation(QDesktopServices::DesktopLocation), tr("MAVLink or Binary Logfile (*.mavlink *.bin *.log)"));
+    return selectLogFile(lastLogDirectory);
+}
+
+/**
+ * @brief Select a log file
+ * @param startDirectory Directory where the file dialog will be opened
+ * @return filename of the logFile
+ */
+bool QGCMAVLinkLogPlayer::selectLogFile(const QString startDirectory)
+{
+    QString fileName = QFileDialog::getOpenFileName(this, tr("Specify MAVLink log file name to replay"), startDirectory, tr("MAVLink or Binary Logfile (*.mavlink *.bin *.log)"));
 
     if (fileName == "")
     {
@@ -177,6 +212,7 @@ bool QGCMAVLinkLogPlayer::selectLogFile()
     }
     else
     {
+        lastLogDirectory = fileName;
         return loadLogFile(fileName);
     }
 }
@@ -370,6 +406,7 @@ void QGCMAVLinkLogPlayer::logLoop()
             //qDebug() << "START TIME: " << startTime;
 
             // Check if these bytes could be correctly decoded
+            // TODO
             if (!ok)
             {
                 ui->logStatsLabel->setText(tr("Error decoding first timestamp, aborting."));
@@ -381,14 +418,15 @@ void QGCMAVLinkLogPlayer::logLoop()
 
 
         // Initialization seems fine, load next chunk
+        //this is ok because before we already read the timestamp of this paket before
         QByteArray chunk = logFile.read(timeLen+packetLen);
-        QByteArray packet = chunk.mid(0, packetLen);
+        QByteArray packet = chunk.left(packetLen);
 
         // Emit this packet
         emit bytesReady(logLink, packet);
 
         // Check if reached end of file before reading next timestamp
-        if (chunk.length() < timeLen+packetLen || logFile.atEnd())
+        if (chunk.length() < (timeLen + packetLen) || logFile.atEnd())
         {
             // Reached end of file
             reset();
@@ -400,6 +438,7 @@ void QGCMAVLinkLogPlayer::logLoop()
         }
 
         // End of file not reached, read next timestamp
+        // which is located after current packet
         QByteArray rawTime = chunk.mid(packetLen);
 
         // This is the timestamp of the next packet

src/ui/QGCMAVLinkLogPlayer.h

@@ -49,6 +49,8 @@ public slots:
     /** @brief Reset the logfile */
     bool reset(int packetIndex=0);
     /** @brief Select logfile */
+    bool selectLogFile(const QString startDirectory);
+    /** @brief Select logfile */
     bool selectLogFile();
     /** @brief Load log file */
     bool loadLogFile(const QString& file);
@@ -81,8 +83,12 @@ protected:
     unsigned int currPacketCount;
     static const int packetLen = MAVLINK_MAX_PACKET_LEN;
     static const int timeLen = sizeof(quint64);
+    QString lastLogDirectory;
     void changeEvent(QEvent *e);
 
+    void loadSettings();
+    void storeSettings();
+
 private:
     Ui::QGCMAVLinkLogPlayer *ui;
 };

src/ui/QGCToolBar.cc

@@ -23,6 +23,7 @@ This file is part of the QGROUNDCONTROL project
 
 #include <QToolButton>
 #include <QLabel>
+#include <QSpacerItem>
 #include "QGCToolBar.h"
 #include "UASManager.h"
 #include "MainWindow.h"
@@ -38,7 +39,9 @@ QGCToolBar::QGCToolBar(QWidget *parent) :
     batteryVoltage(0),
     wpId(0),
     wpDistance(0),
-    systemArmed(false)
+    systemArmed(false),
+    lastLogDirectory(QDesktopServices::storageLocation(QDesktopServices::DesktopLocation)),
+    currentLink(NULL)
 {
     setObjectName("QGC_TOOLBAR");
 
@@ -109,8 +112,13 @@ QGCToolBar::QGCToolBar(QWidget *parent) :
 	toolBarMessageLabel->setToolTip(tr("Most recent system message"));
     addWidget(toolBarMessageLabel);
 
+    QWidget* spacer = new QWidget();
+    spacer->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding);
+    addWidget(spacer);
+
     connectButton = new QPushButton(tr("Connect"), this);
     connectButton->setToolTip(tr("Connect wireless link to MAV"));
+    connectButton->setCheckable(true);
     addWidget(connectButton);
     connect(connectButton, SIGNAL(clicked(bool)), this, SLOT(connectLink(bool)));
 
@@ -120,9 +128,21 @@ QGCToolBar::QGCToolBar(QWidget *parent) :
     setActiveUAS(UASManager::instance()->getActiveUAS());
     connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*)));
 
+    if (LinkManager::instance()->getLinks().count() > 2)
+        addLink(LinkManager::instance()->getLinks().last());
+    // XXX implies that connect button is always active for the last used link
+    connect(LinkManager::instance(), SIGNAL(newLink(LinkInterface*)), this, SLOT(addLink(LinkInterface*)));
+
+    // Update label if required
+    if (LinkManager::instance()->getLinks().count() < 3) {
+        connectButton->setText(tr("New Link"));
+    }
+
     // Set the toolbar to be updated every 2s
     connect(&updateViewTimer, SIGNAL(timeout()), this, SLOT(updateView()));
     updateViewTimer.start(2000);
+
+    loadSettings();
 }
 
 void QGCToolBar::heartbeatTimeout(bool timeout, unsigned int ms)
@@ -171,13 +191,13 @@ void QGCToolBar::playLogFile(bool checked)
             player->playPause(false);
             if (checked)
             {
-                if (!player->selectLogFile()) return;
+                if (!player->selectLogFile(lastLogDirectory)) return;
             }
         }
         // If no replaying happens already, start it
         else
         {
-            if (!player->selectLogFile()) return;
+            if (!player->selectLogFile(lastLogDirectory)) return;
         }
         player->playPause(checked);
     }
@@ -192,7 +212,7 @@ void QGCToolBar::logging(bool checked)
     if (checked)
     {
 		// Prompt the user for a filename/location to save to
-        QString fileName = QFileDialog::getSaveFileName(this, tr("Specify MAVLink log file to save to"), QDesktopServices::storageLocation(QDesktopServices::DesktopLocation), tr("MAVLink Logfile (*.mavlink *.log *.bin);;"));
+        QString fileName = QFileDialog::getSaveFileName(this, tr("Specify MAVLink log file to save to"), lastLogDirectory, tr("MAVLink Logfile (*.mavlink *.log *.bin);;"));
 
 		// Check that they didn't cancel out
 		if (fileName.isNull())
@@ -224,6 +244,7 @@ void QGCToolBar::logging(bool checked)
         {
             MainWindow::instance()->getMAVLink()->setLogfileName(fileName);
             MainWindow::instance()->getMAVLink()->enableLogging(true);
+            lastLogDirectory = file.absoluteDir().absolutePath(); //save last log directory
         }
     }
 }
@@ -461,8 +482,52 @@ void QGCToolBar::receiveTextMessage(int uasid, int componentid, int severity, QS
     lastSystemMessageTimeMs = QGC::groundTimeMilliseconds();
 }
 
+void QGCToolBar::addLink(LinkInterface* link)
+{
+    // XXX magic number
+    if (LinkManager::instance()->getLinks().count() > 2) {
+        currentLink = link;
+        connect(currentLink, SIGNAL(connected(bool)), this, SLOT(updateLinkState(bool)));
+        updateLinkState(link->isConnected());
+    }
+}
+
+void QGCToolBar::removeLink(LinkInterface* link)
+{
+    if (link == currentLink) {
+        currentLink = NULL;
+        // XXX magic number
+        if (LinkManager::instance()->getLinks().count() > 2) {
+            currentLink = LinkManager::instance()->getLinks().last();
+            updateLinkState(currentLink->isConnected());
+        } else {
+            connectButton->setText(tr("New Link"));
+        }
+    }
+}
+
+void QGCToolBar::updateLinkState(bool connected)
+{
+    if (currentLink && currentLink->isConnected())
+    {
+        connectButton->setText(tr("Disconnect"));
+        connectButton->blockSignals(true);
+        connectButton->setChecked(true);
+        connectButton->blockSignals(false);
+    }
+    else
+    {
+        connectButton->setText(tr("Connect"));
+        connectButton->blockSignals(true);
+        connectButton->setChecked(false);
+        connectButton->blockSignals(false);
+    }
+}
+
 void QGCToolBar::connectLink(bool connect)
 {
+    // No serial port yet present
+    // XXX magic number
     if (connect && LinkManager::instance()->getLinks().count() < 3)
     {
         MainWindow::instance()->addLink();
@@ -471,19 +536,24 @@ void QGCToolBar::connectLink(bool connect)
     } else if (!connect && LinkManager::instance()->getLinks().count() > 2) {
         LinkManager::instance()->getLinks().last()->disconnect();
     }
+}
 
-    if (LinkManager::instance()->getLinks().count() > 2) {
-        if (LinkManager::instance()->getLinks().last()->isConnected())
-        {
-            connectButton->setText(tr("Disconnect"));
-        }
-        else
-        {
-            connectButton->setText(tr("Connect"));
-        }
 
-    }
+void QGCToolBar::loadSettings()
+{
+    QSettings settings;
+    settings.beginGroup("QGC_TOOLBAR");
+    lastLogDirectory = settings.value("LAST_LOG_DIRECTORY", lastLogDirectory).toString();
+    settings.endGroup();
+}
 
+void QGCToolBar::storeSettings()
+{
+    QSettings settings;
+    settings.beginGroup("QGC_TOOLBAR");
+    settings.setValue("LAST_LOG_DIRECTORY", lastLogDirectory);
+    settings.endGroup();
+    settings.sync();
 }
 
 void QGCToolBar::clearStatusString()
@@ -494,6 +564,7 @@ void QGCToolBar::clearStatusString()
 
 QGCToolBar::~QGCToolBar()
 {
+    storeSettings();
     if (toggleLoggingAction) toggleLoggingAction->deleteLater();
     if (logReplayAction) logReplayAction->deleteLater();
 }

src/ui/QGCToolBar.h

@@ -45,6 +45,12 @@ public:
 public slots:
     /** @brief Set the system that is currently displayed by this widget */
     void setActiveUAS(UASInterface* active);
+    /** @brief Set the link which is currently handled with connecting / disconnecting */
+    void addLink(LinkInterface* link);
+    /** @brief Remove link which is currently handled */
+    void removeLink(LinkInterface* link);
+    /** @brief Update the link state */
+    void updateLinkState(bool connected);
     /** @brief Set the system state */
     void updateState(UASInterface* system, QString name, QString description);
     /** @brief Set the system mode */
@@ -80,6 +86,8 @@ public slots:
 
 protected:
     void createCustomWidgets();
+    void storeSettings();
+    void loadSettings();
 
     QAction* toggleLoggingAction;
     QAction* logReplayAction;
@@ -109,6 +117,8 @@ protected:
     quint64 lastSystemMessageTimeMs;
     QTimer updateViewTimer;
     bool systemArmed;
+    QString lastLogDirectory;
+    LinkInterface* currentLink;
 };
 
 #endif // QGCTOOLBAR_H

src/ui/designer/QGCCommandButton.cc

@@ -118,7 +118,7 @@ void QGCCommandButton::sendCommand()
                 int component = ui->editComponentSpinBox->value();
 
                 QGCToolWidgetItem::uas->executeCommand(command, confirm, param1, param2, param3, param4, param5, param6, param7, component);
-                qDebug() << __FILE__ << __LINE__ << "SENDING COMMAND" << index;
+                //qDebug() << __FILE__ << __LINE__ << "SENDING COMMAND" << index;
             }
         }
     }
@@ -226,7 +226,7 @@ void QGCCommandButton::endEditMode()
 
 void QGCCommandButton::writeSettings(QSettings& settings)
 {
-    qDebug() << "COMMAND BUTTON WRITING SETTINGS";
+    //qDebug() << "COMMAND BUTTON WRITING SETTINGS";
     settings.setValue("TYPE", "COMMANDBUTTON");
     settings.setValue("QGC_COMMAND_BUTTON_DESCRIPTION", ui->nameLabel->text());
     settings.setValue("QGC_COMMAND_BUTTON_BUTTONTEXT", ui->commandButton->text());

src/ui/designer/QGCToolWidget.cc

@@ -28,7 +28,7 @@ QGCToolWidget::QGCToolWidget(const QString& title, QWidget *parent, QSettings* s
     {
         widgetTitle = QString("%1 %2").arg(title).arg(QGCToolWidget::instances()->count());
     }
-    qDebug() << "WidgetTitle" << widgetTitle;
+    //qDebug() << "WidgetTitle" << widgetTitle;
 
     setObjectName(widgetTitle);
     createActions();
@@ -99,12 +99,12 @@ QList<QGCToolWidget*> QGCToolWidget::createWidgetsFromSettings(QWidget* parent,
     if (!settingsFile.isEmpty())
     {
         settings = new QSettings(settingsFile, QSettings::IniFormat);
-        qDebug() << "LOADING SETTINGS FROM" << settingsFile;
+       //qDebug() << "LOADING SETTINGS FROM" << settingsFile;
     }
     else
     {
         settings = new QSettings();
-        qDebug() << "LOADING SETTINGS FROM DEFAULT" << settings->fileName();
+        //qDebug() << "LOADING SETTINGS FROM DEFAULT" << settings->fileName();
     }
 
     QList<QGCToolWidget*> newWidgets;
@@ -116,7 +116,7 @@ QList<QGCToolWidget*> QGCToolWidget::createWidgetsFromSettings(QWidget* parent,
 
         if (!instances()->contains(name) && name.length() != 0)
         {
-            qDebug() << "CREATED WIDGET:" << name;
+            //qDebug() << "CREATED WIDGET:" << name;
             QGCToolWidget* tool = new QGCToolWidget(name, parent, settings);
             newWidgets.append(tool);
         }
@@ -127,12 +127,12 @@ QList<QGCToolWidget*> QGCToolWidget::createWidgetsFromSettings(QWidget* parent,
         }
         else
         {
-            qDebug() << "WIDGET" << name << "DID ALREADY EXIST, REJECTING";
+            //qDebug() << "WIDGET" << name << "DID ALREADY EXIST, REJECTING";
         }
     }
     settings->endArray();
 
-    qDebug() << "NEW WIDGETS: " << newWidgets.size();
+    //qDebug() << "NEW WIDGETS: " << newWidgets.size();
 
     // Load individual widget items
     for (int i = 0; i < newWidgets.size(); i++)
@@ -158,7 +158,7 @@ bool QGCToolWidget::loadSettings(const QString& settings, bool singleinstance)
         // Do not use setTitle() here,
         // interferes with loading settings
         widgetTitle = widgetName;
-        qDebug() << "WIDGET TITLE LOADED: " << widgetName;
+        //qDebug() << "WIDGET TITLE LOADED: " << widgetName;
         loadSettings(set);
         return true;
     }
@@ -172,9 +172,9 @@ void QGCToolWidget::loadSettings(QSettings& settings)
 {
     QString widgetName = getTitle();
     settings.beginGroup(widgetName);
-    qDebug() << "LOADING FOR" << widgetName;
+    //qDebug() << "LOADING FOR" << widgetName;
     int size = settings.beginReadArray("QGC_TOOL_WIDGET_ITEMS");
-    qDebug() << "CHILDREN SIZE:" << size;
+    //qDebug() << "CHILDREN SIZE:" << size;
     for (int j = 0; j < size; j++)
     {
         settings.setArrayIndex(j);
@@ -185,12 +185,12 @@ void QGCToolWidget::loadSettings(QSettings& settings)
             if (type == "COMMANDBUTTON")
             {
                 item = new QGCCommandButton(this);
-                qDebug() << "CREATED COMMANDBUTTON";
+                //qDebug() << "CREATED COMMANDBUTTON";
             }
             else if (type == "SLIDER")
             {
                 item = new QGCParamSlider(this);
-                qDebug() << "CREATED PARAM SLIDER";
+                //qDebug() << "CREATED PARAM SLIDER";
             }
 
             if (item)
@@ -199,12 +199,12 @@ void QGCToolWidget::loadSettings(QSettings& settings)
                 addToolWidget(item);
                 item->readSettings(settings);
 
-                qDebug() << "Created tool widget";
+                //qDebug() << "Created tool widget";
             }
         }
         else
         {
-            qDebug() << "UNKNOWN TOOL WIDGET TYPE";
+            //qDebug() << "UNKNOWN TOOL WIDGET TYPE";
         }
     }
     settings.endArray();
@@ -218,12 +218,12 @@ void QGCToolWidget::storeWidgetsToSettings(QString settingsFile)
     if (!settingsFile.isEmpty())
     {
         settings = new QSettings(settingsFile, QSettings::IniFormat);
-        qDebug() << "STORING SETTINGS TO" << settings->fileName();
+        //qDebug() << "STORING SETTINGS TO" << settings->fileName();
     }
     else
     {
         settings = new QSettings();
-        qDebug() << "STORING SETTINGS TO DEFAULT" << settings->fileName();
+        //qDebug() << "STORING SETTINGS TO DEFAULT" << settings->fileName();
     }
 
     int preArraySize = settings->beginReadArray("QGC_TOOL_WIDGET_NAMES");
@@ -237,7 +237,7 @@ void QGCToolWidget::storeWidgetsToSettings(QString settingsFile)
         {
             // Updating value
             settings->setValue("TITLE", instances()->values().at(i)->getTitle());
-            qDebug() << "WRITING TITLE" << instances()->values().at(i)->getTitle();
+            //qDebug() << "WRITING TITLE" << instances()->values().at(i)->getTitle();
         }
         else
         {
@@ -269,7 +269,7 @@ void QGCToolWidget::storeSettings(const QString& settingsFile)
 
 void QGCToolWidget::storeSettings(QSettings& settings)
 {
-    qDebug() << "WRITING WIDGET" << widgetTitle << "TO SETTINGS";
+    //qDebug() << "WRITING WIDGET" << widgetTitle << "TO SETTINGS";
     settings.beginGroup(widgetTitle);
     settings.beginWriteArray("QGC_TOOL_WIDGET_ITEMS");
     int k = 0; // QGCToolItem counter
@@ -285,7 +285,7 @@ void QGCToolWidget::storeSettings(QSettings& settings)
             item->writeSettings(settings);
         }
     }
-    qDebug() << "WROTE" << k << "SUB-WIDGETS TO SETTINGS";
+    //qDebug() << "WROTE" << k << "SUB-WIDGETS TO SETTINGS";
     settings.endArray();
     settings.endGroup();
 }

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()));