From 03ff0d1f3a698156495a59e186fcc2bc9822526a Mon Sep 17 00:00:00 2001 From: dogmaphobic Date: Tue, 29 Dec 2015 14:40:20 -0500 Subject: [PATCH] Using HDOP for GPS signal strength indicator. --- src/Vehicle/Vehicle.cc | 41 ++++++++++++++- src/Vehicle/Vehicle.h | 15 ++++++ src/uas/UAS.cc | 29 +++++++++- src/uas/UAS.h | 67 +++++++++++++++++------- src/ui/toolbar/MainToolBar.qml | 24 +++++++++ src/ui/toolbar/MainToolBarIndicators.qml | 24 ++++----- 6 files changed, 163 insertions(+), 37 deletions(-) diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index a973cc9d1..9aa7a1101 100644 --- a/src/Vehicle/Vehicle.cc +++ b/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(_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 diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 12633e93b..b5425b15c 100644 --- a/src/Vehicle/Vehicle.h +++ b/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; diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 7f8c630e5..32c69048f 100644 --- a/src/uas/UAS.cc +++ b/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); diff --git a/src/uas/UAS.h b/src/uas/UAS.h index e33f44bff..80b35954e 100644 --- a/src/uas/UAS.h +++ b/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 components;///< IDs and names of all detected onboard components - + QList 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; diff --git a/src/ui/toolbar/MainToolBar.qml b/src/ui/toolbar/MainToolBar.qml index ab6589567..3bd098dd7 100644 --- a/src/ui/toolbar/MainToolBar.qml +++ b/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: { diff --git a/src/ui/toolbar/MainToolBarIndicators.qml b/src/ui/toolbar/MainToolBarIndicators.qml index a3a23869a..93370d135 100644 --- a/src/ui/toolbar/MainToolBarIndicators.qml +++ b/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 } } -- 2.22.0