Merge pull request #2122 from DonLakeFlyer/WaypointDistance

Calculate/Display distance between waypoints

src/MissionEditor/MissionEditor.qml

@@ -38,7 +38,9 @@ import QGroundControl.Controllers   1.0
 /// Mission Editor
 
 QGCView {
-    viewPanel: panel
+    id:         _root
+
+    viewPanel:  panel
 
     // zOrder comes from the Loader in MainWindow.qml
     z: QGroundControl.zOrderTopMost
@@ -89,6 +91,15 @@ QGCView {
         }
     }
 
+    function showDistance(missionItem) {
+        if (missionItem.distance < 0.0) {
+            waypointDistanceDisplay.visible = false
+        } else {
+            waypointDistanceDisplay.distance = missionItem.distance
+            waypointDistanceDisplay.visible = true
+        }
+    }
+
     MissionController {
         id:         controller
 
@@ -184,9 +195,9 @@ QGCView {
                     y:              missionItemIndicator ? (missionItemIndicator.y + missionItemIndicator.anchorPoint.y - (itemDragger.height / 2)) : 100
                     width:          _radius * 2
                     height:         _radius * 2
-                    radius:         _radius
-                    border.width:   2
-                    border.color:   "white"
+                    //radius:         _radius
+                    //border.width:   2
+                    //border.color:   "white"
                     color:          "transparent"
                     visible:        false
                     z:              QGroundControl.zOrderMapItems + 1    // Above item icons
@@ -285,6 +296,7 @@ QGCView {
                                 itemDragger.missionItem.coordinate = coordinate
                                 editorMap.latitude = itemDragger.missionItem.coordinate.latitude
                                 editorMap.longitude = itemDragger.missionItem.coordinate.longitude
+                                _root.showDistance(itemDragger.missionItem)
                             }
                         }
                     }
@@ -313,19 +325,22 @@ QGCView {
                             target: object
 
                             onIsCurrentItemChanged: {
-                                if (object.isCurrentItem && object.specifiesCoordinate) {
-                                    // Setup our drag item
-                                    if (object.sequenceNumber != 0) {
-                                        itemDragger.visible = true
-                                        itemDragger.missionItem = Qt.binding(function() { return object })
-                                        itemDragger.missionItemIndicator = Qt.binding(function() { return itemIndicator })
-                                    } else {
-                                        itemDragger.clearItem()
-                                    }
+                                if (object.isCurrentItem) {
+                                    _root.showDistance(object)
+                                    if (object.specifiesCoordinate) {
+                                        // Setup our drag item
+                                        if (object.sequenceNumber != 0) {
+                                            itemDragger.visible = true
+                                            itemDragger.missionItem = Qt.binding(function() { return object })
+                                            itemDragger.missionItemIndicator = Qt.binding(function() { return itemIndicator })
+                                        } else {
+                                            itemDragger.clearItem()
+                                        }
 
-                                    // Move to the new position
-                                    editorMap.latitude = object.coordinate.latitude
-                                    editorMap.longitude = object.coordinate.longitude
+                                        // Move to the new position
+                                        editorMap.latitude = object.coordinate.latitude
+                                        editorMap.longitude = object.coordinate.longitude
+                                    }
                                 }
                             }
                         }
@@ -854,6 +869,31 @@ QGCView {
                     z:                  QGroundControl.zOrderWidgets
                     checked:            _showHelp
                 }
+
+                Rectangle {
+                    id:                 waypointDistanceDisplay
+                    anchors.margins:    margins
+                    anchors.left:       parent.left
+                    anchors.bottom:     parent.bottom
+                    width:              distanceLabel.width + margins
+                    height:             distanceLabel.height + margins
+                    radius:             ScreenTools.defaultFontPixelWidth
+                    color:              qgcPal.window
+                    opacity:            0.80
+                    visible:            false
+
+                    property real distance: 0
+
+                    readonly property real margins: ScreenTools.defaultFontPixelWidth
+
+                    QGCLabel {
+                        id:                         distanceLabel
+                        anchors.verticalCenter:     parent.verticalCenter
+                        anchors.horizontalCenter:   parent.horizonalCenter
+                        color:                      qgcPal.text
+                        text:                       "Distance: " + Math.round(parent.distance) + " meters"
+                    }
+                }
             } // FlightMap
         } // Item - split view container
     } // QGCViewPanel

src/MissionItem.cc

