Merge pull request #2500 from dogmaphobic/moreGPSData

Using HDOP for GPS signal strength indicator.

src/Vehicle/Vehicle.cc

@@ -90,6 +90,9 @@ Vehicle::Vehicle(LinkInterface*             link,
     , _batteryConsumed(-1.0)
     , _currentHeartbeatTimeout(0)
     , _satelliteCount(-1)
+    , _satRawHDOP(1e10f)
+    , _satRawVDOP(1e10f)
+    , _satRawCOG(0.0)
     , _satelliteLock(0)
     , _updateCount(0)
     , _rcRSSI(0)
@@ -137,9 +140,16 @@ Vehicle::Vehicle(LinkInterface*             link,
     emit heartbeatTimeoutChanged();
 
     _mav = uas();
-    // Reset satellite count (no GPS)
+    // Reset satellite data (no GPS)
     _satelliteCount = -1;
+    _satRawHDOP     = 1e10f;
+    _satRawVDOP     = 1e10f;
+    _satRawCOG      = 0.0;
+    emit satRawHDOPChanged();
+    emit satRawVDOPChanged();
+    emit satRawCOGChanged();
     emit satelliteCountChanged();
+
     // Reset connection lost (if any)
     _currentHeartbeatTimeout = 0;
     emit heartbeatTimeoutChanged();
@@ -161,7 +171,10 @@ Vehicle::Vehicle(LinkInterface*             link,
     UAS* pUas = dynamic_cast<UAS*>(_mav);
     if(pUas) {
         _setSatelliteCount(pUas->getSatelliteCount(), QString(""));
-        connect(pUas, &UAS::satelliteCountChanged, this, &Vehicle::_setSatelliteCount);
+        connect(pUas, &UAS::satelliteCountChanged,  this, &Vehicle::_setSatelliteCount);
+        connect(pUas, &UAS::satRawHDOPChanged,      this, &Vehicle::_setSatRawHDOP);
+        connect(pUas, &UAS::satRawVDOPChanged,      this, &Vehicle::_setSatRawVDOP);
+        connect(pUas, &UAS::satRawCOGChanged,       this, &Vehicle::_setSatRawCOG);
     }
 
     _loadSettings();
@@ -754,6 +767,30 @@ void Vehicle::_setSatelliteCount(double val, QString)
     }
 }
 
+void Vehicle::_setSatRawHDOP(double val)
+{
+    if(_satRawHDOP != val) {
+        _satRawHDOP = val;
+        emit satRawHDOPChanged();
+    }
+}
+
+void Vehicle::_setSatRawVDOP(double val)
+{
+    if(_satRawVDOP != val) {
+        _satRawVDOP = val;
+        emit satRawVDOPChanged();
+    }
+}
+
+void Vehicle::_setSatRawCOG(double val)
+{
+    if(_satRawCOG != val) {
+        _satRawCOG = val;
+        emit satRawCOGChanged();
+    }
+}
+
 void Vehicle::_setSatLoc(UASInterface*, int fix)
 {
     // fix 0: lost, 1: at least one satellite, but no GPS fix, 2: 2D lock, 3: 3D lock

src/Vehicle/Vehicle.h

@@ -93,6 +93,9 @@ public:
     Q_PROPERTY(double               batteryPercent          READ batteryPercent                         NOTIFY batteryPercentChanged)
     Q_PROPERTY(double               batteryConsumed         READ batteryConsumed                        NOTIFY batteryConsumedChanged)
     Q_PROPERTY(int                  satelliteCount          READ satelliteCount                         NOTIFY satelliteCountChanged)
+    Q_PROPERTY(double               satRawHDOP              READ satRawHDOP                             NOTIFY satRawHDOPChanged)
+    Q_PROPERTY(double               satRawVDOP              READ satRawVDOP                             NOTIFY satRawVDOPChanged)
+    Q_PROPERTY(double               satRawCOG               READ satRawCOG                              NOTIFY satRawCOGChanged)
     Q_PROPERTY(QString              currentState            READ currentState                           NOTIFY currentStateChanged)
     Q_PROPERTY(int                  satelliteLock           READ satelliteLock                          NOTIFY satelliteLockChanged)
     Q_PROPERTY(unsigned int         heartbeatTimeout        READ heartbeatTimeout                       NOTIFY heartbeatTimeoutChanged)
@@ -250,6 +253,9 @@ public:
     float           longitude           () { return _coordinate.longitude(); }
     bool            mavPresent          () { return _mav != NULL; }
     int             satelliteCount      () { return _satelliteCount; }
+    double          satRawHDOP          () { return _satRawHDOP; }
+    double          satRawVDOP          () { return _satRawVDOP; }
+    double          satRawCOG           () { return _satRawCOG; }
     double          batteryVoltage      () { return _batteryVoltage; }
     double          batteryPercent      () { return _batteryPercent; }
     double          batteryConsumed     () { return _batteryConsumed; }
@@ -309,6 +315,9 @@ signals:
     void heartbeatTimeoutChanged();
     void currentConfigChanged   ();
     void satelliteCountChanged  ();
+    void satRawHDOPChanged      ();
+    void satRawVDOPChanged      ();
+    void satRawCOGChanged       ();
     void currentStateChanged    ();
     void satelliteLockChanged   ();
     void flowImageIndexChanged  ();
@@ -352,6 +361,9 @@ private slots:
     void _updateState                       (UASInterface* system, QString name, QString description);
     void _heartbeatTimeout                  (bool timeout, unsigned int ms);
     void _setSatelliteCount                 (double val, QString name);
+    void _setSatRawHDOP                     (double val);
+    void _setSatRawVDOP                     (double val);
+    void _setSatRawCOG                      (double val);
     void _setSatLoc                         (UASInterface* uas, int fix);
     /** @brief A new camera image has arrived */
     void _imageReady                        (UASInterface* uas);
@@ -425,6 +437,9 @@ private:
     QString         _currentState;
     unsigned int    _currentHeartbeatTimeout;
     int             _satelliteCount;
+    double          _satRawHDOP;
+    double          _satRawVDOP;
+    double          _satRawCOG;
     int             _satelliteLock;
     int             _updateCount;
     QString         _formatedMessage;

src/uas/UAS.cc

@@ -96,6 +96,10 @@ UAS::UAS(MAVLinkProtocol* protocol, Vehicle* vehicle, FirmwarePluginManager * fi
     altitudeWGS84(0.0),
     altitudeRelative(0.0),
 
+    satRawHDOP(1e10f),
+    satRawVDOP(1e10f),
+    satRawCOG(0.0),
+
     globalEstimatorActive(false),
 
     latitude_gps(0.0),
@@ -715,9 +719,9 @@ void UAS::receiveMessage(mavlink_message_t message)
                 positionLock = true;
                 isGlobalPositionKnown = true;
 
-                latitude_gps = pos.lat/(double)1E7;
+                latitude_gps  = pos.lat/(double)1E7;
                 longitude_gps = pos.lon/(double)1E7;
-                altitude_gps = pos.alt/1000.0;
+                altitude_gps  = pos.alt/1000.0;
 
                 // If no GLOBAL_POSITION_INT messages ever received, use these raw GPS values instead.
                 if (!globalEstimatorActive) {
@@ -738,6 +742,27 @@ void UAS::receiveMessage(mavlink_message_t message)
                 }
             }
 
+            double dtmp;
+            //-- Raw GPS data
+            dtmp = pos.eph == 0xFFFF ? 1e10f : pos.eph / 100.0;
+            if(dtmp != satRawHDOP)
+            {
+                satRawHDOP = dtmp;
+                emit satRawHDOPChanged(satRawHDOP);
+            }
+            dtmp = pos.epv == 0xFFFF ? 1e10f : pos.epv / 100.0;
+            if(dtmp != satRawVDOP)
+            {
+                satRawVDOP = dtmp;
+                emit satRawVDOPChanged(satRawVDOP);
+            }
+            dtmp = pos.cog == 0xFFFF ? 0.0 : pos.cog / 100.0;
+            if(dtmp != satRawCOG)
+            {
+                satRawCOG = dtmp;
+                emit satRawCOGChanged(satRawCOG);
+            }
+
             // Emit this signal after the above signals. This way a trigger on gps lock signal which then asks for vehicle position
             // gets a good position.
             emit localizationChanged(this, loc_type);

src/uas/UAS.h

@@ -81,24 +81,27 @@ public:
     /** @brief Add one measurement and get low-passed voltage */
     float filterVoltage(float value);
 
-    Q_PROPERTY(double latitude READ getLatitude WRITE setLatitude NOTIFY latitudeChanged)
-    Q_PROPERTY(double longitude READ getLongitude WRITE setLongitude NOTIFY longitudeChanged)
-    Q_PROPERTY(double satelliteCount READ getSatelliteCount WRITE setSatelliteCount NOTIFY satelliteCountChanged)
-    Q_PROPERTY(bool isGlobalPositionKnown READ globalPositionKnown)
-    Q_PROPERTY(double roll READ getRoll WRITE setRoll NOTIFY rollChanged)
-    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 altitudeAMSLFT READ getAltitudeAMSLFT NOTIFY altitudeAMSLFTChanged)
-    Q_PROPERTY(double altitudeWGS84 READ getAltitudeWGS84 WRITE setAltitudeWGS84 NOTIFY altitudeWGS84Changed)
-    Q_PROPERTY(double altitudeRelative READ getAltitudeRelative WRITE setAltitudeRelative NOTIFY altitudeRelativeChanged)
+    Q_PROPERTY(double   latitude                READ getLatitude            WRITE setLatitude           NOTIFY latitudeChanged)
+    Q_PROPERTY(double   longitude               READ getLongitude           WRITE setLongitude          NOTIFY longitudeChanged)
+    Q_PROPERTY(double   satelliteCount          READ getSatelliteCount      WRITE setSatelliteCount     NOTIFY satelliteCountChanged)
+    Q_PROPERTY(bool     isGlobalPositionKnown   READ globalPositionKnown)
+    Q_PROPERTY(double   roll                    READ getRoll                WRITE setRoll               NOTIFY rollChanged)
+    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   altitudeAMSLFT          READ getAltitudeAMSLFT                                  NOTIFY altitudeAMSLFTChanged)
+    Q_PROPERTY(double   altitudeWGS84           READ getAltitudeWGS84       WRITE setAltitudeWGS84      NOTIFY altitudeWGS84Changed)
+    Q_PROPERTY(double   altitudeRelative        READ getAltitudeRelative    WRITE setAltitudeRelative   NOTIFY altitudeRelativeChanged)
+    Q_PROPERTY(double   satRawHDOP              READ getSatRawHDOP                                      NOTIFY satRawHDOPChanged)
+    Q_PROPERTY(double   satRawVDOP              READ getSatRawVDOP                                      NOTIFY satRawVDOPChanged)
+    Q_PROPERTY(double   satRawCOG               READ getSatRawCOG                                       NOTIFY satRawCOGChanged)
 
     void clearVehicle(void) { _vehicle = NULL; }
