diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index 14d99fb40e61e92031ab3f573618b6b119eefa1d..2b53005c9e2ef25cf12f4be6924c14dfe49a09a2 100644 --- a/src/MissionManager/MissionController.cc +++ b/src/MissionManager/MissionController.cc @@ -895,6 +895,28 @@ double MissionController::_calcDistanceToHome(VisualMissionItem* currentItem, Vi return distanceOk ? homeCoord.distanceTo(currentCoord) : 0.0; } +void MissionController::_addWaypointLineSegment(CoordVectHashTable& prevItemPairHashTable, VisualItemPair& pair) +{ + if (prevItemPairHashTable.contains(pair)) { + // Pair already exists and connected, just re-use + _linesTable[pair] = prevItemPairHashTable.take(pair); + } else { + // Create a new segment and wire update notifiers + auto linevect = new CoordinateVector(pair.first->isSimpleItem() ? pair.first->coordinate() : pair.first->exitCoordinate(), pair.second->coordinate(), this); + auto originNotifier = pair.first->isSimpleItem() ? &VisualMissionItem::coordinateChanged : &VisualMissionItem::exitCoordinateChanged; + auto endNotifier = &VisualMissionItem::coordinateChanged; + + // Use signals/slots to update the coordinate endpoints + connect(pair.first, originNotifier, linevect, &CoordinateVector::setCoordinate1); + connect(pair.second, 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(pair.second, &VisualMissionItem::coordinateChanged, this, &MissionController::_recalcMissionFlightStatus); + _linesTable[pair] = linevect; + } +} + void MissionController::_recalcWaypointLines(void) { bool firstCoordinateItem = true; @@ -908,7 +930,15 @@ void MissionController::_recalcWaypointLines(void) _linesTable.clear(); _waypointLines.clear(); - bool linkBackToHome = false; + bool linkEndToHome; + SimpleMissionItem* lastItem = _visualItems->value(_visualItems->count() - 1); + if (lastItem && (int)lastItem->command() == MAV_CMD_NAV_RETURN_TO_LAUNCH) { + linkEndToHome = true; + } else { + linkEndToHome = _settingsItem->missionEndRTL(); + } + + bool linkStartToHome = false; for (int i=1; i<_visualItems->count(); i++) { VisualMissionItem* item = qobject_cast(_visualItems->get(i)); @@ -918,36 +948,24 @@ void MissionController::_recalcWaypointLines(void) item->isSimpleItem() && (qobject_cast(item)->command() == MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF || qobject_cast(item)->command() == MavlinkQmlSingleton::MAV_CMD_NAV_VTOL_TAKEOFF)) { - linkBackToHome = true; + linkStartToHome = true; } if (item->specifiesCoordinate()) { if (!item->isStandaloneCoordinate()) { firstCoordinateItem = false; VisualItemPair pair(lastCoordinateItem, item); - if (lastCoordinateItem != _settingsItem || (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::_recalcMissionFlightStatus); - _linesTable[pair] = linevect; - } + if (lastCoordinateItem != _settingsItem || (showHomePosition && linkStartToHome)) { + _addWaypointLineSegment(old_table, pair); } lastCoordinateItem = item; } } } + if (linkEndToHome && lastCoordinateItem != _settingsItem && showHomePosition) { + VisualItemPair pair(lastCoordinateItem, _settingsItem); + _addWaypointLineSegment(old_table, pair); + } { // Create a temporary QObjectList and replace the model data @@ -1029,7 +1047,7 @@ void MissionController::_recalcMissionFlightStatus() _resetMissionFlightStatus(); bool vtolInHover = true; - bool linkBackToHome = false; + bool linkStartToHome = false; for (int i=0; i<_visualItems->count(); i++) { VisualMissionItem* item = qobject_cast(_visualItems->get(i)); @@ -1074,7 +1092,7 @@ void MissionController::_recalcMissionFlightStatus() // Link back to home if first item is takeoff and we have home position if (firstCoordinateItem && simpleItem && simpleItem->command() == MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF) { if (showHomePosition) { - linkBackToHome = true; + linkStartToHome = true; } } @@ -1129,7 +1147,7 @@ void MissionController::_recalcMissionFlightStatus() if (!item->isStandaloneCoordinate()) { firstCoordinateItem = false; - if (lastCoordinateItem != _settingsItem || linkBackToHome) { + if (lastCoordinateItem != _settingsItem || linkStartToHome) { // This is a subsequent waypoint or we are forcing the first waypoint back to home double azimuth, distance, altDifference; @@ -1261,7 +1279,7 @@ void MissionController::_setPlannedHomePositionFromFirstCoordinate(void) return; } - // Set the planned home position to be a deltae from first coordinate + // Set the planned home position to be a delta from first coordinate for (int i=1; i<_visualItems->count(); i++) { VisualMissionItem* item = _visualItems->value(i); @@ -1300,8 +1318,9 @@ void MissionController::_initAllVisualItems(void) _settingsItem->setCoordinate(_managerVehicle->homePosition()); } - connect(_settingsItem, &MissionSettingsItem::coordinateChanged, this, &MissionController::_recalcAll); - connect(_settingsItem, &MissionSettingsItem::coordinateChanged, this, &MissionController::plannedHomePositionChanged); + connect(_settingsItem, &MissionSettingsItem::coordinateChanged, this, &MissionController::_recalcAll); + connect(_settingsItem, &MissionSettingsItem::missionEndRTLChanged, this, &MissionController::_recalcAll); + connect(_settingsItem, &MissionSettingsItem::coordinateChanged, this, &MissionController::plannedHomePositionChanged); for (int i=0; i<_visualItems->count(); i++) { VisualMissionItem* item = qobject_cast(_visualItems->get(i)); diff --git a/src/MissionManager/MissionController.h b/src/MissionManager/MissionController.h index b53a9de865c9d23a75edfb0c959b0910c3d2662c..e875c1ac92a5354cee0dcd18807cd57a4b727965 100644 --- a/src/MissionManager/MissionController.h +++ b/src/MissionManager/MissionController.h @@ -209,6 +209,7 @@ private: void _updateBatteryInfo(int waypointIndex); bool _loadItemsFromJson(const QJsonObject& json, QmlObjectListModel* visualItems, QString& errorString); void _initLoadedVisualItems(QmlObjectListModel* loadedVisualItems); + void _addWaypointLineSegment(CoordVectHashTable& prevItemPairHashTable, VisualItemPair& pair); private: MissionManager* _missionManager; diff --git a/src/MissionManager/MissionSettingsItem.cc b/src/MissionManager/MissionSettingsItem.cc index e68c6a91f3379abd5e60d6cef124f5ebfb322e33..a19412bd8fdd6159a2425e93c9d6135e025a100f 100644 --- a/src/MissionManager/MissionSettingsItem.cc +++ b/src/MissionManager/MissionSettingsItem.cc @@ -273,3 +273,11 @@ double MissionSettingsItem::specifiedFlightSpeed(void) return std::numeric_limits::quiet_NaN(); } } + +void MissionSettingsItem::setMissionEndRTL(bool missionEndRTL) +{ + if (missionEndRTL != _missionEndRTL) { + _missionEndRTL = missionEndRTL; + emit missionEndRTLChanged(missionEndRTL); + } +} diff --git a/src/MissionManager/MissionSettingsItem.h b/src/MissionManager/MissionSettingsItem.h index 8679f4925d9dc651aa1815243f6ace97584dcea3..9a511fdcd928899a896957b43c41a6f9f3f27d3f 100644 --- a/src/MissionManager/MissionSettingsItem.h +++ b/src/MissionManager/MissionSettingsItem.h @@ -26,15 +26,17 @@ class MissionSettingsItem : public ComplexMissionItem public: MissionSettingsItem(Vehicle* vehicle, QObject* parent = NULL); - Q_PROPERTY(Fact* plannedHomePositionAltitude READ plannedHomePositionAltitude CONSTANT) - Q_PROPERTY(bool missionEndRTL MEMBER _missionEndRTL NOTIFY missionEndRTLChanged) - Q_PROPERTY(QObject* cameraSection READ cameraSection CONSTANT) - Q_PROPERTY(QObject* speedSection READ speedSection CONSTANT) + Q_PROPERTY(Fact* plannedHomePositionAltitude READ plannedHomePositionAltitude CONSTANT) + Q_PROPERTY(bool missionEndRTL READ missionEndRTL WRITE setMissionEndRTL NOTIFY missionEndRTLChanged) + Q_PROPERTY(QObject* cameraSection READ cameraSection CONSTANT) + Q_PROPERTY(QObject* speedSection READ speedSection CONSTANT) - Fact* plannedHomePositionAltitude (void) { return &_plannedHomePositionAltitudeFact; } + Fact* plannedHomePositionAltitude (void) { return &_plannedHomePositionAltitudeFact; } + bool missionEndRTL (void) const { return _missionEndRTL; } + CameraSection* cameraSection (void) { return &_cameraSection; } + SpeedSection* speedSection (void) { return &_speedSection; } - CameraSection* cameraSection(void) { return &_cameraSection; } - SpeedSection* speedSection(void) { return &_speedSection; } + void setMissionEndRTL(bool missionEndRTL); /// Scans the loaded items for settings items bool scanForMissionSettings(QmlObjectListModel* visualItems, int scanIndex);