@@ -80,6 +80,7 @@ MissionItem::MissionItem(QObject*       parent,
     , _autocontinue(autocontinue)
     , _isCurrentItem(isCurrentItem)
     , _reachedTime(0)
+    , _distance(0.0)
     , _headingDegreesFact(NULL)
     , _dirty(false)
     , _homePositionSpecialCase(false)
@@ -189,6 +190,7 @@ const MissionItem& MissionItem::operator=(const MissionItem& other)
     _command                    = other._command;
     _autocontinue               = other._autocontinue;
     _reachedTime                = other._reachedTime;
+    _distance                   = other._distance;
     _altitudeRelativeToHomeFact = other._altitudeRelativeToHomeFact;
     _dirty                      = other._dirty;
     _homePositionSpecialCase    = other._homePositionSpecialCase;
@@ -231,7 +233,8 @@ void MissionItem::_connectSignals(void)
     connect(_longitudeFact, &Fact::valueChanged, this, &MissionItem::_coordinateFactChanged);
     connect(_altitudeFact,  &Fact::valueChanged, this, &MissionItem::_coordinateFactChanged);
 
-    connect(_headingDegreesFact, &Fact::valueChanged, this, &MissionItem::_headingDegreesFactChanged);
+    connect(_headingDegreesFact,            &Fact::valueChanged, this, &MissionItem::_headingDegreesFactChanged);
+    connect(_altitudeRelativeToHomeFact,    &Fact::valueChanged, this, &MissionItem::_altitudeRelativeToHomeFactChanged);
 }
 
 MissionItem::~MissionItem()
@@ -282,7 +285,6 @@ void MissionItem::setSequenceNumber(int sequenceNumber)
 {
     _sequenceNumber = sequenceNumber;
     emit sequenceNumberChanged(_sequenceNumber);
-    emit changed(this);
 }
 
 void MissionItem::setX(double x)
@@ -314,7 +316,6 @@ void MissionItem::setLatitude(double lat)
     if (_latitudeFact->value().toDouble() != lat)
     {
         _latitudeFact->setValue(lat);
-        emit changed(this);
         emit coordinateChanged(coordinate());
     }
 }
@@ -324,7 +325,6 @@ void MissionItem::setLongitude(double lon)
     if (_longitudeFact->value().toDouble() != lon)
     {
         _longitudeFact->setValue(lon);
-        emit changed(this);
         emit coordinateChanged(coordinate());
     }
 }
@@ -334,7 +334,6 @@ void MissionItem::setAltitude(double altitude)
     if (_altitudeFact->value().toDouble() != altitude)
     {
         _altitudeFact->setValue(altitude);
-        emit changed(this);
         emit valueStringsChanged(valueStrings());
         emit coordinateChanged(coordinate());
     }
@@ -376,7 +375,6 @@ void MissionItem::setAction(int /*MAV_CMD*/ action)
             setFrame(MAV_FRAME_MISSION);
         }
 
-        emit changed(this);
         emit commandNameChanged(commandName());
         emit commandChanged((MavlinkQmlSingleton::Qml_MAV_CMD)_command);
         emit valueLabelsChanged(valueLabels());
@@ -398,7 +396,7 @@ void MissionItem::setFrame(int /*MAV_FRAME*/ frame)
     if (_frame != frame) {
         _altitudeRelativeToHomeFact->setValue(frame == MAV_FRAME_GLOBAL_RELATIVE_ALT);
         _frame = frame;
-        emit changed(this);
+        emit frameChanged(_frame);
     }
 }
 
@@ -406,7 +404,7 @@ void MissionItem::setAutocontinue(bool autoContinue)
 {
     if (_autocontinue != autoContinue) {
         _autocontinue = autoContinue;
-        emit changed(this);
+        emit autoContinueChanged(_autocontinue);
     }
 }
 
@@ -428,7 +426,6 @@ void MissionItem::setParam1(double param)
     if (param1() != param)
     {
         _param1Fact->setValue(param);
-        emit changed(this);
         emit valueStringsChanged(valueStrings());
     }
 }
@@ -439,7 +436,6 @@ void MissionItem::setParam2(double param)
     {
         _param2Fact->setValue(param);
         emit valueStringsChanged(valueStrings());
-        emit changed(this);
     }
 }
 
@@ -473,7 +469,6 @@ void MissionItem::setLoiterOrbitRadius(double radius)
     if (loiterOrbitRadius() != radius) {
         _loiterOrbitRadiusFact->setValue(radius);
         emit valueStringsChanged(valueStrings());
-        emit changed(this);
     }
 }
 
@@ -813,7 +808,6 @@ void MissionItem::setHeadingDegrees(double headingDegrees)
 {
     if (_headingDegreesFact->value().toDouble() != headingDegrees) {
         _headingDegreesFact->setValue(headingDegrees);
-        emit changed(this);
         emit valueStringsChanged(valueStrings());
         emit headingDegreesChanged(headingDegrees);
     }
@@ -911,8 +905,25 @@ void MissionItem::_headingDegreesFactChanged(QVariant value)
     emit headingDegreesChanged(value.toDouble());
 }
 
