From 970e41aa072d9be92a818ebef1569ecd2f766c5e Mon Sep 17 00:00:00 2001 From: Nate Weibley Date: Thu, 16 Jun 2016 16:55:59 -0400 Subject: [PATCH] Update the MissionController line model to a much faster and less expensive implementation --- src/MissionManager/MissionController.cc | 103 ++++++++++++++++++++---- src/MissionManager/MissionController.h | 10 ++- 2 files changed, 96 insertions(+), 17 deletions(-) diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index aad4a0ad8..6c57a942d 100644 --- a/src/MissionManager/MissionController.cc +++ b/src/MissionManager/MissionController.cc @@ -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 = _lines_table; + _lines_table.clear(); + _waypointLines.clear(); + + bool linkBackToHome = false; + for (int i=1; i<_visualItems->count(); i++) { + VisualMissionItem* item = qobject_cast(_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(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 + _lines_table[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); + _lines_table[pair] = linevect; + } + } + lastCoordinateItem = item; + } + } + } + + + { + // Create a temporary QObjectList and replace the model data + QObjectList objs; + objs.reserve(_lines_table.count()); + foreach(CoordinateVector *vect, _lines_table.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(_visualItems->get(0)); + + SimpleMissionItem* homeItem = qobject_cast(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.clearAndDeleteContents(); - bool linkBackToHome = false; for (int i=1; i<_visualItems->count(); i++) { VisualMissionItem* item = qobject_cast(_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(); diff --git a/src/MissionManager/MissionController.h b/src/MissionManager/MissionController.h index 08f6f97e7..dcc7afafb 100644 --- a/src/MissionManager/MissionController.h +++ b/src/MissionManager/MissionController.h @@ -12,6 +12,7 @@ #define MissionController_H #include +#include #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 VisualItemPair; +typedef QHash 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 _lines_table; Vehicle* _activeVehicle; bool _autoSync; bool _firstItemsFromVehicle; -- 2.22.0