Merge pull request #2345 from DonLakeFlyer/AltWork

Show altitude difference from prev waypoint in editor

src/MissionEditor/MissionEditor.qml

@@ -59,7 +59,8 @@ QGCView {
     readonly property int       _addMissionItemsButtonAutoOffTimeout:   10000
     readonly property var       _defaultVehicleCoordinate:   QtPositioning.coordinate(37.803784, -122.462276)
 
-    property var    _missionItems:  controller.missionItems
+    property var    _missionItems:          controller.missionItems
+    property var    _currentMissionItem
 
     property bool   gpsLock:        _activeVehicle ? _activeVehicle.coordinateValid : false
     property bool   _firstGpsLock:  true
@@ -92,16 +93,6 @@ QGCView {
         }
     }
 
-    function showDistance(missionItem) {
-        if (missionItem.distance < 0.0) {
-            waypointValuesDisplay.visible = false
-        } else {
-            waypointValuesDisplay.azimuth = missionItem.azimuth
-            waypointValuesDisplay.distance = missionItem.distance
-            waypointValuesDisplay.visible = true
-        }
-    }
-
     MissionController {
         id:         controller
 
@@ -131,8 +122,14 @@ QGCView {
     }
 
     function setCurrentItem(index) {
+        _currentMissionItem = undefined
         for (var i=0; i<_missionItems.count; i++) {
-            _missionItems.get(i).isCurrentItem = (i == index)
+            if (i == index) {
+                _currentMissionItem = _missionItems.get(i)
+                _currentMissionItem.isCurrentItem = true
+            } else {
+                _missionItems.get(i).isCurrentItem = false
+            }
         }
     }
 
@@ -300,7 +297,6 @@ QGCView {
                                 itemDragger.missionItem.coordinate = coordinate
                                 editorMap.latitude = itemDragger.missionItem.coordinate.latitude
                                 editorMap.longitude = itemDragger.missionItem.coordinate.longitude
-                                _root.showDistance(itemDragger.missionItem)
                             }
                         }
                     }
