Merge pull request #3584 from NaterGator/missionperformance

Rework mission lines + various mission-related bug fixes

src/FlightDisplay/FlightDisplayViewMap.qml

@@ -67,8 +67,8 @@ FlightMap {
             line.color: "red"
             z:          QGroundControl.zOrderMapItems - 1
             path: [
-                { latitude: object.coordinate1.latitude, longitude: object.coordinate1.longitude },
-                { latitude: object.coordinate2.latitude, longitude: object.coordinate2.longitude },
+                object.coordinate1,
+                object.coordinate2,
             ]
         }
     }

src/FlightMap/MapItems/MissionLineView.qml

@@ -27,8 +27,8 @@ MapItemView {
         z:          QGroundControl.zOrderMapItems - 1   // Under item indicators
 
         path: [
-            { latitude: object.coordinate1.latitude, longitude: object.coordinate1.longitude },
-            { latitude: object.coordinate2.latitude, longitude: object.coordinate2.longitude },
+            object.coordinate1,
+            object.coordinate2,
         ]
     }
 }

src/MissionEditor/MissionEditor.qml

@@ -81,6 +81,7 @@ QGCView {
         } else {
             controller.loadMissionFromFilePicker()
             fitViewportToMissionItems()
+            _currentMissionItem = _visualItems.get(0)
         }
     }
 
@@ -185,6 +186,7 @@ QGCView {
             onFilenameReturned: {
                 controller.loadMissionFromFile(filename)
                 fitViewportToMissionItems()
+                _currentMissionItem = _visualItems.get(0)
             }
         }
     }
@@ -498,6 +500,11 @@ QGCView {
                                 controller.removeMissionItem(index)
                             }
 
+                            onInsert: {
+                                var sequenceNumber = controller.insertSimpleMissionItem(editorMap.center, insertAfterIndex)
+                                setCurrentItem(sequenceNumber)
+                            }
+
                             onMoveHomeToMapCenter: controller.visualItems.get(0).coordinate = editorMap.center
 
                             Connections {

src/MissionManager/MissionCommands.cc

@@ -86,12 +86,7 @@ MAV_AUTOPILOT MissionCommands::_firmwareTypeFromVehicle(Vehicle* vehicle) const
     if (vehicle) {
         return vehicle->firmwareType();
     } else {
-        QSettings settings;
-
-        // FIXME: Hack duplicated code from QGroundControlQmlGlobal. Had to do this for now since
-        // QGroundControlQmlGlobal is not available from C++ side.
-
-        return (MAV_AUTOPILOT)settings.value("OfflineEditingFirmwareType", MAV_AUTOPILOT_ARDUPILOTMEGA).toInt();
+        return (MAV_AUTOPILOT)QGroundControlQmlGlobal::offlineEditingFirmwareType()->rawValue().toInt();
     }
 }
 

src/MissionManager/MissionController.cc

@@ -186,7 +186,7 @@ int MissionController::insertSimpleMissionItem(QGeoCoordinate coordinate, int i)
 
     _recalcAll();
 
-    return sequenceNumber;
+    return newItem->sequenceNumber();
 }
 
 int MissionController::insertComplexMissionItem(QGeoCoordinate coordinate, int i)
@@ -202,7 +202,7 @@ int MissionController::insertComplexMissionItem(QGeoCoordinate coordinate, int i
 
     _recalcAll();
 
-    return sequenceNumber;
+    return newItem->sequenceNumber();
 }
 
 void MissionController::removeMissionItem(int index)
@@ -608,13 +608,98 @@ void MissionController::_recalcWaypointLines(void)
 
     if (!homeItem) {
         qWarning() << "Home item is not SimpleMissionItem";
+    } else {
+        connect(homeItem, &VisualMissionItem::coordinateChanged, this, &MissionController::_recalcAltitudeRangeBearing);
     }
 
     bool    showHomePosition =  homeItem->showHomePosition();
-    double  homeAlt =           homeItem->coordinate().altitude();
 
     qCDebug(MissionControllerLog) << "_recalcWaypointLines";
 
