Merge pull request #428 from DrTon/alt_speed_fix

Altitude and speed rewrite

src/comm/QGCXPlaneLink.cc

@@ -911,14 +911,14 @@ void QGCXPlaneLink::setRandomPosition()
     double offLon = rand() / static_cast<double>(RAND_MAX) / 500.0 + 1.0/500.0;
     double offAlt = rand() / static_cast<double>(RAND_MAX) * 200.0 + 100.0;
 
-    if (mav->getAltitude() + offAlt < 0)
+    if (mav->getAltitudeAMSL() + offAlt < 0)
     {
         offAlt *= -1.0;
     }
 
     setPositionAttitude(mav->getLatitude() + offLat,
                         mav->getLongitude() + offLon,
-                        mav->getAltitude() + offAlt,
+                        mav->getAltitudeAMSL() + offAlt,
                         mav->getRoll(),
                         mav->getPitch(),
                         mav->getYaw());
@@ -935,7 +935,7 @@ void QGCXPlaneLink::setRandomAttitude()
 
     setPositionAttitude(mav->getLatitude(),
                         mav->getLongitude(),
-                        mav->getAltitude(),
+                        mav->getAltitudeAMSL(),
                         roll,
                         pitch,
                         yaw);

src/uas/UAS.cc

@@ -112,7 +112,15 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(),
 
     latitude(0.0),
     longitude(0.0),
-    altitude(0.0),
+    altitudeAMSL(0.0),
+    altitudeRelative(0.0),
+
+    airSpeed(NAN),
+    groundSpeed(NAN),
+
+    speedX(0.0),
+    speedY(0.0),
+    speedZ(0.0),
 
     globalEstimatorActive(false),
     latitude_gps(0.0),
@@ -828,18 +836,17 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
 
             if (!attitudeKnown)
             {
-                //yaw = QGC::limitAngleToPMPId((((double)hud.heading-180.0)/360.0)*M_PI);
-                setYaw(QGC::limitAngleToPMPId((((double)hud.heading-180.0)/360.0)*M_PI));
+                setYaw(QGC::limitAngleToPMPId((((double)hud.heading)/180.0)*M_PI));
                 emit attitudeChanged(this, getRoll(), getPitch(), getYaw(), time);
             }
 
-            // The primary altitude is the one that the UAV uses for navigation.
-            // We assume! that the HUD message reports that as altitude.
-            emit primaryAltitudeChanged(this, hud.alt, time);
-
-            emit primarySpeedChanged(this, hud.airspeed, time);
-            emit gpsSpeedChanged(this, hud.groundspeed, time);
-            emit climbRateChanged(this, hud.climb, time);
+            setAltitudeAMSL(hud.alt);
+            setGroundSpeed(hud.groundspeed);
+            if (!isnan(hud.airspeed))
+                setAirSpeed(hud.airspeed);
+            speedZ = -hud.climb;
+            emit altitudeChanged(this, altitudeAMSL, altitudeRelative, -speedZ, time);
+            emit speedChanged(this, groundSpeed, airSpeed, time);
         }
             break;
         case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