@@ -328,7 +324,6 @@ QGCView {
                         function updateItemIndicator()
                         {
                             if (object.isCurrentItem) {
-                                _root.showDistance(object)
                                 if (object.specifiesCoordinate) {
                                     // Setup our drag item
                                     if (object.sequenceNumber != 0) {
@@ -908,13 +903,14 @@ QGCView {
                     radius:             ScreenTools.defaultFontPixelWidth
                     color:              qgcPal.window
                     opacity:            0.80
-                    visible:            false
-
-                    property real azimuth:  0
-                    property real distance: 0
+                    visible:            _currentMissionItem ? _currentMissionItem.distance != -1 : false
 
                     readonly property real margins: ScreenTools.defaultFontPixelWidth
 
+                    property real _altDifference:   _currentMissionItem ? _currentMissionItem.altDifference : 0
+                    property real _azimuth:         _currentMissionItem ? _currentMissionItem.azimuth : 0
+                    property real _distance:        _currentMissionItem ? _currentMissionItem.distance : 0
+
                     Column {
                         id:                 valuesColumn
                         anchors.leftMargin: parent.margins
@@ -923,15 +919,21 @@ QGCView {
                         anchors.top:        parent.top
 
                         QGCLabel {
+                            id:     distanceLabel
                             color:  qgcPal.text
-                            text:   "Azimuth: " + Math.round(waypointValuesDisplay.azimuth)
+                            text:   "Distance: " + Math.round(waypointValuesDisplay._distance) + " meters"
+                        }
+
+                        QGCLabel {
+                            color:  qgcPal.text
+                            text:   "Alt diff: " + Math.round(waypointValuesDisplay._altDifference) + " meters"
                         }
 
                         QGCLabel {
-                            id:     distanceLabel
                             color:  qgcPal.text
-                            text:   "Distance: " + Math.round(waypointValuesDisplay.distance) + " meters"
+                            text:   "Azimuth: " + Math.round(waypointValuesDisplay._azimuth)
                         }
+
                     }
                 }
             } // FlightMap

src/MissionManager/MissionController.cc

@@ -280,28 +280,28 @@ void MissionController::saveMissionToFile(void)
     _missionItems->setDirty(false);
 }
 
-void MissionController::_calcPrevWaypointValues(bool homePositionValid, double homeAlt, MissionItem* item1, MissionItem* item2, double* azimuth, double* distance)
+void MissionController::_calcPrevWaypointValues(bool homePositionValid, double homeAlt, MissionItem* currentItem, MissionItem* prevItem, double* azimuth, double* distance, double* altDifference)
 {
-    QGeoCoordinate  coord1 =        item1->coordinate();
-    QGeoCoordinate  coord2 =        item2->coordinate();
+    QGeoCoordinate  currentCoord =  currentItem->coordinate();
+    QGeoCoordinate  prevCoord =     prevItem->coordinate();
     bool            distanceOk =    false;
 
     // Convert to fixed altitudes
 
     qCDebug(MissionControllerLog) << homePositionValid << homeAlt
-                                  << item1->relativeAltitude() << item1->coordinate().altitude()
-                                  << item2->relativeAltitude() << item2->coordinate().altitude();
+                                  << currentItem->relativeAltitude() << currentItem->coordinate().altitude()
+                                  << prevItem->relativeAltitude() << prevItem->coordinate().altitude();
 
     if (homePositionValid) {
         distanceOk = true;
-        if (item1->relativeAltitude()) {
-            coord1.setAltitude(homeAlt + coord1.altitude());
+        if (currentItem->relativeAltitude()) {
+            currentCoord.setAltitude(homeAlt + currentCoord.altitude());
         }
-        if (item2->relativeAltitude()) {
-            coord2.setAltitude(homeAlt + coord2.altitude());
+        if (prevItem->relativeAltitude()) {
+            prevCoord.setAltitude(homeAlt + prevCoord.altitude());
         }
     } else {
-        if (item1->relativeAltitude() && item2->relativeAltitude()) {
+        if (prevItem->relativeAltitude() && currentItem->relativeAltitude()) {
             distanceOk = true;
         }
     }
@@ -309,11 +309,13 @@ void MissionController::_calcPrevWaypointValues(bool homePositionValid, double h
     qCDebug(MissionControllerLog) << "distanceOk" << distanceOk;
 
     if (distanceOk) {
-        *distance = item1->coordinate().distanceTo(item2->coordinate());
-        *azimuth = item1->coordinate().azimuthTo(item2->coordinate());
+        *altDifference = currentCoord.altitude() - prevCoord.altitude();
+        *distance = prevCoord.distanceTo(currentCoord);
+        *azimuth = prevCoord.azimuthTo(currentCoord);
     } else {
+        *altDifference = 0.0;
         *azimuth = 0.0;
-        *distance = -1.0;
+        *distance = -1.0;   // Signals no values
     }
 }
 
@@ -349,11 +351,11 @@ void MissionController::_recalcWaypointLines(void)
                 if (item->command() == MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF) {
                     // The first coordinate we hit is a takeoff command so link back to home position if valid
                     if (homePositionValid) {
-                        double azimuth, distance;
+                        double azimuth, distance, altDifference;
 
                         _waypointLines.append(new CoordinateVector(homeItem->coordinate(), item->coordinate()));
-                        _calcPrevWaypointValues(homePositionValid, homeAlt, homeItem, item, &azimuth, &distance);
-                        qDebug() << azimuth << distance;
+                        _calcPrevWaypointValues(homePositionValid, homeAlt, item, homeItem, &azimuth, &distance, &altDifference);
+                        item->setAltDifference(altDifference);
                         item->setAzimuth(azimuth);
                         item->setDistance(distance);
                     }
@@ -362,12 +364,12 @@ void MissionController::_recalcWaypointLines(void)
                 }
                 firstCoordinateItem = false;
             } else if (!lastCoordinateItem->homePosition() || lastCoordinateItem->homePositionValid()) {
-                double azimuth, distance;
+                double azimuth, distance, altDifference;
 
                 // Subsequent coordinate items link to last coordinate item. If the last coordinate item
                 // is an invalid home position we skip the line
-                _calcPrevWaypointValues(homePositionValid, homeAlt, lastCoordinateItem, item, &azimuth, &distance);
-                qDebug() << azimuth << distance;
+                _calcPrevWaypointValues(homePositionValid, homeAlt, item, lastCoordinateItem, &azimuth, &distance, &altDifference);
+                item->setAltDifference(altDifference);
                 item->setAzimuth(azimuth);
                 item->setDistance(distance);
                 _waypointLines.append(new CoordinateVector(lastCoordinateItem->coordinate(), item->coordinate()));

src/MissionManager/MissionController.h

@@ -94,7 +94,7 @@ private:
     void _autoSyncSend(void);
     void _setupMissionItems(bool loadFromVehicle, bool forceLoad);
     void _setupActiveVehicle(Vehicle* activeVehicle, bool forceLoadFromVehicle);
-    void _calcPrevWaypointValues(bool homePositionValid, double homeAlt, MissionItem* item1, MissionItem* item2, double* azimuth, double* distance);
+    void _calcPrevWaypointValues(bool homePositionValid, double homeAlt, MissionItem* currentItem, MissionItem* prevItem, double* azimuth, double* distance, double* altDifference);
     bool _findLastAltitude(double* lastAltitude);
     bool _findLastAcceptanceRadius(double* lastAcceptanceRadius);
 

src/MissionManager/MissionItem.cc

@@ -138,6 +138,7 @@ MissionItem::MissionItem(int             sequenceNumber,
     , _dirty(false)
     , _sequenceNumber(sequenceNumber)
     , _isCurrentItem(isCurrentItem)
+    , _altDifference(0.0)
     , _azimuth(0.0)
     , _distance(0.0)
     , _homePositionSpecialCase(false)
@@ -194,6 +195,7 @@ MissionItem::MissionItem(const MissionItem& other, QObject* parent)
     , _dirty(false)
     , _sequenceNumber(0)
     , _isCurrentItem(false)
+    , _altDifference(0.0)
     , _azimuth(0.0)
     , _distance(0.0)
     , _homePositionSpecialCase(false)
@@ -237,7 +239,8 @@ const MissionItem& MissionItem::operator=(const MissionItem& other)
     setSequenceNumber(other._sequenceNumber);
     setAutoContinue(other.autoContinue());
     setIsCurrentItem(other._isCurrentItem);
-    setAzimuth(other._distance);
+    setAltDifference(other._altDifference);
+    setAzimuth(other._azimuth);
     setDistance(other._distance);
     setHomePositionSpecialCase(other._homePositionSpecialCase);
     setHomePositionValid(other._homePositionValid);
@@ -717,6 +720,12 @@ void MissionItem::setDistance(double distance)
     emit distanceChanged(_distance);
 }
 
+void MissionItem::setAltDifference(double altDifference)
+{
+    _altDifference = altDifference;
+    emit altDifferenceChanged(_altDifference);
+}
+
 void MissionItem::setAzimuth(double azimuth)
 {
     _azimuth = azimuth;

src/MissionManager/MissionItem.h

@@ -68,6 +68,7 @@ public:
 
     const MissionItem& operator=(const MissionItem& other);
     
+    Q_PROPERTY(double           altDifference       READ altDifference          WRITE setAltDifference      NOTIFY altDifferenceChanged)        ///< Change in altitude from previous waypoint
     Q_PROPERTY(double           azimuth             READ azimuth                WRITE setAzimuth            NOTIFY azimuthChanged)              ///< Azimuth to previous waypoint
     Q_PROPERTY(QString          category            READ category                                           NOTIFY commandChanged)
     Q_PROPERTY(MavlinkQmlSingleton::Qml_MAV_CMD command READ command            WRITE setCommand            NOTIFY commandChanged)
@@ -96,6 +97,7 @@ public:
 
     // Property accesors
     
+    double          altDifference       (void) const    { return _altDifference; }
     double          azimuth             (void) const    { return _azimuth; }
     QString         category            (void) const;
     MavlinkQmlSingleton::Qml_MAV_CMD command(void) const { return (MavlinkQmlSingleton::Qml_MAV_CMD)_commandFact.cookedValue().toInt(); };
@@ -134,8 +136,9 @@ public:
     void setHomePositionValid(bool homePositionValid);
     void setHomePositionSpecialCase(bool homePositionSpecialCase) { _homePositionSpecialCase = homePositionSpecialCase; }
 
-    void setDistance(double distance);
+    void setAltDifference(double altDifference);
     void setAzimuth(double azimuth);
+    void setDistance(double distance);
 
     // C++ only methods
 
@@ -173,6 +176,7 @@ public slots:
     void setDefaultsForCommand(void);
 
 signals:
+    void altDifferenceChanged       (double altDifference);
     void azimuthChanged             (double azimuth);
     void commandChanged             (MavlinkQmlSingleton::Qml_MAV_CMD command);
     void coordinateChanged          (const QGeoCoordinate& coordinate);
@@ -212,6 +216,7 @@ private:
     bool        _dirty;
     int         _sequenceNumber;
     bool        _isCurrentItem;
+    double      _altDifference;             ///< Difference in altitude from previous waypoint
     double      _azimuth;                   ///< Azimuth to previous waypoint
     double      _distance;                  ///< Distance to previous waypoint
     bool        _homePositionSpecialCase;   ///< true: This item is being used as a ui home position indicator