+    CoordVectHashTable old_table = _linesTable;
+    _linesTable.clear();
+    _waypointLines.clear();
+
+    bool linkBackToHome = false;
+    for (int i=1; i<_visualItems->count(); i++) {
+        VisualMissionItem* item = qobject_cast<VisualMissionItem*>(_visualItems->get(i));
+
+
+        // If we still haven't found the first coordinate item and we hit a a takeoff command link back to home
+        if (firstCoordinateItem &&
+                item->isSimpleItem() &&
+                qobject_cast<SimpleMissionItem*>(item)->command() == MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF) {
+            linkBackToHome = true;
+        }
+
+        if (item->specifiesCoordinate()) {
+            if (!item->isStandaloneCoordinate()) {
+                firstCoordinateItem = false;
+                VisualItemPair pair(lastCoordinateItem, item);
+                if (lastCoordinateItem != homeItem || (showHomePosition && linkBackToHome)) {
+                    if (old_table.contains(pair)) {
+                        // Do nothing, this segment already exists and is wired up
+                        _linesTable[pair] = old_table.take(pair);
+                    } else {
+                        // Create a new segment and wire update notifiers
+                        auto linevect       = new CoordinateVector(lastCoordinateItem->isSimpleItem() ? lastCoordinateItem->coordinate() : lastCoordinateItem->exitCoordinate(), item->coordinate(), this);
+                        auto originNotifier = lastCoordinateItem->isSimpleItem() ? &VisualMissionItem::coordinateChanged : &VisualMissionItem::exitCoordinateChanged,
+                             endNotifier    = &VisualMissionItem::coordinateChanged;
+                        // Use signals/slots to update the coordinate endpoints
+                        connect(lastCoordinateItem, originNotifier, linevect, &CoordinateVector::setCoordinate1);
+                        connect(item,               endNotifier,    linevect, &CoordinateVector::setCoordinate2);
+
+                        // FIXME: We should ideally have signals for 2D position change, alt change, and 3D position change
+                        // Not optimal, but still pretty fast, do a full update of range/bearing/altitudes
+                        connect(item, &VisualMissionItem::coordinateChanged, this, &MissionController::_recalcAltitudeRangeBearing);
+                        _linesTable[pair] = linevect;
+                    }
+                }
+                lastCoordinateItem = item;
+            }
+        }
+    }
+
+
+    {
+        // Create a temporary QObjectList and replace the model data
+        QObjectList objs;
+        objs.reserve(_linesTable.count());
+        foreach(CoordinateVector *vect, _linesTable.values()) {
+            objs.append(vect);
+        }
+
+        // We don't delete here because many links may still be valid
+        _waypointLines.swapObjectList(objs);
+    }
+
+
+    // Anything left in the old table is an obsolete line object that can go
+    qDeleteAll(old_table);
+
+    _recalcAltitudeRangeBearing();
+
+    emit waypointLinesChanged();
+}
+
+void MissionController::_recalcAltitudeRangeBearing()
+{
+    if (!_visualItems->count())
+        return;
+
+    bool                firstCoordinateItem =   true;
+    VisualMissionItem*  lastCoordinateItem =    qobject_cast<VisualMissionItem*>(_visualItems->get(0));
+
+    SimpleMissionItem*  homeItem = qobject_cast<SimpleMissionItem*>(lastCoordinateItem);
+
+    if (!homeItem) {
+        qWarning() << "Home item is not SimpleMissionItem";
+    }
+
+    bool    showHomePosition =  homeItem->showHomePosition();
+
+    qCDebug(MissionControllerLog) << "_recalcAltitudeRangeBearing";
+
     // 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.
@@ -626,11 +711,9 @@ void MissionController::_recalcWaypointLines(void)
 
     double minAltSeen = 0.0;
     double maxAltSeen = 0.0;
-    double homePositionAltitude = homeItem->coordinate().altitude();
+    const double homePositionAltitude = homeItem->coordinate().altitude();
     minAltSeen = maxAltSeen = homeItem->coordinate().altitude();
 
-    _waypointLines.clear();
-
     bool linkBackToHome = false;
     for (int i=1; i<_visualItems->count(); i++) {
         VisualMissionItem* item = qobject_cast<VisualMissionItem*>(_visualItems->get(i));
@@ -672,11 +755,10 @@ void MissionController::_recalcWaypointLines(void)
 
                     // Subsequent coordinate items link to last coordinate item. If the last coordinate item
                     // is an invalid home position we skip the line
-                    _calcPrevWaypointValues(homeAlt, item, lastCoordinateItem, &azimuth, &distance, &altDifference);
+                    _calcPrevWaypointValues(homePositionAltitude, item, lastCoordinateItem, &azimuth, &distance, &altDifference);
                     item->setAltDifference(altDifference);
                     item->setAzimuth(azimuth);
                     item->setDistance(distance);
-                    _waypointLines.append(new CoordinateVector(lastCoordinateItem->isSimpleItem() ? lastCoordinateItem->coordinate() : lastCoordinateItem->exitCoordinate(), item->coordinate()));
                 }
                 lastCoordinateItem = item;
             }
@@ -700,8 +782,6 @@ void MissionController::_recalcWaypointLines(void)
             }
         }
     }
-
-    emit waypointLinesChanged();
 }
 
 // This will update the sequence numbers to be sequential starting from 0