@@ -855,13 +862,17 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
 
             if (!wrongComponent)
             {
-                localX = pos.x;
-                localY = pos.y;
-                localZ = pos.z;
+                setLocalX(pos.x);
+                setLocalY(pos.y);
+                setLocalZ(pos.z);
+
+                speedX = pos.vx;
+                speedY = pos.vy;
+                speedZ = pos.vz;
 
                 // Emit
-                emit localPositionChanged(this, pos.x, pos.y, pos.z, time);
-                emit velocityChanged_NED(this, pos.vx, pos.vy, pos.vz, time);
+                emit localPositionChanged(this, localX, localY, localZ, time);
+                emit velocityChanged_NED(this, speedX, speedY, speedZ, time);
 
                 // Set internal state
                 if (!positionLock) {
@@ -888,15 +899,13 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
         {
             mavlink_global_position_int_t pos;
             mavlink_msg_global_position_int_decode(&message, &pos);
+
             quint64 time = getUnixTime();
+
             setLatitude(pos.lat/(double)1E7);
             setLongitude(pos.lon/(double)1E7);
-
-            // dongfang: Beware. There are 2 altitudes in this message; neither is the primary.
-            // pos.alt is GPS altitude and pos.relative_alt is above-home altitude.
-            // It would be nice if APM could be modified to present the primary (mix) alt. instead
-            // of the GPS alt. in this message.
-            setAltitude(pos.alt/1000.0);
+            setAltitudeAMSL(pos.alt/1000.0);
+            setAltitudeRelative(pos.relative_alt/1000.0);
 
             globalEstimatorActive = true;
 
@@ -904,14 +913,13 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
             speedY = pos.vy/100.0;
             speedZ = pos.vz/100.0;
 
-            emit globalPositionChanged(this, getLatitude(), getLongitude(), getAltitude(), time);
-            // dongfang: The altitude is GPS altitude. Bugger. It needs to be changed to primary.
-            emit gpsAltitudeChanged(this, getAltitude(), time);
+            emit globalPositionChanged(this, getLatitude(), getLongitude(), getAltitudeAMSL(), time);
+            emit altitudeChanged(this, altitudeAMSL, altitudeRelative, -speedZ, time);
             // We had some frame mess here, global and local axes were mixed.
             emit velocityChanged_NED(this, speedX, speedY, speedZ, time);
 
-            double groundspeed = qSqrt(speedX*speedX+speedY*speedY);
-            emit gpsSpeedChanged(this, groundspeed, time);
+            setGroundSpeed(qSqrt(speedX*speedX+speedY*speedY));
+            emit speedChanged(this, groundSpeed, airSpeed, time);
 
             // Set internal state
             if (!positionLock)
@@ -930,9 +938,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
             mavlink_gps_raw_int_t pos;
             mavlink_msg_gps_raw_int_decode(&message, &pos);
 
-            // SANITY CHECK
-            // only accept values in a realistic range
-            // quint64 time = getUnixTime(pos.time_usec);
             quint64 time = getUnixTime(pos.time_usec);
 
             emit gpsLocalizationChanged(this, pos.fix_type);
@@ -947,6 +952,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
 
             if (pos.fix_type > 2)
             {
+                positionLock = true;
+                isGlobalPositionKnown = true;
 
                 latitude_gps = pos.lat/(double)1E7;
                 longitude_gps = pos.lon/(double)1E7;
@@ -956,29 +963,16 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 if (!globalEstimatorActive) {
                     setLatitude(latitude_gps);
                     setLongitude(longitude_gps);
-                    setAltitude(altitude_gps);
-                    emit globalPositionChanged(this, getLatitude(), getLongitude(), getAltitude(), time);
-                    emit gpsAltitudeChanged(this, getAltitude(), time);
-                }
+                    setAltitudeAMSL(altitude_gps);
+                    emit globalPositionChanged(this, getLatitude(), getLongitude(), getAltitudeAMSL(), time);
+                    emit altitudeChanged(this, altitudeAMSL, altitudeRelative, -speedZ, time);
 
-                positionLock = true;
-                isGlobalPositionKnown = true;
-
-                // Smaller than threshold and not NaN
-
-                float vel = pos.vel/100.0f;
-
-                // If no GLOBAL_POSITION_INT messages ever received, use these raw GPS values instead.
-                if (!globalEstimatorActive) {
-                    if ((vel < 1000000) && !isnan(vel) && !isinf(vel))
-                    {
-                        //emit speedChanged(this, vel, 0.0, 0.0, time);
+                    float vel = pos.vel/100.0f;
+                    // Smaller than threshold and not NaN
+                    if ((vel < 1000000) && !isnan(vel) && !isinf(vel)) {
                         setGroundSpeed(vel);
-                        // TODO: Other sources also? Actually this condition does not quite belong here.
-                        emit gpsSpeedChanged(this, vel, time);
-                    }
-                    else
-                    {
+                        emit speedChanged(this, groundSpeed, airSpeed, time);
+                    } else {
                         emit textMessageReceived(uasId, message.compid, 255, QString("GCS ERROR: RECEIVED INVALID SPEED OF %1 m/s").arg(vel));
                     }
                 }

src/uas/UAS.h

@@ -103,7 +103,11 @@ public:
     Q_PROPERTY(double pitch READ getPitch WRITE setPitch NOTIFY pitchChanged)
     Q_PROPERTY(double yaw READ getYaw WRITE setYaw NOTIFY yawChanged)
     Q_PROPERTY(double distToWaypoint READ getDistToWaypoint WRITE setDistToWaypoint NOTIFY distToWaypointChanged)
+    Q_PROPERTY(double airSpeed READ getGroundSpeed WRITE setGroundSpeed NOTIFY airSpeedChanged)
     Q_PROPERTY(double groundSpeed READ getGroundSpeed WRITE setGroundSpeed NOTIFY groundSpeedChanged)
+    Q_PROPERTY(double bearingToWaypoint READ getBearingToWaypoint WRITE setBearingToWaypoint NOTIFY bearingToWaypointChanged)
+    Q_PROPERTY(double altitudeAMSL READ getAltitudeAMSL WRITE setAltitudeAMSL NOTIFY altitudeAMSLChanged)
+    Q_PROPERTY(double altitudeRelative READ getAltitudeRelative WRITE setAltitudeRelative NOTIFY altitudeRelativeChanged)
 
     void setGroundSpeed(double val)
     {
@@ -115,17 +119,24 @@ public:
     {
         return groundSpeed;
     }
-    Q_PROPERTY(double bearingToWaypoint READ getBearingToWaypoint WRITE setBearingToWaypoint NOTIFY bearingToWaypointChanged)
 
-    // dongfang: There is not only one altitude; there are at least (APM) GPS altitude, mix altitude and mix-altitude relative to home.
-    // I have made this property refer to the mix-altitude ASL as this is the one actually used in navigation by APM.
-    Q_PROPERTY(double altitude READ getAltitude WRITE setAltitude NOTIFY altitudeChanged)
+    void setAirSpeed(double val)
+    {
+        airSpeed = val;
+        emit airSpeedChanged(val,"airSpeed");
+        emit valueChanged(this->uasId,"airSpeed","m/s",QVariant(val),getUnixTime());
+    }
+
+    double getAirSpeed() const
+    {
+        return airSpeed;
+    }
 
     void setLocalX(double val)
     {
         localX = val;
         emit localXChanged(val,"localX");
-        emit valueChanged(this->uasId,"localX","M",QVariant(val),getUnixTime());
+        emit valueChanged(this->uasId,"localX","m",QVariant(val),getUnixTime());
     }
 
     double getLocalX() const
@@ -137,7 +148,7 @@ public:
     {
         localY = val;
         emit localYChanged(val,"localY");
-        emit valueChanged(this->uasId,"localY","M",QVariant(val),getUnixTime());
+        emit valueChanged(this->uasId,"localY","m",QVariant(val),getUnixTime());
     }
     double getLocalY() const
     {
@@ -148,7 +159,7 @@ public:
     {
         localZ = val;
         emit localZChanged(val,"localZ");
-        emit valueChanged(this->uasId,"localZ","M",QVariant(val),getUnixTime());
+        emit valueChanged(this->uasId,"localZ","m",QVariant(val),getUnixTime());
     }
     double getLocalZ() const
     {
@@ -179,23 +190,35 @@ public:
         return longitude;
     }
 
-    void setAltitude(double val)
+    void setAltitudeAMSL(double val)
     {
-        altitude = val;
-        emit altitudeChanged(val, "altitude");
-        emit valueChanged(this->uasId,"altitude","M",QVariant(val),getUnixTime());
+        altitudeAMSL = val;
+        emit altitudeAMSLChanged(val, "altitudeAMSL");
+        emit valueChanged(this->uasId,"altitudeAMSL","m",QVariant(val),getUnixTime());
     }
 
-    double getAltitude() const
+    double getAltitudeAMSL() const
     {
-        return altitude;
+        return altitudeAMSL;
+    }
+
+    void setAltitudeRelative(double val)
+    {
+        altitudeRelative = val;
+        emit altitudeRelativeChanged(val, "altitudeRelative");
+        emit valueChanged(this->uasId,"altitudeRelative","m",QVariant(val),getUnixTime());
+    }
+
+    double getAltitudeRelative() const
+    {
+        return altitudeRelative;
     }
 
     void setSatelliteCount(double val)
     {
         satelliteCount = val;
         emit satelliteCountChanged(val,"satelliteCount");
-        emit valueChanged(this->uasId,"satelliteCount","M",QVariant(val),getUnixTime());
+        emit valueChanged(this->uasId,"satelliteCount","",QVariant(val),getUnixTime());
     }
 
     double getSatelliteCount() const
@@ -217,7 +240,7 @@ public:
     {
         distToWaypoint = val;
         emit distToWaypointChanged(val,"distToWaypoint");
-        emit valueChanged(this->uasId,"distToWaypoint","M",QVariant(val),getUnixTime());
+        emit valueChanged(this->uasId,"distToWaypoint","m",QVariant(val),getUnixTime());
     }
 
     double getDistToWaypoint() const
@@ -229,7 +252,7 @@ public:
     {
         bearingToWaypoint = val;
         emit bearingToWaypointChanged(val,"bearingToWaypoint");
-        emit valueChanged(this->uasId,"bearingToWaypoint","M",QVariant(val),getUnixTime());
+        emit valueChanged(this->uasId,"bearingToWaypoint","deg",QVariant(val),getUnixTime());
     }
 
     double getBearingToWaypoint() const
@@ -414,8 +437,8 @@ protected: //COMMENTS FOR TEST UNIT
 
     /// POSITION
     bool positionLock;          ///< Status if position information is available or not
-    bool isLocalPositionKnown;      ///< If the local position has been received for this MAV
-    bool isGlobalPositionKnown;     ///< If the global position has been received for this MAV
+    bool isLocalPositionKnown;  ///< If the local position has been received for this MAV
+    bool isGlobalPositionKnown; ///< If the global position has been received for this MAV
 
     double localX;
     double localY;
@@ -423,7 +446,8 @@ protected: //COMMENTS FOR TEST UNIT
 
     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
+    double altitudeAMSL;        ///< Global altitude as estimated by position estimator
+    double altitudeRelative;    ///< Altitude above home as estimated by position estimator
 
     double satelliteCount;      ///< Number of satellites visible to raw GPS
     bool globalEstimatorActive; ///< Global position estimator present, do not fall back to GPS raw for position
@@ -439,7 +463,8 @@ protected: //COMMENTS FOR TEST UNIT
 
     /// WAYPOINT NAVIGATION
     double distToWaypoint;       ///< Distance to next waypoint
-    double groundSpeed;         ///< GPS Groundspeed
+    double airSpeed;             ///< Airspeed
+    double groundSpeed;          ///< Groundspeed
     double bearingToWaypoint;    ///< Bearing to next waypoint
     UASWaypointManager waypointManager;
 
@@ -921,18 +946,16 @@ signals:
     void localZChanged(double val,QString name);
     void longitudeChanged(double val,QString name);
     void latitudeChanged(double val,QString name);
-    void altitudeChanged(double val,QString name);
+    void altitudeAMSLChanged(double val,QString name);
+    void altitudeRelativeChanged(double val,QString name);
     void rollChanged(double val,QString name);
     void pitchChanged(double val,QString name);
     void yawChanged(double val,QString name);
     void satelliteCountChanged(double val,QString name);
     void distToWaypointChanged(double val,QString name);
     void groundSpeedChanged(double val, QString name);
+    void airSpeedChanged(double val, QString name);
     void bearingToWaypointChanged(double val,QString name);
-
-    //void primaryAltitudeChanged(UASInterface*, double altitude, quint64 usec);
-    //void gpsAltitudeChanged(UASInterface*, double altitude, quint64 usec);
-    //void velocityChanged_NED(UASInterface*, double vx, double vy, double vz, quint64 usec);
 protected:
     /** @brief Get the UNIX timestamp in milliseconds, enter microseconds */
     quint64 getUnixTime(quint64 time=0);

src/uas/UASInterface.h

@@ -124,7 +124,8 @@ public:
 
     virtual double getLatitude() const = 0;
     virtual double getLongitude() const = 0;
-    virtual double getAltitude() const = 0;
+    virtual double getAltitudeAMSL() const = 0;
+    virtual double getAltitudeRelative() const = 0;
     virtual bool globalPositionKnown() const = 0;
 
     virtual double getRoll() const = 0;
@@ -523,16 +524,12 @@ signals:
     void localPositionChanged(UASInterface*, double x, double y, double z, quint64 usec);
     void localPositionChanged(UASInterface*, int component, double x, double y, double z, quint64 usec);
     void globalPositionChanged(UASInterface*, double lat, double lon, double alt, quint64 usec);
-    void primaryAltitudeChanged(UASInterface*, double altitude, quint64 usec);
-    void gpsAltitudeChanged(UASInterface*, double altitude, quint64 usec);
+    void altitudeChanged(UASInterface*, double altitudeAMSL, double altitudeRelative, double climbRate, quint64 usec);
     /** @brief Update the status of one satellite used for localization */
     void gpsSatelliteStatusChanged(int uasid, int satid, float azimuth, float direction, float snr, bool used);
 
     // The horizontal speed (a scalar)
-    void primarySpeedChanged(UASInterface*, double speed, quint64 usec);
-    void gpsSpeedChanged(UASInterface*, double speed, quint64 usec);
-    // The vertical speed (a scalar)
-    void climbRateChanged(UASInterface*, double climb, quint64 usec);
+    void speedChanged(UASInterface* uas, double groundSpeed, double airSpeed, quint64 usec);
     // Consider adding a MAV_FRAME parameter to this; could help specifying what the 3 scalars are.
     void velocityChanged_NED(UASInterface*, double vx, double vy, double vz, quint64 usec);
 

src/ui/PrimaryFlightDisplay.cc

@@ -87,10 +87,6 @@ static const int AIRSPEED_LINEAR_SPAN = 15;
 static const int AIRSPEED_LINEAR_RESOLUTION = 1;
 static const int AIRSPEED_LINEAR_MAJOR_RESOLUTION = 5;
 
-static const int UNKNOWN_ATTITUDE = -1000;
-static const int UNKNOWN_ALTITUDE = -1000;
-static const int UNKNOWN_SPEED = -1;
-
 /*
  *@TODO:
  * global fixed pens (and painters too?)
@@ -125,26 +121,19 @@ PrimaryFlightDisplay::PrimaryFlightDisplay(int width, int height, QWidget *paren
 
     uas(NULL),
 
-    /*
-    altimeterMode(GPS_MAIN),
-    altimeterFrame(ASL),
-    speedMode(GROUND_MAIN),
-    */
-
-    roll(UNKNOWN_ATTITUDE),
-    pitch(UNKNOWN_ATTITUDE),
-    heading(UNKNOWN_ATTITUDE),
+    roll(0),
+    pitch(0),
+    heading(0),
 
-    primaryAltitude(UNKNOWN_ALTITUDE),
-    GPSAltitude(UNKNOWN_ALTITUDE),
-    aboveHomeAltitude(UNKNOWN_ALTITUDE),
+    altitudeAMSL(NAN),
+    altitudeRelative(NAN),
 
-    primarySpeed(UNKNOWN_SPEED),
-    groundspeed(UNKNOWN_SPEED),
-    verticalVelocity(UNKNOWN_ALTITUDE),
+    groundSpeed(NAN),
+    airSpeed(NAN),
+    climbRate(NAN),
 
     navigationCrosstrackError(0),
-    navigationTargetBearing(UNKNOWN_ATTITUDE),
+    navigationTargetBearing(NAN),
 
     layout(COMPASS_INTEGRATED),
     style(OVERLAY_HSI),
@@ -262,32 +251,11 @@ void PrimaryFlightDisplay::forgetUAS(UASInterface* uas)
 {
     if (this->uas != NULL && this->uas == uas) {
         // Disconnect any previously connected active MAV
-        disconnect(this->uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)),
-                   this, SLOT(updateAttitude(UASInterface*, double, double, double, quint64)));
-        disconnect(this->uas, SIGNAL(attitudeChanged(UASInterface*,int,double,double,double,quint64)),
-                   this, SLOT(updateAttitude(UASInterface*,int,double, double, double, quint64)));
-        //disconnect(this->uas, SIGNAL(waypointSelected(int,int)),
-        //           this, SLOT(selectWaypoint(int, int)));
-        disconnect(this->uas, SIGNAL(primarySpeedChanged(UASInterface*, double, quint64)),
-                   this, SLOT(updatePrimarySpeed(UASInterface*,double,quint64)));
-        disconnect(this->uas, SIGNAL(gpsSpeedChanged(UASInterface*, double, quint64)),
-                   this, SLOT(updateGPSSpeed(UASInterface*,double,quint64)));
-        disconnect(this->uas, SIGNAL(climbRateChanged(UASInterface*, double, quint64)),
-                   this,SLOT(updateClimbRate(UASInterface*, double, quint64)));
-        disconnect(this->uas, SIGNAL(primaryAltitudeChanged(UASInterface*, double, quint64)),
-                   this, SLOT(updatePrimaryAltitude(UASInterface*, double, quint64)));
-        disconnect(this->uas, SIGNAL(gpsAltitudeChanged(UASInterface*, double, quint64)),
-                   this, SLOT(updateGPSAltitude(UASInterface*, double, quint64)));
-        disconnect(this->uas, SIGNAL(navigationControllerErrorsChanged(UASInterface*, double, double, double)),
-                   this, SLOT(updateNavigationControllerErrors(UASInterface*, double, double, double)));
-
-        //disconnect(this->uas, SIGNAL(batteryChanged(UASInterface*, double, double, double, int)), this, SLOT(updateBattery(UASInterface*, double, double, double, int)));
-        //disconnect(this->uas, SIGNAL(statusChanged(UASInterface*,QString,QString)), this, SLOT(updateState(UASInterface*,QString)));
-        //disconnect(this->uas, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString)));
-        //disconnect(this->uas, SIGNAL(heartbeat(UASInterface*)), this, SLOT(receiveHeartbeat(UASInterface*)));
-        //disconnect(this->uas, SIGNAL(armingChanged(bool)), this, SLOT(updateArmed(bool)));
-        //disconnect(this->uas, SIGNAL(satelliteCountChanged(double, QString)), this, SLOT(updateSatelliteCount(double, QString)));
-        //disconnect(this->uas, SIGNAL(localizationChanged(UASInterface* uas, int fix)), this, SLOT(updateGPSFixType(UASInterface*,int)));
+        disconnect(this->uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*, double, double, double, quint64)));
+        disconnect(this->uas, SIGNAL(attitudeChanged(UASInterface*,int,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,int,double, double, double, quint64)));
+        disconnect(this->uas, SIGNAL(speedChanged(UASInterface*, double, double, quint64)), this, SLOT(updateSpeed(UASInterface*, double, double, quint64)));
+        disconnect(this->uas, SIGNAL(altitudeChanged(UASInterface*, double, double, double, quint64)), this, SLOT(updateAltitude(UASInterface*, double, double, double, quint64)));
+        disconnect(this->uas, SIGNAL(navigationControllerErrorsChanged(UASInterface*, double, double, double)), this, SLOT(updateNavigationControllerErrors(UASInterface*, double, double, double)));
     }
 }
 
@@ -308,24 +276,8 @@ void PrimaryFlightDisplay::setActiveUAS(UASInterface* uas)
         // Setup communication
         connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*, double, double, double, quint64)));
         connect(uas, SIGNAL(attitudeChanged(UASInterface*,int,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,int,double, double, double, quint64)));