+void MissionItem::_altitudeRelativeToHomeFactChanged(QVariant value)
+{
+    // Don't call setFrame, that will cause a signalling loop
+
+    int frame = value.toBool() ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL;
+    if (_frame != frame) {
+        _frame = frame;
+        emit frameChanged(_frame);
+    }
+}
+
 void MissionItem::setHomePositionValid(bool homePositionValid)
 {
     _homePositionValid = homePositionValid;
     emit homePositionValidChanged(_homePositionValid);
 }
+
+void MissionItem::setDistance(double distance)
+{
+    _distance = distance;
+    emit distanceChanged(_distance);
+}

src/MissionItem.h

@@ -91,6 +91,9 @@ public:
     /// true: home position should be shown
     Q_PROPERTY(bool                 homePositionValid   READ homePositionValid      WRITE setHomePositionValid NOTIFY homePositionValidChanged)
 
+    /// Distance to previous waypoint, set by UI controller
+    Q_PROPERTY(double               distance            READ distance               WRITE setDistance       NOTIFY distanceChanged)
+
     // Property accesors
     
     int sequenceNumber(void) const { return _sequenceNumber; }
@@ -134,6 +137,9 @@ public:
     bool homePosition(void) { return _homePositionSpecialCase; }
     bool homePositionValid(void) { return _homePositionValid; }
     void setHomePositionValid(bool homePositionValid);
+
+    double distance(void) { return _distance; }
+    void setDistance(double distance);
     
     // C++ only methods
     
@@ -207,6 +213,8 @@ public:
     
     void setHomePositionSpecialCase(bool homePositionSpecialCase) { _homePositionSpecialCase = homePositionSpecialCase; }
 
+    bool relativeAltitude(void) { return _frame == MAV_FRAME_GLOBAL_RELATIVE_ALT; }
+
     static const double defaultPitch;
     static const double defaultHeading;
     static const double defaultAltitude;
@@ -221,15 +229,13 @@ signals:
     void headingDegreesChanged(double heading);
     void dirtyChanged(bool dirty);
     void homePositionValidChanged(bool homePostionValid);
-
-    /** @brief Announces a change to the waypoint data */
-    void changed(MissionItem* wp);
-
-    
+    void distanceChanged(float distance);
+    void frameChanged(int frame);
     void commandNameChanged(QString type);
     void commandChanged(MavlinkQmlSingleton::Qml_MAV_CMD command);
     void valueLabelsChanged(QStringList valueLabels);
     void valueStringsChanged(QStringList valueStrings);
+    bool autoContinueChanged(bool autoContinue);
     
 public:
     /** @brief Set the waypoint action */
@@ -253,14 +259,11 @@ public:
     /** @brief Wether this waypoint has been reached yet */
     bool isReached      () { return (_reachedTime > 0); }
 
-    void setChanged() {
-        emit changed(this);
-    }
-    
 private slots:
     void _factValueChanged(QVariant value);
     void _coordinateFactChanged(QVariant value);
     void _headingDegreesFactChanged(QVariant value);
+    void _altitudeRelativeToHomeFactChanged(QVariant value);
 
 private:
     QString _oneDecimalString(double value);
@@ -279,6 +282,7 @@ private:
     bool                                _autocontinue;
     bool                                _isCurrentItem;
     quint64                             _reachedTime;
+    double                              _distance;
     
     Fact*           _latitudeFact;
     Fact*           _longitudeFact;

src/MissionManager/MissionController.cc

@@ -288,24 +288,72 @@ void MissionController::saveMissionToFile(void)
     _missionItems->setDirty(false);
 }
 