@@ -822,7 +902,6 @@ void MissionController::_initVisualItem(VisualMissionItem* visualItem)
 {
     _visualItems->setDirty(false);
 
-    connect(visualItem, &VisualMissionItem::coordinateChanged,                          this, &MissionController::_itemCoordinateChanged);
     connect(visualItem, &VisualMissionItem::specifiesCoordinateChanged,                 this, &MissionController::_recalcWaypointLines);
     connect(visualItem, &VisualMissionItem::coordinateHasRelativeAltitudeChanged,       this, &MissionController::_recalcWaypointLines);
     connect(visualItem, &VisualMissionItem::exitCoordinateHasRelativeAltitudeChanged,   this, &MissionController::_recalcWaypointLines);
@@ -848,12 +927,6 @@ void MissionController::_deinitVisualItem(VisualMissionItem* visualItem)
     disconnect(visualItem, 0, 0, 0);
 }
 
-void MissionController::_itemCoordinateChanged(const QGeoCoordinate& coordinate)
-{
-    Q_UNUSED(coordinate);
-    _recalcWaypointLines();
-}
-
 void MissionController::_itemCommandChanged(void)
 {
     _recalcChildItems();

src/MissionManager/MissionController.h

@@ -12,6 +12,7 @@
 #define MissionController_H
 
 #include <QObject>
+#include <QHash>
 
 #include "QmlObjectListModel.h"
 #include "Vehicle.h"
@@ -19,8 +20,12 @@
 #include "MavlinkQmlSingleton.h"
 #include "VisualMissionItem.h"
 
+class CoordinateVector;
+
 Q_DECLARE_LOGGING_CATEGORY(MissionControllerLog)
 
+typedef QPair<VisualMissionItem*,VisualMissionItem*> VisualItemPair;
+typedef QHash<VisualItemPair, CoordinateVector*> CoordVectHashTable;
 class MissionController : public QObject
 {
     Q_OBJECT
@@ -77,7 +82,6 @@ signals:
 
 private slots:
     void _newMissionItemsAvailableFromVehicle();
-    void _itemCoordinateChanged(const QGeoCoordinate& coordinate);
     void _itemCommandChanged(void);
     void _activeVehicleChanged(Vehicle* activeVehicle);
     void _activeVehicleHomePositionAvailableChanged(bool homePositionAvailable);
@@ -86,6 +90,7 @@ private slots:
     void _inProgressChanged(bool inProgress);
     void _currentMissionItemChanged(int sequenceNumber);
     void _recalcWaypointLines(void);
+    void _recalcAltitudeRangeBearing();
 
 private:
     void _recalcSequence(void);
@@ -97,7 +102,7 @@ private:
     void _deinitVisualItem(VisualMissionItem* item);
     void _autoSyncSend(void);
     void _setupActiveVehicle(Vehicle* activeVehicle, bool forceLoadFromVehicle);
-    void _calcPrevWaypointValues(double homeAlt, VisualMissionItem* currentItem, VisualMissionItem* prevItem, double* azimuth, double* distance, double* altDifference);
+    static void _calcPrevWaypointValues(double homeAlt, VisualMissionItem* currentItem, VisualMissionItem* prevItem, double* azimuth, double* distance, double* altDifference);
     bool _findLastAltitude(double* lastAltitude);
     bool _findLastAcceptanceRadius(double* lastAcceptanceRadius);
     void _addPlannedHomePosition(QmlObjectListModel* visualItems, bool addToCenter);
@@ -112,6 +117,7 @@ private:
     QmlObjectListModel* _visualItems;
     QmlObjectListModel* _complexItems;
     QmlObjectListModel  _waypointLines;
+    CoordVectHashTable  _linesTable;
     Vehicle*            _activeVehicle;
     bool                _autoSync;
     bool                _firstItemsFromVehicle;

src/MissionManager/SimpleMissionItem.cc

@@ -593,5 +593,10 @@ void SimpleMissionItem::setCoordinate(const QGeoCoordinate& coordinate)
 
 void SimpleMissionItem::setSequenceNumber(int sequenceNumber)
 {
-    _missionItem.setSequenceNumber(sequenceNumber);
+    if (_missionItem.sequenceNumber() != sequenceNumber) {
+        _missionItem.setSequenceNumber(sequenceNumber);
+        emit sequenceNumberChanged(sequenceNumber);
+        // This is too likely to ignore
+        emit abbreviationChanged();
+    }
 }

src/MissionManager/VisualMissionItem.cc

@@ -70,24 +70,32 @@ void VisualMissionItem::setIsCurrentItem(bool isCurrentItem)
 
 void VisualMissionItem::setDistance(double distance)
 {
-    _distance = distance;
-    emit distanceChanged(_distance);
+    if (!qFuzzyCompare(_distance, distance)) {
+        _distance = distance;
+        emit distanceChanged(_distance);
+    }
 }
 
 void VisualMissionItem::setAltDifference(double altDifference)
 {
-    _altDifference = altDifference;
-    emit altDifferenceChanged(_altDifference);
+    if (!qFuzzyCompare(_altDifference, altDifference)) {
+        _altDifference = altDifference;
+        emit altDifferenceChanged(_altDifference);
+    }
 }
 
 void VisualMissionItem::setAltPercent(double altPercent)
 {
-    _altPercent = altPercent;
-    emit altPercentChanged(_altPercent);
+    if (!qFuzzyCompare(_altPercent, altPercent)) {
+        _altPercent = altPercent;
+        emit altPercentChanged(_altPercent);
+    }
 }
 
 void VisualMissionItem::setAzimuth(double azimuth)
 {
-    _azimuth = azimuth;
-    emit azimuthChanged(_azimuth);
+    if (!qFuzzyCompare(_azimuth, azimuth)) {
+        _azimuth = azimuth;
+        emit azimuthChanged(_azimuth);
+    }
 }

src/QmlControls/CoordinateVector.cc

@@ -27,15 +27,24 @@ CoordinateVector::CoordinateVector(const QGeoCoordinate& coordinate1, const QGeo
     
 }
 
-CoordinateVector::~CoordinateVector()
+void CoordinateVector::setCoordinates(const QGeoCoordinate& coordinate1, const QGeoCoordinate& coordinate2)
 {
-    
+    setCoordinate1(coordinate1);
+    setCoordinate2(coordinate2);
 }
 
-void CoordinateVector::setCoordinates(const QGeoCoordinate& coordinate1, const QGeoCoordinate& coordinate2)
+void CoordinateVector::setCoordinate1(const QGeoCoordinate &coordinate)
+{
+    if (_coordinate1 != coordinate) {
+        _coordinate1 = coordinate;
+        emit coordinate1Changed(_coordinate1);
+    }
+}
+
+void CoordinateVector::setCoordinate2(const QGeoCoordinate &coordinate)
 {
-    _coordinate1 = coordinate1;
-    _coordinate2 = coordinate2;
-    emit coordinate1Changed(_coordinate1);
-    emit coordinate2Changed(_coordinate2);
+    if (_coordinate2 != coordinate) {
+        _coordinate2 = coordinate;
+        emit coordinate2Changed(_coordinate2);
+    }
 }

src/QmlControls/CoordinateVector.h

@@ -21,12 +21,15 @@ class CoordinateVector : public QObject
 public:
     CoordinateVector(QObject* parent = NULL);
     CoordinateVector(const QGeoCoordinate& coordinate1, const QGeoCoordinate& coordinate2, QObject* parent = NULL);
-    ~CoordinateVector();
     
     Q_PROPERTY(QGeoCoordinate coordinate1 MEMBER _coordinate1 NOTIFY coordinate1Changed)
     Q_PROPERTY(QGeoCoordinate coordinate2 MEMBER _coordinate2 NOTIFY coordinate2Changed)
     
     void setCoordinates(const QGeoCoordinate& coordinate1, const QGeoCoordinate& coordinate2);
+
+public slots:
+    void setCoordinate1(const QGeoCoordinate& coordinate);
+    void setCoordinate2(const QGeoCoordinate& coordinate);
     
 signals:
     void coordinate1Changed(QGeoCoordinate coordinate);

src/QmlControls/MissionItemEditor.qml

@@ -24,7 +24,7 @@ Rectangle {
 
     signal clicked
     signal remove
-    signal insert(int i)
+    signal insert(int insertAfterIndex)
     signal moveHomeToMapCenter
 
     property bool   _currentItem:       missionItem.isCurrentItem

src/QmlControls/QmlObjectListModel.cc

@@ -179,6 +179,15 @@ void QmlObjectListModel::append(QObject* object)
     insert(_objectList.count(), object);
 }
 
+QObjectList QmlObjectListModel::swapObjectList(QObjectList newlist)
+{
+    QObjectList oldlist(_objectList);
+    beginResetModel();
+    _objectList = newlist;
+    endResetModel();
+    return oldlist;
+}
+
 int QmlObjectListModel::count(void) const
 {
     return rowCount();

src/QmlControls/QmlObjectListModel.h

@@ -37,6 +37,7 @@ public:
     void setDirty(bool dirty);
     
     void append(QObject* object);
+    QObjectList swapObjectList(QObjectList newlist);
     void clear(void);
     QObject* removeAt(int i);
     QObject* removeOne(QObject* object) { return removeAt(indexOf(object)); }