-    
+
     void setGroundSpeed(double val)
     {
         groundSpeed = val;
@@ -225,6 +228,21 @@ public:
         return altitudeRelative;
     }
 
+    double getSatRawHDOP() const
+    {
+        return satRawHDOP;
+    }
+
+    double getSatRawVDOP() const
+    {
+        return satRawVDOP;
+    }
+
+    double getSatRawCOG() const
+    {
+        return satRawCOG;
+    }
+
     void setSatelliteCount(double val)
     {
         satelliteCount = val;
@@ -361,7 +379,7 @@ protected: //COMMENTS FOR TEST UNIT
     /// LINK ID AND STATUS
     int uasId;                    ///< Unique system ID
     QMap<int, QString> components;///< IDs and names of all detected onboard components
-    
+
     QList<int> unknownPackets;    ///< Packet IDs which are unknown and have been received
     MAVLinkProtocol* mavlink;     ///< Reference to the MAVLink instance
     float receiveDropRate;        ///< Percentage of packets that were dropped on the MAV's receiving link (from GCS and other MAVs)
@@ -415,10 +433,14 @@ protected: //COMMENTS FOR TEST UNIT
     double latitude;            ///< Global latitude as estimated by position estimator
     double longitude;           ///< Global longitude as estimated by position estimator
     double altitudeAMSL;        ///< Global altitude as estimated by position estimator, AMSL
-    double altitudeAMSLFT;        ///< Global altitude as estimated by position estimator, AMSL
-    double altitudeWGS84;        ///< Global altitude as estimated by position estimator, WGS84
+    double altitudeAMSLFT;      ///< Global altitude as estimated by position estimator, AMSL
+    double altitudeWGS84;       ///< Global altitude as estimated by position estimator, WGS84
     double altitudeRelative;    ///< Altitude above home as estimated by position estimator
 
+    double satRawHDOP;
+    double satRawVDOP;
+    double satRawCOG;
+
     double satelliteCount;      ///< Number of satellites visible to raw GPS
     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
@@ -604,6 +626,11 @@ signals:
     void altitudeAMSLFTChanged(double val,QString name);
     void altitudeWGS84Changed(double val,QString name);
     void altitudeRelativeChanged(double val,QString name);
+
+    void satRawHDOPChanged  (double value);
+    void satRawVDOPChanged  (double value);
+    void satRawCOGChanged   (double value);
+
     void rollChanged(double val,QString name);
     void pitchChanged(double val,QString name);
     void yawChanged(double val,QString name);
@@ -637,7 +664,7 @@ protected:
 
 private:
     void _say(const QString& text, int severity = 6);
-    
+
 private:
     Vehicle*                _vehicle;
     FirmwarePluginManager*  _firmwarePluginManager;

src/ui/toolbar/MainToolBar.qml

@@ -254,6 +254,30 @@ Rectangle {
                         text:   getGpsLockStatus()
                         color:  colorWhite
                     }
+                    QGCLabel {
+                        text:   "HDOP:"
+                        color:  colorWhite
+                    }
+                    QGCLabel {
+                        text:   activeVehicle ? (activeVehicle.satRawHDOP.toFixed(0)) : "N/A"
+                        color:  colorWhite
+                    }
+                    QGCLabel {
+                        text:   "VDOP:"
+                        color:  colorWhite
+                    }
+                    QGCLabel {
+                        text:   activeVehicle ? (activeVehicle.satRawVDOP.toFixed(0)) : "N/A"
+                        color:  colorWhite
+                    }
+                    QGCLabel {
+                        text:   "Course Over Ground:"
+                        color:  colorWhite
+                    }
+                    QGCLabel {
+                        text:   activeVehicle ? (activeVehicle.satRawCOG).toFixed(2) : "N/A"
+                        color:  colorWhite
+                    }
                 }
             }
             Component.onCompleted: {

src/ui/toolbar/MainToolBarIndicators.qml

@@ -34,18 +34,16 @@ import QGroundControl.ScreenTools   1.0
 Row {
     spacing:  tbSpacing * 2
 
-    function getSatStrength(count) {
-        if (count < 1)
-            return 0
-        if (count < 4)
-            return 20
-        if (count < 6)
-            return 40
-        if (count < 8)
-            return 60
-        if (count < 10)
-            return 80
-        return 100
+    function getSatStrength(hdop) {
+        if (hdop < 3)
+            return 100
+        if (hdop < 6)
+            return 75
+        if (hdop < 11)
+            return 50
+        if (hdop < 21)
+            return 25
+        return 0
     }
 
     function getMessageColor() {
@@ -183,7 +181,7 @@ Row {
             }
             SignalStrength {
                 size:           mainWindow.tbCellHeight * 0.5
-                percent:        activeVehicle ? getSatStrength(activeVehicle.satelliteCount) : ""
+                percent:        activeVehicle ? getSatStrength(activeVehicle.satRawHDOP) : ""
                 anchors.verticalCenter: parent.verticalCenter
             }
         }