+double MissionController::_calcDistance(bool homePositionValid, double homeAlt, MissionItem* item1, MissionItem* item2)
+{
+    QGeoCoordinate  coord1 =        item1->coordinate();
+    QGeoCoordinate  coord2 =        item2->coordinate();
+    bool            distanceOk =    false;
+
+    // Convert to fixed altitudes
+
+    qCDebug(MissionControllerLog) << homePositionValid << homeAlt
+                                  << item1->relativeAltitude() << item1->coordinate().altitude()
+                                  << item2->relativeAltitude() << item2->coordinate().altitude();
+
+    if (homePositionValid) {
+        distanceOk = true;
+        if (item1->relativeAltitude()) {
+            coord1.setAltitude(homeAlt + coord1.altitude());
+        }
+        if (item2->relativeAltitude()) {
+            coord2.setAltitude(homeAlt + coord2.altitude());
+        }
+    } else {
+        if (item1->relativeAltitude() && item2->relativeAltitude()) {
+            distanceOk = true;
+        }
+    }
+
+    qCDebug(MissionControllerLog) << "distanceOk" << distanceOk;
+
+    if (distanceOk) {
+        return item1->coordinate().distanceTo(item2->coordinate());
+    } else {
+        return -1.0;
+    }
+}
+
 void MissionController::_recalcWaypointLines(void)
 {
     MissionItem*    lastCoordinateItem =    qobject_cast<MissionItem*>(_missionItems->get(0));
+    MissionItem*    homeItem =              lastCoordinateItem;
     bool            firstCoordinateItem =   true;
+    bool            homePositionValid =     homeItem->homePositionValid();
+    double          homeAlt =               homeItem->coordinate().altitude();
+
+    qCDebug(MissionControllerLog) << "_recalcWaypointLines";
+
+    // If home position is valid we can calculate distances between all waypoints.
+    // If home position is not valid we can only calculate distances between waypoints which are
+    // both relative altitude.
+
+    // No distance for first item
+    lastCoordinateItem->setDistance(-1.0);
 
     _waypointLines.clear();
 
     for (int i=1; i<_missionItems->count(); i++) {
         MissionItem* item = qobject_cast<MissionItem*>(_missionItems->get(i));
 
+        item->setDistance(-1.0);    // Assume the worst
+
         if (item->specifiesCoordinate()) {
             if (firstCoordinateItem) {
                 if (item->command() == MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF) {
-                    MissionItem* homeItem = qobject_cast<MissionItem*>(_missionItems->get(0));
-
                     // The first coordinate we hit is a takeoff command so link back to home position if valid
-                    if (homeItem->homePositionValid()) {
-                        _waypointLines.append(new CoordinateVector(qobject_cast<MissionItem*>(_missionItems->get(0))->coordinate(), item->coordinate()));
+                    if (homePositionValid) {
+                        _waypointLines.append(new CoordinateVector(homeItem->coordinate(), item->coordinate()));
+                        item->setDistance(_calcDistance(homePositionValid, homeAlt, homeItem, item));
                     }
                 } else {
                     // First coordiante is not a takeoff command, it does not link backwards to anything
@@ -314,6 +362,7 @@ void MissionController::_recalcWaypointLines(void)
             } else if (!lastCoordinateItem->homePosition() || lastCoordinateItem->homePositionValid()) {
                 // Subsequent coordinate items link to last coordinate item. If the last coordinate item
                 // is an invalid home position we skip the line
+                item->setDistance(_calcDistance(homePositionValid, homeAlt, lastCoordinateItem, item));
                 _waypointLines.append(new CoordinateVector(lastCoordinateItem->coordinate(), item->coordinate()));
             }
             lastCoordinateItem = item;
@@ -407,12 +456,14 @@ void MissionController::_initMissionItem(MissionItem* item)
 
     connect(item, &MissionItem::commandChanged,     this, &MissionController::_itemCommandChanged);
     connect(item, &MissionItem::coordinateChanged,  this, &MissionController::_itemCoordinateChanged);
+    connect(item, &MissionItem::frameChanged,       this, &MissionController::_itemFrameChanged);
 }
 
 void MissionController::_deinitMissionItem(MissionItem* item)
 {
     disconnect(item, &MissionItem::commandChanged,     this, &MissionController::_itemCommandChanged);
     disconnect(item, &MissionItem::coordinateChanged,  this, &MissionController::_itemCoordinateChanged);
+    disconnect(item, &MissionItem::frameChanged,       this, &MissionController::_itemFrameChanged);
 }
 
 void MissionController::_itemCoordinateChanged(const QGeoCoordinate& coordinate)
@@ -421,6 +472,12 @@ void MissionController::_itemCoordinateChanged(const QGeoCoordinate& coordinate)
     _recalcWaypointLines();
 }
 
+void MissionController::_itemFrameChanged(int frame)
+{
+    Q_UNUSED(frame);
+    _recalcWaypointLines();
+}
+
 void MissionController::_itemCommandChanged(MavlinkQmlSingleton::Qml_MAV_CMD command)
 {
     Q_UNUSED(command);;

src/MissionManager/MissionController.h

@@ -77,6 +77,7 @@ signals:
 private slots:
     void _newMissionItemsAvailableFromVehicle();
     void _itemCoordinateChanged(const QGeoCoordinate& coordinate);
+    void _itemFrameChanged(int frame);
     void _itemCommandChanged(MavlinkQmlSingleton::Qml_MAV_CMD command);
     void _activeVehicleChanged(Vehicle* activeVehicle);
     void _activeVehicleHomePositionAvailableChanged(bool homePositionAvailable);
@@ -96,6 +97,7 @@ private:
     void _autoSyncSend(void);
     void _setupMissionItems(bool loadFromVehicle, bool forceLoad);
     void _setupActiveVehicle(Vehicle* activeVehicle, bool forceLoadFromVehicle);
+    double _calcDistance(bool homePositionValid, double homeAlt, MissionItem* item1, MissionItem* item2);
 
 private:
     bool                _editMode;