Merge pull request #6944 from DonLakeFlyer/DistanceToGCS

Add Distance to GCS to Vehicle FactGroup

ChangeLog.md

@@ -8,6 +8,7 @@ Note: This file only contains high level features or important fixes.
 * Add support for specifying fixed RTK based station location in Settings/General.
 * Added Airmap integration to QGC
 * Add ESTIMATOR_STATUS values to new estimatorStatus Vehicle FactGroup. These are now available to display in instrument panel.
+* Make Distance to GCS to available for display from instrument panel.
 
 ## 3.4
 

src/FlightMap/FlightMap.qml

@@ -34,7 +34,7 @@ Map {
 
     property string mapName:                        'defaultMap'
     property bool   isSatelliteMap:                 activeMapType.name.indexOf("Satellite") > -1 || activeMapType.name.indexOf("Hybrid") > -1
-    property var    gcsPosition:                    QtPositioning.coordinate()
+    property var    gcsPosition:                    QGroundControl.qgcPositionManger.gcsPosition
     property bool   userPanned:                     false   ///< true: the user has manually panned the map
     property bool   allowGCSLocationCenter:         false   ///< true: map will center/zoom to gcs location one time
     property bool   allowVehicleLocationCenter:     false   ///< true: map will center/zoom to vehicle location one time
@@ -80,19 +80,12 @@ Map {
 
     ExclusiveGroup { id: mapTypeGroup }
 
-    // Update ground station position
+    // Center map to gcs location
-    Connections {
+    onGcsPositionChanged: {
-        target: QGroundControl.qgcPositionManger
+        if (gcsPosition.isValid && allowGCSLocationCenter && !firstGCSPositionReceived && !firstVehiclePositionReceived) {
-
+            firstGCSPositionReceived = true
-        onLastPositionUpdated: {
+            center = gcsPosition
-            if (valid && lastPosition.latitude && Math.abs(lastPosition.latitude)  > 0.001 && lastPosition.longitude && Math.abs(lastPosition.longitude)  > 0.001) {
+            zoomLevel = QGroundControl.flightMapInitialZoom
-                gcsPosition = QtPositioning.coordinate(lastPosition.latitude,lastPosition.longitude)
-                if (!firstGCSPositionReceived && !firstVehiclePositionReceived && allowGCSLocationCenter) {
-                    firstGCSPositionReceived = true
-                    center = gcsPosition
-                    zoomLevel = QGroundControl.flightMapInitialZoom
-                }
-            }
         }
     }
 

src/PositionManager/PositionManager.cpp

@@ -60,7 +60,19 @@ void QGCPositionManager::setNmeaSourceDevice(QIODevice* device)
 
 void QGCPositionManager::_positionUpdated(const QGeoPositionInfo &update)
 {
-    emit lastPositionUpdated(update.isValid(), QVariant::fromValue(update.coordinate()));
+    QGeoCoordinate newGCSPosition = QGeoCoordinate();
+
+    if (update.isValid()) {
+        // Note that gcsPosition filters out possible crap values
+        if (qAbs(update.coordinate().latitude()) > 0.001 && qAbs(update.coordinate().longitude()) > 0.001) {
+            newGCSPosition = update.coordinate();
+        }
+    }
+    if (newGCSPosition != _gcsPosition) {
+        _gcsPosition = newGCSPosition;
+        emit gcsPositionChanged(_gcsPosition);
+    }
+
     emit positionInfoUpdated(update);
 }
 

src/PositionManager/PositionManager.h

@@ -25,6 +25,8 @@ public:
     QGCPositionManager(QGCApplication* app, QGCToolbox* toolbox);
     ~QGCPositionManager();
 
+    Q_PROPERTY(QGeoCoordinate gcsPosition READ gcsPosition NOTIFY gcsPositionChanged)
+
     enum QGCPositionSource {
         Simulated,
         InternalGPS,
@@ -32,6 +34,8 @@ public:
         NmeaGPS
     };
 
+    QGeoCoordinate gcsPosition(void) { return _gcsPosition; }
+
     void setPositionSource(QGCPositionSource source);
 
     int updateInterval() const;
@@ -45,13 +49,15 @@ private slots:
     void _error(QGeoPositionInfoSource::Error positioningError);
 
 signals:
-    void lastPositionUpdated(bool valid, QVariant lastPosition);
+    void gcsPositionChanged(QGeoCoordinate gcsPosition);
     void positionInfoUpdated(QGeoPositionInfo update);
 
 private:
-    int _updateInterval;
+    int             _updateInterval;
-    QGeoPositionInfoSource * _currentSource;
+    QGeoCoordinate  _gcsPosition;
-    QGeoPositionInfoSource * _defaultSource;
+
-    QNmeaPositionInfoSource * _nmeaSource;
+    QGeoPositionInfoSource*     _currentSource;
-    QGeoPositionInfoSource * _simulatedSource;
+    QGeoPositionInfoSource*     _defaultSource;
+    QNmeaPositionInfoSource*    _nmeaSource;
+    QGeoPositionInfoSource*     _simulatedSource;
 };

src/Vehicle/Vehicle.cc

@@ -39,6 +39,7 @@
 #include "QGCCameraManager.h"
 #include "VideoReceiver.h"
 #include "VideoManager.h"
+#include "PositionManager.h"
 #if defined(QGC_AIRMAP_ENABLED)
 #include "AirspaceVehicleManager.h"
 #endif
@@ -69,6 +70,7 @@ const char* Vehicle::_altitudeAMSLFactName =        "altitudeAMSL";
 const char* Vehicle::_flightDistanceFactName =      "flightDistance";
 const char* Vehicle::_flightTimeFactName =          "flightTime";
 const char* Vehicle::_distanceToHomeFactName =      "distanceToHome";
+const char* Vehicle::_distanceToGCSFactName =       "distanceToGCS";
 const char* Vehicle::_hobbsFactName =               "hobbs";
 
 const char* Vehicle::_gpsFactGroupName =                "gps";
@@ -192,6 +194,7 @@ Vehicle::Vehicle(LinkInterface*             link,
     , _flightDistanceFact   (0, _flightDistanceFactName,    FactMetaData::valueTypeDouble)
     , _flightTimeFact       (0, _flightTimeFactName,        FactMetaData::valueTypeElapsedTimeInSeconds)
     , _distanceToHomeFact   (0, _distanceToHomeFactName,    FactMetaData::valueTypeDouble)
+    , _distanceToGCSFact    (0, _distanceToGCSFactName,     FactMetaData::valueTypeDouble)
     , _hobbsFact            (0, _hobbsFactName,             FactMetaData::valueTypeString)
     , _gpsFactGroup(this)
     , _battery1FactGroup(this)
@@ -385,6 +388,7 @@ Vehicle::Vehicle(MAV_AUTOPILOT              firmwareType,
     , _flightDistanceFact   (0, _flightDistanceFactName,    FactMetaData::valueTypeDouble)
     , _flightTimeFact       (0, _flightTimeFactName,        FactMetaData::valueTypeElapsedTimeInSeconds)
     , _distanceToHomeFact   (0, _distanceToHomeFactName,    FactMetaData::valueTypeDouble)
+    , _distanceToGCSFact    (0, _distanceToGCSFactName,     FactMetaData::valueTypeDouble)
     , _hobbsFact            (0, _hobbsFactName,             FactMetaData::valueTypeString)
     , _gpsFactGroup(this)
     , _battery1FactGroup(this)
@@ -405,9 +409,12 @@ void Vehicle::_commonInit(void)
     connect(_firmwarePlugin, &FirmwarePlugin::toolbarIndicatorsChanged, this, &Vehicle::toolBarIndicatorsChanged);
 
     connect(this, &Vehicle::coordinateChanged,      this, &Vehicle::_updateDistanceToHome);
+    connect(this, &Vehicle::coordinateChanged,      this, &Vehicle::_updateDistanceToGCS);
     connect(this, &Vehicle::homePositionChanged,    this, &Vehicle::_updateDistanceToHome);
     connect(this, &Vehicle::hobbsMeterChanged,      this, &Vehicle::_updateHobbsMeter);
 
+    connect(_toolbox->qgcPositionManager(), &QGCPositionManager::gcsPositionChanged, this, &Vehicle::_updateDistanceToGCS);
+
     _missionManager = new MissionManager(this);
     connect(_missionManager, &MissionManager::error,                    this, &Vehicle::_missionManagerError);
     connect(_missionManager, &MissionManager::newMissionItemsAvailable, this, &Vehicle::_missionLoadComplete);
@@ -453,6 +460,7 @@ void Vehicle::_commonInit(void)
     _addFact(&_flightDistanceFact,      _flightDistanceFactName);
     _addFact(&_flightTimeFact,          _flightTimeFactName);
     _addFact(&_distanceToHomeFact,      _distanceToHomeFactName);
+    _addFact(&_distanceToGCSFact,       _distanceToGCSFactName);
 
     _hobbsFact.setRawValue(QVariant(QString("0000:00:00")));
     _addFact(&_hobbsFact,               _hobbsFactName);
@@ -3598,6 +3606,16 @@ void Vehicle::_updateDistanceToHome(void)
     }
 }
 
+void Vehicle::_updateDistanceToGCS(void)
+{
+    QGeoCoordinate gcsPosition = _toolbox->qgcPositionManager()->gcsPosition();
+    if (coordinate().isValid() && gcsPosition.isValid()) {
+        _distanceToGCSFact.setRawValue(coordinate().distanceTo(gcsPosition));
+    } else {
+        _distanceToGCSFact.setRawValue(qQNaN());
+    }
+}
+
 void Vehicle::_updateHobbsMeter(void)
 {
     _hobbsFact.setRawValue(hobbsMeter());

src/Vehicle/Vehicle.h

@@ -656,6 +656,7 @@ public:
     Q_PROPERTY(Fact* altitudeAMSL       READ altitudeAMSL       CONSTANT)
     Q_PROPERTY(Fact* flightDistance     READ flightDistance     CONSTANT)
     Q_PROPERTY(Fact* distanceToHome     READ distanceToHome     CONSTANT)
+    Q_PROPERTY(Fact* distanceToGCS      READ distanceToGCS      CONSTANT)
     Q_PROPERTY(Fact* hobbs              READ hobbs              CONSTANT)
 
     Q_PROPERTY(FactGroup* gps               READ gpsFactGroup               CONSTANT)
@@ -945,6 +946,7 @@ public:
     Fact* altitudeAMSL      (void) { return &_altitudeAMSLFact; }
     Fact* flightDistance    (void) { return &_flightDistanceFact; }
     Fact* distanceToHome    (void) { return &_distanceToHomeFact; }
+    Fact* distanceToGCS     (void) { return &_distanceToGCSFact; }
     Fact* hobbs             (void) { return &_hobbsFact; }
 
     FactGroup* gpsFactGroup             (void) { return &_gpsFactGroup; }
@@ -1190,6 +1192,7 @@ private slots:
     void _clearTrajectoryPoints(void);
     void _clearCameraTriggerPoints(void);
     void _updateDistanceToHome(void);
+    void _updateDistanceToGCS(void);
     void _updateHobbsMeter(void);
     void _vehicleParamLoaded(bool ready);
     void _sendQGCTimeToVehicle(void);
@@ -1453,6 +1456,7 @@ private:
     Fact _flightDistanceFact;
     Fact _flightTimeFact;
     Fact _distanceToHomeFact;
+    Fact _distanceToGCSFact;
     Fact _hobbsFact;
 
     VehicleGPSFactGroup             _gpsFactGroup;
@@ -1480,6 +1484,7 @@ private:
     static const char* _flightDistanceFactName;
     static const char* _flightTimeFactName;
     static const char* _distanceToHomeFactName;
+    static const char* _distanceToGCSFactName;
     static const char* _hobbsFactName;
 
     static const char* _gpsFactGroupName;

src/Vehicle/VehicleFact.json

@@ -91,6 +91,13 @@
     "units":            "m"
 },
 {
+    "name":             "distanceToGCS",
+    "shortDescription": "Distance to GCS",
+    "type":             "double",
+    "decimalPlaces":    1,
+    "units":            "m"
+},
+{
     "name":             "flightTime",
     "shortDescription": "Flight Time",
     "type":             "elapsedSeconds",