-
-        //connect(uas, SIGNAL(batteryChanged(UASInterface*, double, double, double, int)), this, SLOT(updateBattery(UASInterface*, double, double, double, int)));
-        //connect(uas, SIGNAL(statusChanged(UASInterface*,QString,QString)), this, SLOT(updateState(UASInterface*,QString)));
-        //connect(uas, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString)));
-        //connect(uas, SIGNAL(heartbeat(UASInterface*)), this, SLOT(receiveHeartbeat(UASInterface*)));
-        //connect(uas, SIGNAL(armingChanged(bool)), this, SLOT(updateArmed(bool)));
-        //connect(uas, SIGNAL(satelliteCountChanged(double, QString)), this, SLOT(updateSatelliteCount(double, QString)));
-
-        //connect(uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64)));
-        //connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64)));
-        //connect(uas, SIGNAL(waypointSelected(int,int)), this,
-        //        SLOT(selectWaypoint(int, int)));
-        connect(uas, SIGNAL(primarySpeedChanged(UASInterface*, double, quint64)), this, SLOT(updatePrimarySpeed(UASInterface*,double,quint64)));
-        connect(uas, SIGNAL(gpsSpeedChanged(UASInterface*, double, quint64)), this, SLOT(updateGPSSpeed(UASInterface*,double,quint64)));
-        connect(uas, SIGNAL(climbRateChanged(UASInterface*, double, quint64)), this,
-                SLOT(updateClimbRate(UASInterface*, double, quint64)));
-        connect(uas, SIGNAL(primaryAltitudeChanged(UASInterface*, double, quint64)), this, SLOT(updatePrimaryAltitude(UASInterface*, double, quint64)));
-        connect(uas, SIGNAL(gpsAltitudeChanged(UASInterface*, double, quint64)), this, SLOT(updateGPSAltitude(UASInterface*, double, quint64)));
+        connect(uas, SIGNAL(speedChanged(UASInterface*, double, double, quint64)), this, SLOT(updateSpeed(UASInterface*, double, double, quint64)));
+        connect(uas, SIGNAL(altitudeChanged(UASInterface*, double, double, double, quint64)), this, SLOT(updateAltitude(UASInterface*, double, double, double, quint64)));
         connect(uas, SIGNAL(navigationControllerErrorsChanged(UASInterface*, double, double, double)), this, SLOT(updateNavigationControllerErrors(UASInterface*, double, double, double)));
 
         // Set new UAS
