diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index 45e41a1c6ae73718b463185f586903810452e34d..bc085196e1486577ef467330e344155b145997b1 100644 --- a/src/MissionManager/MissionController.cc +++ b/src/MissionManager/MissionController.cc @@ -377,7 +377,7 @@ VisualMissionItem* MissionController::_insertSimpleMissionItemWorker(QGeoCoordin } // We send the click coordinate through here to be able to set the planned home position from the user click location if needed - _recalcAllWithClickCoordinate(coordinate); + _recalcAllWithCoordinate(coordinate); if (makeCurrentItem) { setCurrentPlanViewIndex(newItem->sequenceNumber(), true); @@ -392,14 +392,11 @@ VisualMissionItem* MissionController::insertSimpleMissionItem(QGeoCoordinate coo return _insertSimpleMissionItemWorker(coordinate, MAV_CMD_NAV_WAYPOINT, visualItemIndex, makeCurrentItem); } -VisualMissionItem* MissionController::insertTakeoffItem(QGeoCoordinate coordinate, int visualItemIndex, bool makeCurrentItem) +VisualMissionItem* MissionController::insertTakeoffItem(QGeoCoordinate /*coordinate*/, int visualItemIndex, bool makeCurrentItem) { int sequenceNumber = _nextSequenceNumber(); TakeoffMissionItem * newItem = new TakeoffMissionItem(_controllerVehicle->vtol() ? MAV_CMD_NAV_VTOL_TAKEOFF : MAV_CMD_NAV_TAKEOFF, _controllerVehicle, _flyView, _settingsItem, this); newItem->setSequenceNumber(sequenceNumber); - if (!newItem->coordinate().isValid()) { - newItem->setCoordinate(coordinate); - } newItem->setWizardMode(true); _initVisualItem(newItem); @@ -419,8 +416,7 @@ VisualMissionItem* MissionController::insertTakeoffItem(QGeoCoordinate coordinat _visualItems->insert(visualItemIndex, newItem); } - // We send the click coordinate through here to be able to set the planned home position from the user click location if needed - _recalcAllWithClickCoordinate(coordinate); + _recalcAll(); if (makeCurrentItem) { setCurrentPlanViewIndex(newItem->sequenceNumber(), true); @@ -466,7 +462,7 @@ VisualMissionItem* MissionController::insertComplexMissionItem(QString itemName, return nullptr; } - _insertComplexMissionItemWorker(newItem, visualItemIndex, makeCurrentItem); + _insertComplexMissionItemWorker(mapCenterCoordinate, newItem, visualItemIndex, makeCurrentItem); return newItem; } @@ -486,12 +482,12 @@ VisualMissionItem* MissionController::insertComplexMissionItemFromKMLOrSHP(QStri return nullptr; } - _insertComplexMissionItemWorker(newItem, visualItemIndex, makeCurrentItem); + _insertComplexMissionItemWorker(QGeoCoordinate(), newItem, visualItemIndex, makeCurrentItem); return newItem; } -void MissionController::_insertComplexMissionItemWorker(ComplexMissionItem* complexItem, int visualItemIndex, bool makeCurrentItem) +void MissionController::_insertComplexMissionItemWorker(const QGeoCoordinate& mapCenterCoordinate, ComplexMissionItem* complexItem, int visualItemIndex, bool makeCurrentItem) { int sequenceNumber = _nextSequenceNumber(); bool surveyStyleItem = qobject_cast(complexItem) || @@ -538,7 +534,7 @@ void MissionController::_insertComplexMissionItemWorker(ComplexMissionItem* comp if(!complexItem->isSimpleItem()) { connect(complexItem, &ComplexMissionItem::boundingCubeChanged, this, &MissionController::_complexBoundingBoxChanged); } - _recalcAll(); + _recalcAllWithCoordinate(mapCenterCoordinate); if (makeCurrentItem) { setCurrentPlanViewIndex(complexItem->sequenceNumber(), true); @@ -1649,6 +1645,7 @@ void MissionController::_recalcChildItems(void) void MissionController::_setPlannedHomePositionFromFirstCoordinate(const QGeoCoordinate& clickCoordinate) { + bool foundFirstCoordinate = false; QGeoCoordinate firstCoordinate; if (_settingsItem->coordinate().isValid()) { @@ -1659,14 +1656,15 @@ void MissionController::_setPlannedHomePositionFromFirstCoordinate(const QGeoCoo for (int i=1; i<_visualItems->count(); i++) { VisualMissionItem* item = _visualItems->value(i); - if (item->specifiesCoordinate()) { + if (item->specifiesCoordinate() && item->coordinate().isValid()) { + foundFirstCoordinate = true; firstCoordinate = item->coordinate(); break; } } // No item specifying a coordinate was found, in this case it we have a clickCoordinate use that - if (!firstCoordinate.isValid()) { + if (!foundFirstCoordinate) { firstCoordinate = clickCoordinate; } @@ -1677,11 +1675,10 @@ void MissionController::_setPlannedHomePositionFromFirstCoordinate(const QGeoCoo } } -/// @param clickCoordinate The location of the user click when inserting a new item -void MissionController::_recalcAllWithClickCoordinate(QGeoCoordinate& clickCoordinate) +void MissionController::_recalcAllWithCoordinate(const QGeoCoordinate& coordinate) { if (!_flyView) { - _setPlannedHomePositionFromFirstCoordinate(clickCoordinate); + _setPlannedHomePositionFromFirstCoordinate(coordinate); } _recalcSequence(); _recalcChildItems(); @@ -1692,7 +1689,7 @@ void MissionController::_recalcAllWithClickCoordinate(QGeoCoordinate& clickCoord void MissionController::_recalcAll(void) { QGeoCoordinate emptyCoord; - _recalcAllWithClickCoordinate(emptyCoord); + _recalcAllWithCoordinate(emptyCoord); } /// Initializes a new set of mission items @@ -1817,39 +1814,14 @@ void MissionController::managerVehicleChanged(Vehicle* managerVehicle) connect(_missionManager, &MissionManager::lastCurrentIndexChanged, this, &MissionController::resumeMissionIndexChanged); connect(_missionManager, &MissionManager::resumeMissionReady, this, &MissionController::resumeMissionReady); connect(_missionManager, &MissionManager::resumeMissionUploadFail, this, &MissionController::resumeMissionUploadFail); - connect(_managerVehicle, &Vehicle::homePositionChanged, this, &MissionController::_managerVehicleHomePositionChanged); connect(_managerVehicle, &Vehicle::defaultCruiseSpeedChanged, this, &MissionController::_recalcMissionFlightStatus); connect(_managerVehicle, &Vehicle::defaultHoverSpeedChanged, this, &MissionController::_recalcMissionFlightStatus); connect(_managerVehicle, &Vehicle::vehicleTypeChanged, this, &MissionController::complexMissionItemNamesChanged); - if (!_masterController->offline()) { - _managerVehicleHomePositionChanged(); - } - emit complexMissionItemNamesChanged(); emit resumeMissionIndexChanged(); } -void MissionController::_managerVehicleHomePositionChanged(void) -{ - if (_visualItems) { - bool currentDirtyBit = dirty(); - - MissionSettingsItem* settingsItem = qobject_cast(_visualItems->get(0)); - if (settingsItem) { - settingsItem->setHomePositionFromVehicle(_managerVehicle); - } else { - qWarning() << "First item is not MissionSettingsItem"; - } - - if (!currentDirtyBit) { - // Don't let this trip the dirty bit. Otherwise plan will keep getting marked dirty if vehicle home - // changes. - _visualItems->setDirty(false); - } - } -} - void MissionController::_inProgressChanged(bool inProgress) { emit syncInProgressChanged(inProgress); @@ -1858,7 +1830,7 @@ void MissionController::_inProgressChanged(bool inProgress) bool MissionController::_findPreviousAltitude(int newIndex, double* prevAltitude, int* prevAltitudeMode) { bool found = false; - double foundAltitude; + double foundAltitude = 0; int foundAltitudeMode; if (newIndex > _visualItems->count()) { diff --git a/src/MissionManager/MissionController.h b/src/MissionManager/MissionController.h index bcea44bb549f1301df0723ce7cdbd3da22d7a0fe..310372060da80157ae7fadd0a0cf5ce531af934b 100644 --- a/src/MissionManager/MissionController.h +++ b/src/MissionManager/MissionController.h @@ -260,7 +260,6 @@ signals: private slots: void _newMissionItemsAvailableFromVehicle(bool removeAllRequested); void _itemCommandChanged(void); - void _managerVehicleHomePositionChanged(void); void _inProgressChanged(bool inProgress); void _currentMissionIndexChanged(int sequenceNumber); void _recalcWaypointLines(void); @@ -278,7 +277,7 @@ private: void _init(void); void _recalcSequence(void); void _recalcChildItems(void); - void _recalcAllWithClickCoordinate(QGeoCoordinate& clickCoordinate); + void _recalcAllWithCoordinate(const QGeoCoordinate& coordinate); void _initAllVisualItems(void); void _deinitAllVisualItems(void); void _initVisualItem(VisualMissionItem* item); @@ -308,7 +307,7 @@ private: CoordinateVector* _addWaypointLineSegment(CoordVectHashTable& prevItemPairHashTable, VisualItemPair& pair); void _addTimeDistance(bool vtolInHover, double hoverTime, double cruiseTime, double extraTime, double distance, int seqNum); VisualMissionItem* _insertSimpleMissionItemWorker(QGeoCoordinate coordinate, MAV_CMD command, int visualItemIndex, bool makeCurrentItem); - void _insertComplexMissionItemWorker(ComplexMissionItem* complexItem, int visualItemIndex, bool makeCurrentItem); + void _insertComplexMissionItemWorker(const QGeoCoordinate& mapCenterCoordinate, ComplexMissionItem* complexItem, int visualItemIndex, bool makeCurrentItem); void _warnIfTerrainFrameUsed(void); private: diff --git a/src/MissionManager/TakeoffMissionItem.cc b/src/MissionManager/TakeoffMissionItem.cc index 5faa12316a74b4f089377890761ba3cfcb80f9b3..eb448fe8fae7e169c371866c18463e2346246908 100644 --- a/src/MissionManager/TakeoffMissionItem.cc +++ b/src/MissionManager/TakeoffMissionItem.cc @@ -50,10 +50,9 @@ void TakeoffMissionItem::_init(void) { _editorQml = QStringLiteral("qrc:/qml/SimpleItemEditor.qml"); - if (_settingsItem->coordinate().isValid()) { - // Either the user has set a Launch location or it came from a connected vehicle. - // Use it as starting point. - setCoordinate(_settingsItem->coordinate()); + QGeoCoordinate homePosition = _vehicle->homePosition(); + if (homePosition.isValid()) { + _settingsItem->setCoordinate(homePosition); } connect(_settingsItem, &MissionSettingsItem::coordinateChanged, this, &TakeoffMissionItem::launchCoordinateChanged); @@ -76,19 +75,11 @@ void TakeoffMissionItem::setLaunchTakeoffAtSameLocation(bool launchTakeoffAtSame void TakeoffMissionItem::setCoordinate(const QGeoCoordinate& coordinate) { - if (this->coordinate().isValid() || !_vehicle->fixedWing()) { - if (coordinate != this->coordinate()) { - if (_launchTakeoffAtSameLocation) { - setLaunchCoordinate(coordinate); - } - SimpleMissionItem::setCoordinate(coordinate); - } - } else { - // First time setup for fixed wing - if (!launchCoordinate().isValid()) { - setLaunchCoordinate(coordinate); + if (coordinate != this->coordinate()) { + SimpleMissionItem::setCoordinate(coordinate); + if (_launchTakeoffAtSameLocation) { + _settingsItem->setCoordinate(coordinate); } - SimpleMissionItem::setCoordinate(launchCoordinate().atDistanceAndAzimuth(60, 0)); } } @@ -99,10 +90,22 @@ bool TakeoffMissionItem::isTakeoffCommand(MAV_CMD command) void TakeoffMissionItem::_initLaunchTakeoffAtSameLocation(void) { - if (_vehicle->fixedWing()) { - setLaunchTakeoffAtSameLocation(!specifiesCoordinate()); + if (specifiesCoordinate()) { + if (_vehicle->fixedWing()) { + setLaunchTakeoffAtSameLocation(false); + } else { + // PX4 specifies a coordinate for takeoff even for non fixed wing. But it makes more sense to not have a coordinate + // from and end user standpoint. So even for PX4 we try to keep launch and takeoff at the same position. Unless the + // user has moved/loaded launch at a different location than takeoff. + if (coordinate().isValid() && _settingsItem->coordinate().isValid()) { + setLaunchTakeoffAtSameLocation(coordinate().latitude() == _settingsItem->coordinate().latitude() && coordinate().longitude() == _settingsItem->coordinate().longitude()); + } else { + setLaunchTakeoffAtSameLocation(true); + } + + } } else { - setLaunchTakeoffAtSameLocation(coordinate().latitude() == _settingsItem->coordinate().latitude() && coordinate().longitude() == _settingsItem->coordinate().longitude()); + setLaunchTakeoffAtSameLocation(true); } } @@ -123,3 +126,33 @@ bool TakeoffMissionItem::load(const QJsonObject& json, int sequenceNumber, QStri } return success; } + +void TakeoffMissionItem::setLaunchCoordinate(const QGeoCoordinate& launchCoordinate) +{ + if (!launchCoordinate.isValid()) { + return; + } + + _settingsItem->setCoordinate(launchCoordinate); + + if (!coordinate().isValid()) { + QGeoCoordinate takeoffCoordinate; + if (_launchTakeoffAtSameLocation) { + takeoffCoordinate = launchCoordinate; + } else { + double altitude = this->altitude()->rawValue().toDouble(); + double distance = 0.0; + + if (coordinateHasRelativeAltitude()) { + // Offset for fixed wing climb out of 30 degrees + if (altitude != 0.0) { + distance = altitude / tan(qDegreesToRadians(30.0)); + } + } else { + distance = altitude * 1.5; + } + takeoffCoordinate = launchCoordinate.atDistanceAndAzimuth(distance, 0); + } + SimpleMissionItem::setCoordinate(takeoffCoordinate); + } +} diff --git a/src/MissionManager/TakeoffMissionItem.h b/src/MissionManager/TakeoffMissionItem.h index f42617d45ff87419dce65778808805408c013ec9..3b2b1991c642296bc6e683457df366ba4609b129 100644 --- a/src/MissionManager/TakeoffMissionItem.h +++ b/src/MissionManager/TakeoffMissionItem.h @@ -29,7 +29,7 @@ public: QGeoCoordinate launchCoordinate (void) const { return _settingsItem->coordinate(); } bool launchTakeoffAtSameLocation (void) const { return _launchTakeoffAtSameLocation; } - void setLaunchCoordinate (const QGeoCoordinate& launchCoordinate) { _settingsItem->setCoordinate(launchCoordinate); } + void setLaunchCoordinate (const QGeoCoordinate& launchCoordinate); void setLaunchTakeoffAtSameLocation (bool launchTakeoffAtSameLocation); static bool isTakeoffCommand(MAV_CMD command); diff --git a/src/PlanView/FWLandingPatternMapVisual.qml b/src/PlanView/FWLandingPatternMapVisual.qml index 22d85921959a23280a0819886880f99db604dc1c..5dedcdec767b5b1c76e93587548c8b92bd123d57 100644 --- a/src/PlanView/FWLandingPatternMapVisual.qml +++ b/src/PlanView/FWLandingPatternMapVisual.qml @@ -179,6 +179,8 @@ Item { anchors.fill: map z: QGroundControl.zOrderMapItems + 1 // Over item indicators + readonly property int _decimalPlaces: 8 + onClicked: { var coordinate = map.toCoordinate(Qt.point(mouse.x, mouse.y), false /* clipToViewPort */) coordinate.latitude = coordinate.latitude.toFixed(_decimalPlaces) diff --git a/src/PlanView/SimpleItemEditor.qml b/src/PlanView/SimpleItemEditor.qml index a94b26de73fe3a4a509b01c0f14a51bede02a8bd..d904ffd4d4f3fe4e93b09f4e14ca5d7577f29bd4 100644 --- a/src/PlanView/SimpleItemEditor.qml +++ b/src/PlanView/SimpleItemEditor.qml @@ -63,24 +63,29 @@ Rectangle { anchors.left: parent.left anchors.right: parent.right spacing: _margin - visible: missionItem.wizardMode + visible: missionItem.isTakeoffItem && missionItem.wizardMode // Hack special case for takeoff item QGCLabel { - text: qsTr("Adjust the initial launch location by dragging 'L' indicator to the desired location.") + id: initialClickLabel + text: missionItem.launchTakeoffAtSameLocation ? + qsTr("Click in map to set Takeoff location.") : + qsTr("Click in map to set Launch location.") Layout.fillWidth: true wrapMode: Text.WordWrap - visible: !missionItem.launchTakeoffAtSameLocation + visible: missionItem.isTakeoffItem && !missionItem.launchCoordinate.isValid } QGCLabel { - text: qsTr("Adjust the takeoff %1 location by dragging 'T' indicator to the desired location.").arg(missionItem.launchTakeoffAtSameLocation ? "" : qsTr("completion ")) + text: qsTr("Adjust the takeoff completion location by dragging 'T' indicator to the desired location.") Layout.fillWidth: true wrapMode: Text.WordWrap + visible: !initialClickLabel.visible } QGCButton { text: qsTr("Done Adjusting") Layout.fillWidth: true + visible: !initialClickLabel.visible onClicked: { missionItem.wizardMode = false editorRoot.selectNextNotReadyItem() diff --git a/src/PlanView/TakeoffItemMapVisual.qml b/src/PlanView/TakeoffItemMapVisual.qml index 6c38cbeee0b2fca6044178c38950deb05446f49a..9c1fea3ff75178dd7e0902bc0845b28aa98b42f6 100644 --- a/src/PlanView/TakeoffItemMapVisual.qml +++ b/src/PlanView/TakeoffItemMapVisual.qml @@ -46,12 +46,16 @@ Item { QGCDynamicObjectManager { id: _objMgrCommonVisuals } QGCDynamicObjectManager { id: _objMgrEditingVisuals } + QGCDynamicObjectManager { id: _objMgrMouseClick } Component.onCompleted: { addCommonVisuals() if (_missionItem.isCurrentItem && map.planView) { addEditingVisuals() } + if (!_missionItem.launchCoordinate.isValid) { + _objMgrMouseClick.createObject(mouseAreaClickComponent, map, false /* addToMap */) + } } Connections { @@ -127,4 +131,28 @@ Item { } } } + + // Mouse area to capture launch location click + Component { + id: mouseAreaClickComponent + + MouseArea { + anchors.fill: map + z: QGroundControl.zOrderMapItems + 1 // Over item indicators + + readonly property int _decimalPlaces: 8 + + onClicked: { + var coordinate = map.toCoordinate(Qt.point(mouse.x, mouse.y), false /* clipToViewPort */) + coordinate.latitude = coordinate.latitude.toFixed(_decimalPlaces) + coordinate.longitude = coordinate.longitude.toFixed(_decimalPlaces) + coordinate.altitude = coordinate.altitude.toFixed(_decimalPlaces) + _missionItem.launchCoordinate = coordinate + if (_missionItem.launchTakeoffAtSameLocation) { + _missionItem.wizardMode = false + } + _objMgrMouseClick.destroyObjects() + } + } + } }