@@ -338,20 +290,20 @@ void PrimaryFlightDisplay::updateAttitude(UASInterface* uas, double roll, double
     Q_UNUSED(uas);
     Q_UNUSED(timestamp);
         // Called from UAS.cc l. 616
-        if (isnan(roll) || isinf(roll)) {
-            this->roll = UNKNOWN_ATTITUDE;
+        if (isinf(roll)) {
+            this->roll = NAN;
         } else {
             this->roll = roll * (180.0 / M_PI);
         }
 
-        if (isnan(pitch) || isinf(pitch)) {
-            this->pitch = UNKNOWN_ATTITUDE;
+        if (isinf(pitch)) {
+            this->pitch = NAN;
         } else {
             this->pitch = pitch * (180.0 / M_PI);
         }
 
-        if (isnan(yaw) || isinf(yaw)) {
-            this->heading = UNKNOWN_ATTITUDE;
+        if (isinf(yaw)) {
+            this->heading = NAN;
         } else {
             yaw = yaw * (180.0 / M_PI);
             if (yaw<0) yaw+=360;
@@ -366,44 +318,21 @@ void PrimaryFlightDisplay::updateAttitude(UASInterface* uas, int component, doub
     this->updateAttitude(uas, roll, pitch, yaw, timestamp);
 }
 
-void PrimaryFlightDisplay::updatePrimarySpeed(UASInterface* uas, double speed, quint64 timestamp)
-{
-    Q_UNUSED(uas);
-    Q_UNUSED(timestamp);
-
-    primarySpeed = speed;
-    didReceivePrimarySpeed = true;
-}
-
-void PrimaryFlightDisplay::updateGPSSpeed(UASInterface* uas, double speed, quint64 timestamp)
+void PrimaryFlightDisplay::updateSpeed(UASInterface* uas, double _groundSpeed, double _airSpeed, quint64 timestamp)
 {
     Q_UNUSED(uas);
     Q_UNUSED(timestamp);
 
-    groundspeed = speed;
-    if (!didReceivePrimarySpeed)
-        primarySpeed = speed;
+    groundSpeed = _groundSpeed;
+    airSpeed = _airSpeed;
 }
 
-void PrimaryFlightDisplay::updateClimbRate(UASInterface* uas, double climbRate, quint64 timestamp) {
+void PrimaryFlightDisplay::updateAltitude(UASInterface* uas, double _altitudeAMSL, double _altitudeRelative, double _climbRate, quint64 timestamp) {
     Q_UNUSED(uas);
     Q_UNUSED(timestamp);
-    verticalVelocity = climbRate;
-}
-
-void PrimaryFlightDisplay::updatePrimaryAltitude(UASInterface* uas, double altitude, quint64 timestamp) {
-    Q_UNUSED(uas);
-    Q_UNUSED(timestamp);
-    primaryAltitude = altitude;
-    didReceivePrimaryAltitude = true;
-}
-
-void PrimaryFlightDisplay::updateGPSAltitude(UASInterface* uas, double altitude, quint64 timestamp) {
-    Q_UNUSED(uas);
-    Q_UNUSED(timestamp);
-    GPSAltitude = altitude;
-    if (!didReceivePrimaryAltitude)
-        primaryAltitude = altitude;
+    altitudeAMSL = _altitudeAMSL;
+    altitudeRelative = _altitudeRelative;
+    climbRate = _climbRate;
 }
 
 void PrimaryFlightDisplay::updateNavigationControllerErrors(UASInterface* uas, double altitudeError, double speedError, double xtrackError) {
@@ -540,7 +469,7 @@ void PrimaryFlightDisplay::fillInstrumentOpagueBackground(QPainter& painter, QRe
 }
 
 qreal pitchAngleToTranslation(qreal viewHeight, float pitch) {
-    if (pitch == UNKNOWN_ATTITUDE)
+    if (isnan(pitch))
         return 0;
     return pitch * viewHeight / PITCHTRANSLATION;
 }
@@ -602,7 +531,7 @@ void PrimaryFlightDisplay::drawAIGlobalFeatures(
         QRectF paintArea) {
 
     float displayRoll = this->roll;
-    if(displayRoll == UNKNOWN_ATTITUDE)
+    if (isnan(displayRoll))
         displayRoll = 0;
 
     painter.resetTransform();
@@ -678,7 +607,7 @@ void PrimaryFlightDisplay::drawPitchScale(
         ) {
 
     float displayPitch = this->pitch;
-    if (displayPitch == UNKNOWN_ATTITUDE)
+    if (isnan(displayPitch))
         displayPitch = 0;
 
     // The area should be quadratic but if not width is the major size.
@@ -727,7 +656,7 @@ void PrimaryFlightDisplay::drawPitchScale(
             else if (displayDegrees<-90) displayDegrees = -180 - displayDegrees;
             if (SHOW_ZERO_ON_SCALES || degrees) {
                 QString s_number;
-                if (this->pitch == UNKNOWN_ATTITUDE)
+                if (isnan(this->pitch))
                     s_number.sprintf("-");
                 else
                     s_number.sprintf("%d", displayDegrees);
@@ -794,7 +723,7 @@ void PrimaryFlightDisplay::drawAIAttitudeScales(
         float intrusion
         ) {
     float displayRoll = this->roll;
-    if (displayRoll == UNKNOWN_ATTITUDE)
+    if (isnan(displayRoll))
         displayRoll = 0;
     // To save computations, we do these transformations once for both scales:
     painter.resetTransform();
@@ -809,7 +738,7 @@ void PrimaryFlightDisplay::drawAIAttitudeScales(
 
 void PrimaryFlightDisplay::drawAICompassDisk(QPainter& painter, QRectF area, float halfspan) {
     float displayHeading = this->heading;
-    if(displayHeading == UNKNOWN_ATTITUDE)
+    if (isnan(displayHeading))
         displayHeading = 0;
 
     float start = displayHeading - halfspan;
@@ -845,7 +774,7 @@ void PrimaryFlightDisplay::drawAICompassDisk(QPainter& painter, QRectF area, flo
         bool isMajor = displayTick % COMPASS_DISK_MAJORTICK == 0;
 
         // If heading unknown, still draw marks but no numbers.
-        if (this->heading != UNKNOWN_ATTITUDE &&
+        if (!isnan(this->heading) &&
                 (displayTick==30 || displayTick==60 ||
                 displayTick==120 || displayTick==150 ||
                 displayTick==210 || displayTick==240 ||
@@ -870,7 +799,7 @@ void PrimaryFlightDisplay::drawAICompassDisk(QPainter& painter, QRectF area, flo
                     drewArrow = true;
                 }
                 // If heading unknown, still draw marks but no N S E W.
-                if (this->heading != UNKNOWN_ATTITUDE && displayTick%90 == 0) {
+                if (!isnan(this->heading) && displayTick%90 == 0) {
                     // Also draw a label
                     QString name = compassWindNames[displayTick / 45];
                     painter.setPen(scalePen);
@@ -911,7 +840,7 @@ void PrimaryFlightDisplay::drawAICompassDisk(QPainter& painter, QRectF area, flo
 
     QString s_digitalCompass;
 
-    if (this->heading == UNKNOWN_ATTITUDE)
+    if (isnan(this->heading))
         s_digitalCompass.sprintf("---");
     else {
     /* final safeguard for really stupid systems */
@@ -931,7 +860,7 @@ void PrimaryFlightDisplay::drawAICompassDisk(QPainter& painter, QRectF area, flo
 //  navigationCrosstrackError = 500;
 
     // The CDI
-    if (shouldDisplayNavigationData() && navigationTargetBearing != UNKNOWN_ATTITUDE && !isinf(navigationCrosstrackError)) {
+    if (shouldDisplayNavigationData() && !isnan(navigationTargetBearing) && !isinf(navigationCrosstrackError)) {
         painter.resetTransform();
         painter.translate(area.center());
         // TODO : Sign might be wrong?
@@ -964,12 +893,12 @@ void PrimaryFlightDisplay::drawAICompassDisk(QPainter& painter, QRectF area, flo
 
 void PrimaryFlightDisplay::drawAltimeter(
         QPainter& painter,
-        QRectF area, // the area where to draw the tape.
-        float primaryAltitude,
-        float secondaryAltitude,
-        float vv
+        QRectF area
     ) {
 
+    float primaryAltitude = altitudeAMSL;
+    float secondaryAltitude = 0;
+
     painter.resetTransform();
     fillInstrumentBackground(painter, area);
 
@@ -986,7 +915,7 @@ void PrimaryFlightDisplay::drawAltimeter(
     float effectiveHalfHeight = h*0.45;
 
     // not yet implemented: Display of secondary altitude.
-    if (secondaryAltitude != UNKNOWN_ALTITUDE) {
+    if (!isnan(secondaryAltitude)) {
         effectiveHalfHeight-= secondaryAltitudeBoxHeight;
     }
 
@@ -998,7 +927,7 @@ void PrimaryFlightDisplay::drawAltimeter(
     float tickmarkRightMinor = tickmarkLeft+TAPE_GAUGES_TICKWIDTH_MINOR*w;
     float numbersLeft = 0.42*w;
     float markerTip = (tickmarkLeft*2+tickmarkRightMajor)/3;
-    float scaleCenterAltitude = primaryAltitude == UNKNOWN_ALTITUDE ? 0 : primaryAltitude;
+    float scaleCenterAltitude = isnan(primaryAltitude) ? 0 : primaryAltitude;
 
     // altitude scale
     float start = scaleCenterAltitude - ALTIMETER_LINEAR_SPAN/2;
@@ -1045,7 +974,7 @@ void PrimaryFlightDisplay::drawAltimeter(
     painter.setPen(pen);
 
     QString s_alt;
-    if(primaryAltitude == UNKNOWN_ALTITUDE)
+    if (isnan(primaryAltitude))
         s_alt.sprintf("---");
     else
         s_alt.sprintf("%3.0f", primaryAltitude);
@@ -1054,32 +983,33 @@ void PrimaryFlightDisplay::drawAltimeter(
     drawTextCenter(painter, s_alt, mediumTextSize, xCenter, 0);
 
     // draw simple in-tape VVI.
-    if (vv != UNKNOWN_ALTITUDE) {
-    float vvPixHeight = -vv/ALTIMETER_VVI_SPAN * effectiveHalfHeight;
-    if (abs (vvPixHeight)<markerHalfHeight) return; // hidden behind marker.
+    if (!isnan(climbRate)) {
+        float vvPixHeight = -climbRate/ALTIMETER_VVI_SPAN * effectiveHalfHeight;
+        if (abs (vvPixHeight) < markerHalfHeight)
+            return; // hidden behind marker.
 
-    float vvSign = vvPixHeight>0 ? 1 : -1; // reverse y sign
+        float vvSign = vvPixHeight>0 ? 1 : -1; // reverse y sign
 
-    // QRectF vvRect(rightEdge - w*ALTIMETER_VVI_WIDTH, markerHalfHeight*vvSign, w*ALTIMETER_VVI_WIDTH, abs(vvPixHeight)*vvSign);
-    QPointF vvArrowBegin(rightEdge - w*ALTIMETER_VVI_WIDTH/2, markerHalfHeight*vvSign);
-    QPointF vvArrowEnd(rightEdge - w*ALTIMETER_VVI_WIDTH/2, vvPixHeight);
-    painter.drawLine(vvArrowBegin, vvArrowEnd);
+        // QRectF vvRect(rightEdge - w*ALTIMETER_VVI_WIDTH, markerHalfHeight*vvSign, w*ALTIMETER_VVI_WIDTH, abs(vvPixHeight)*vvSign);
+        QPointF vvArrowBegin(rightEdge - w*ALTIMETER_VVI_WIDTH/2, markerHalfHeight*vvSign);
+        QPointF vvArrowEnd(rightEdge - w*ALTIMETER_VVI_WIDTH/2, vvPixHeight);
+        painter.drawLine(vvArrowBegin, vvArrowEnd);
 
-    // Yeah this is a repitition of above code but we are goigd to trash it all anyway, so no fix.
-    float vvArowHeadSize = abs(vvPixHeight - markerHalfHeight*vvSign);
-    if (vvArowHeadSize > w*ALTIMETER_VVI_WIDTH/3) vvArowHeadSize = w*ALTIMETER_VVI_WIDTH/3;
+        // Yeah this is a repitition of above code but we are goigd to trash it all anyway, so no fix.
+        float vvArowHeadSize = abs(vvPixHeight - markerHalfHeight*vvSign);
+        if (vvArowHeadSize > w*ALTIMETER_VVI_WIDTH/3) vvArowHeadSize = w*ALTIMETER_VVI_WIDTH/3;
 
-    float xcenter = rightEdge-w*ALTIMETER_VVI_WIDTH/2;
+        float xcenter = rightEdge-w*ALTIMETER_VVI_WIDTH/2;
 
-    QPointF vvArrowHead(xcenter+vvArowHeadSize, vvPixHeight - vvSign *vvArowHeadSize);
-    painter.drawLine(vvArrowHead, vvArrowEnd);
+        QPointF vvArrowHead(xcenter+vvArowHeadSize, vvPixHeight - vvSign *vvArowHeadSize);
+        painter.drawLine(vvArrowHead, vvArrowEnd);
 
-    vvArrowHead = QPointF(xcenter-vvArowHeadSize, vvPixHeight - vvSign * vvArowHeadSize);
-    painter.drawLine(vvArrowHead, vvArrowEnd);
+        vvArrowHead = QPointF(xcenter-vvArowHeadSize, vvPixHeight - vvSign * vvArowHeadSize);
+        painter.drawLine(vvArrowHead, vvArrowEnd);
     }
 
     // print secondary altitude
-    if (secondaryAltitude != UNKNOWN_ALTITUDE) {
+    if (!isnan(secondaryAltitude)) {
         QRectF saBox(area.x(), area.y()-secondaryAltitudeBoxHeight, w, secondaryAltitudeBoxHeight);
         painter.resetTransform();
         painter.translate(saBox.center());
@@ -1093,9 +1023,7 @@ void PrimaryFlightDisplay::drawAltimeter(
 
 void PrimaryFlightDisplay::drawVelocityMeter(
         QPainter& painter,
-        QRectF area,
-        float speed,
-        float secondarySpeed
+        QRectF area
         ) {
 
     painter.resetTransform();
@@ -1117,8 +1045,8 @@ void PrimaryFlightDisplay::drawVelocityMeter(
 
     // Select between air and ground speed:
 
-    float centerScaleSpeed =
-            speed == UNKNOWN_SPEED ? 0 : speed;
+    float speed = (isAirplane() && isnan(airSpeed)) ? airSpeed : groundSpeed;
+    float centerScaleSpeed = isnan(speed) ? 0 : speed;
 
     float start = centerScaleSpeed - AIRSPEED_LINEAR_SPAN/2;
     float end = centerScaleSpeed + AIRSPEED_LINEAR_SPAN/2;
@@ -1166,7 +1094,7 @@ void PrimaryFlightDisplay::drawVelocityMeter(
     pen.setColor(Qt::white);
     painter.setPen(pen);
     QString s_alt;
-    if (speed == UNKNOWN_SPEED)
+    if (isnan(speed))
         s_alt.sprintf("---");
     else
         s_alt.sprintf("%3.1f", speed);
@@ -1505,9 +1433,9 @@ void PrimaryFlightDisplay::doPaint() {
 
     painter.setClipping(hadClip);
 
-    drawAltimeter(painter, altimeterArea, primaryAltitude, GPSAltitude, verticalVelocity);
+    drawAltimeter(painter, altimeterArea);
 
-    drawVelocityMeter(painter, velocityMeterArea, primarySpeed, groundspeed);
+    drawVelocityMeter(painter, velocityMeterArea);
 
     /*
     drawSensorsStatsPanel(painter, sensorsStatsArea);

src/ui/PrimaryFlightDisplay.h

@@ -18,11 +18,8 @@ public slots:
     /** @brief Attitude from one specific component / redundant autopilot */
     void updateAttitude(UASInterface* uas, int component, double roll, double pitch, double yaw, quint64 timestamp);
 
-    void updatePrimarySpeed(UASInterface* uas, double speed, quint64 timstamp);
-    void updateGPSSpeed(UASInterface* uas, double speed, quint64 timstamp);
-    void updateClimbRate(UASInterface* uas, double altitude, quint64 timestamp);
-    void updatePrimaryAltitude(UASInterface* uas, double altitude, quint64 timestamp);
-    void updateGPSAltitude(UASInterface* uas, double altitude, quint64 timestamp);
+    void updateSpeed(UASInterface* uas, double _groundSpeed, double _airSpeed, quint64 timestamp);
+    void updateAltitude(UASInterface* uas, double _altitudeAMSL, double _altitudeRelative, double _climbRate, quint64 timestamp);
     void updateNavigationControllerErrors(UASInterface* uas, double altitudeError, double speedError, double xtrackError);
 
     /** @brief Set the currently monitored UAS */
@@ -102,8 +99,8 @@ private:
     void drawAICompassDisk(QPainter& painter, QRectF area, float halfspan);
     void drawSeparateCompassDisk(QPainter& painter, QRectF area);
 
-    void drawAltimeter(QPainter& painter, QRectF area, float altitude, float secondaryAltitude, float vv);
-    void drawVelocityMeter(QPainter& painter, QRectF area, float speed, float secondarySpeed);
+    void drawAltimeter(QPainter& painter, QRectF area);
+    void drawVelocityMeter(QPainter& painter, QRectF area);
     void fillInstrumentBackground(QPainter& painter, QRectF edge);
     void fillInstrumentOpagueBackground(QPainter& painter, QRectF edge);
     void drawInstrumentBackground(QPainter& painter, QRectF edge);
@@ -125,24 +122,23 @@ private:
     SpeedMode speedMode;
     */
 
-    bool didReceivePrimaryAltitude;
-    bool didReceivePrimarySpeed;
+    bool didReceiveSpeed;
 
     float roll;
     float pitch;
     float heading;
 
-    float primaryAltitude;
-    float GPSAltitude;
+    float altitudeAMSL;
+    float altitudeRelative;
 
     // APM: GPS and baro mix above home (GPS) altitude. This value comes from the GLOBAL_POSITION_INT message.
     // Do !!!NOT!!! ever do altitude calculations at the ground station. There are enough pitfalls already.
     // If the MP "set home altitude" button is migrated to here, it must set the UAS home altitude, not a GS-local one.
     float aboveHomeAltitude;
 
-    float primarySpeed;
-    float groundspeed;
-    float verticalVelocity;
+    float groundSpeed;
+    float airSpeed;
+    float climbRate;
 
     float navigationAltitudeError;
     float navigationSpeedError;

src/ui/WaypointList.cc

@@ -306,7 +306,7 @@ void WaypointList::addEditable(bool onCurrentPosition)
                     } else {
                         updateStatusLabel(tr("Added default GLOBAL (Relative alt.) waypoint."));
                     }
-                    wp = new Waypoint(0, uas->getLatitude(), uas->getLongitude(), uas->getAltitude(), 0, WPM->getAcceptanceRadiusRecommendation(), 0, 0,true, true, (MAV_FRAME)WPM->getFrameRecommendation(), MAV_CMD_NAV_WAYPOINT);
+                    wp = new Waypoint(0, uas->getLatitude(), uas->getLongitude(), uas->getAltitudeAMSL(), 0, WPM->getAcceptanceRadiusRecommendation(), 0, 0,true, true, (MAV_FRAME)WPM->getFrameRecommendation(), MAV_CMD_NAV_WAYPOINT);
                     WPM->addWaypointEditable(wp);
 
                 } else {

src/ui/map/QGCMapWidget.cc

@@ -462,7 +462,7 @@ void QGCMapWidget::updateGlobalPosition()
 
         // Set new lat/lon position of UAV icon
         internals::PointLatLng pos_lat_lon = internals::PointLatLng(system->getLatitude(), system->getLongitude());
-        uav->SetUAVPos(pos_lat_lon, system->getAltitude());
+        uav->SetUAVPos(pos_lat_lon, system->getAltitudeAMSL());
         // Follow status
         if (followUAVEnabled && system->getUASID() == followUAVID) SetCurrentPosition(pos_lat_lon);
         // Convert from radians to degrees and apply
@@ -490,7 +490,7 @@ void QGCMapWidget::updateLocalPosition()
 
         // Set new lat/lon position of UAV icon
         internals::PointLatLng pos_lat_lon = internals::PointLatLng(system->getLatitude(), system->getLongitude());
-        uav->SetUAVPos(pos_lat_lon, system->getAltitude());
+        uav->SetUAVPos(pos_lat_lon, system->getAltitudeAMSL());
         // Follow status
         if (followUAVEnabled && system->getUASID() == followUAVID) SetCurrentPosition(pos_lat_lon);
         // Convert from radians to degrees and apply

src/ui/map3D/Pixhawk3DWidget.cc

@@ -682,7 +682,7 @@ Pixhawk3DWidget::selectTargetHeading(void)
 
     if (mGlobalViewParams->frame() == MAV_FRAME_GLOBAL)
     {
-        double altitude = mActiveUAS->getAltitude();
+        double altitude = mActiveUAS->getAltitudeAMSL();
 
         QPointF cursorWorldCoords =
             m3DWidget->worldCursorPosition(m3DWidget->mouseCursorCoords(), altitude);
@@ -722,7 +722,7 @@ Pixhawk3DWidget::selectTarget(void)
 
     if (mGlobalViewParams->frame() == MAV_FRAME_GLOBAL)
     {
-        double altitude = mActiveUAS->getAltitude();
+        double altitude = mActiveUAS->getAltitudeAMSL();
 
         QPointF cursorWorldCoords =
             m3DWidget->worldCursorPosition(mCachedMousePos, altitude);
@@ -789,7 +789,7 @@ Pixhawk3DWidget::insertWaypoint(void)
     {
         double latitude = mActiveUAS->getLatitude();
         double longitude = mActiveUAS->getLongitude();
-        double altitude = mActiveUAS->getAltitude();
+        double altitude = mActiveUAS->getAltitudeAMSL();
         double x, y;
         QString utmZone;
         Imagery::LLtoUTM(latitude, longitude, x, y, utmZone);
@@ -845,7 +845,7 @@ Pixhawk3DWidget::moveWaypointPosition(void)
     {
         double latitude = mActiveUAS->getLatitude();
         double longitude = mActiveUAS->getLongitude();
-        double altitude = mActiveUAS->getAltitude();
+        double altitude = mActiveUAS->getAltitudeAMSL();
         double x, y;
         QString utmZone;
         Imagery::LLtoUTM(latitude, longitude, x, y, utmZone);
@@ -1700,7 +1700,7 @@ Pixhawk3DWidget::getPose(UASInterface* uas,
     {
         double latitude = uas->getLatitude();
         double longitude = uas->getLongitude();
-        double altitude = uas->getAltitude();
+        double altitude = uas->getAltitudeAMSL();
 
         Imagery::LLtoUTM(latitude, longitude, x, y, utmZone);
         z = -altitude;
@@ -1742,7 +1742,7 @@ Pixhawk3DWidget::getPosition(UASInterface* uas,
     {
         double latitude = uas->getLatitude();
         double longitude = uas->getLongitude();
-        double altitude = uas->getAltitude();
+        double altitude = uas->getAltitudeAMSL();
 
         Imagery::LLtoUTM(latitude, longitude, x, y, utmZone);
         z = -altitude;

src/ui/map3D/QGCGoogleEarthView.cc

@@ -632,7 +632,7 @@ void QGCGoogleEarthView::updateState()
             uasId = currMav->getUASID();
             lat = currMav->getLatitude();
             lon = currMav->getLongitude();
-            alt = currMav->getAltitude();
+            alt = currMav->getAltitudeAMSL();
             roll = currMav->getRoll();
             pitch = currMav->getPitch();
             yaw = currMav->getYaw();

src/ui/map3D/WaypointGroupNode.cc

@@ -65,7 +65,7 @@ WaypointGroupNode::update(UASInterface* uas, MAV_FRAME frame)
     {
         double latitude = uas->getLatitude();
         double longitude = uas->getLongitude();
-        double altitude = uas->getAltitude();
+        double altitude = uas->getAltitudeAMSL();
 
         QString utmZone;
         Imagery::LLtoUTM(latitude, longitude, robotX, robotY, utmZone);
@@ -75,7 +75,7 @@ WaypointGroupNode::update(UASInterface* uas, MAV_FRAME frame)
     {
         double latitude = uas->getLatitude();
         double longitude = uas->getLongitude();
-        double altitude = uas->getAltitude() + UASManager::instance()->getHomeAltitude();
+        double altitude = uas->getAltitudeRelative();
 
         QString utmZone;
         Imagery::LLtoUTM(latitude, longitude, robotX, robotY, utmZone);

src/ui/uas/UASListWidget.cc

@@ -165,7 +165,6 @@ void UASListWidget::addUAS(UASInterface* uas)
             uWidget = NULL;
         }
     }
-
     if (!uasViews.contains(uas))
     {
         // Only display the UAS in a single link.

src/ui/uas/UASQuickView.cc

@@ -29,11 +29,10 @@ UASQuickView::UASQuickView(QWidget *parent) : QWidget(parent)
     //If we don't have any predefined settings, set some defaults.
     if (uasPropertyValueMap.size() == 0)
     {
-        valueEnabled("altitude");
+        valueEnabled("altitudeAMSL");
+        valueEnabled("altitudeRelative");
         valueEnabled("groundSpeed");
         valueEnabled("distToWP");
-        valueEnabled("yaw");
-        valueEnabled("roll");
     }
 
     QAction *action = new QAction("Add/Remove Items",this);

src/ui/uas/UASQuickViewTextItem.cc

@@ -10,7 +10,6 @@ UASQuickViewTextItem::UASQuickViewTextItem(QWidget *parent) : UASQuickViewItem(p
 
     // Create the title label. Scale the font based on available size.
     titleLabel = new QLabel(this);
-// <<<<<<< HEAD
      titleLabel->setAlignment(Qt::AlignHCenter);
      titleLabel->setSizePolicy(QSizePolicy::MinimumExpanding,QSizePolicy::Minimum);
      titleLabel->setObjectName(QString::fromUtf8("title"));
@@ -25,18 +24,6 @@ UASQuickViewTextItem::UASQuickViewTextItem(QWidget *parent) : UASQuickViewItem(p
      valueLabel->setSizePolicy(QSizePolicy::MinimumExpanding,QSizePolicy::Minimum);
      valueLabel->setObjectName(QString::fromUtf8("value"));
      valueLabel->setText("0.00");
-// =======
-//    titleLabel->setSizePolicy(QSizePolicy::Ignored,QSizePolicy::Ignored);
-//    titleLabel->setAlignment(Qt::AlignHCenter | Qt::AlignBottom);
-//    this->layout()->addWidget(titleLabel);
-//    valueLabel = new QLabel(this);
-//    valueLabel->setSizePolicy(QSizePolicy::Ignored,QSizePolicy::Ignored);
-//    valueLabel->setAlignment(Qt::AlignHCenter | Qt::AlignTop);
-//    valueLabel->setText("0.00");
-//    this->layout()->addWidget(valueLabel);
-//    //spacerItem = new QSpacerItem(20,40,QSizePolicy::Minimum,QSizePolicy::Ignored);
-//    //layout->addSpacerItem(spacerItem);
-// >>>>>>> 34eaf1fb422146f5df3b01fad4d756343b3127c9
     QFont valuefont = valueLabel->font();
     valuefont.setPixelSize(this->height() / 2.0);
     valueLabel->setFont(valuefont);