From e46e65e1b16bfed953b12c6a9b9dc066a7311818 Mon Sep 17 00:00:00 2001 From: Valentin Platzgummer Date: Thu, 17 Oct 2019 12:21:33 +0200 Subject: [PATCH] flight view gui modified, added additional features to wimaController --- src/FlightDisplay/FlightDisplayView.qml | 36 +++--- src/FlightDisplay/FlightDisplayWimaMenu.qml | 63 ++++++++++- src/Wima/WimaController.cc | 116 ++++++++++++++------ src/Wima/WimaController.h | 6 + src/Wima/WimaPlaner.cc | 6 +- 5 files changed, 169 insertions(+), 58 deletions(-) diff --git a/src/FlightDisplay/FlightDisplayView.qml b/src/FlightDisplay/FlightDisplayView.qml index 6a4e25ceb..efe9ec0ab 100644 --- a/src/FlightDisplay/FlightDisplayView.qml +++ b/src/FlightDisplay/FlightDisplayView.qml @@ -510,25 +510,31 @@ QGCView { } } -// FlightDisplayViewWidgets { -// id: flightDisplayViewWidgets -// z: _panel.z + 4 -// height: ScreenTools.availableHeight - (singleMultiSelector.visible ? singleMultiSelector.height + _margins : 0) -// anchors.left: parent.left -// anchors.right: altitudeSlider.visible ? altitudeSlider.left : parent.right -// anchors.bottom: parent.bottom -// qgcView: root -// useLightColors: isBackgroundDark -// missionController: _missionController -// visible: singleVehicleView.checked && !QGroundControl.videoManager.fullScreen -// } - - FlightDisplayWimaMenu { - id: wimaMenu + FlightDisplayViewWidgets { + id: flightDisplayViewWidgets z: _panel.z + 4 + height: ScreenTools.availableHeight - (singleMultiSelector.visible ? singleMultiSelector.height + _margins : 0) anchors.left: parent.left anchors.right: altitudeSlider.visible ? altitudeSlider.left : parent.right anchors.bottom: parent.bottom + qgcView: root + useLightColors: isBackgroundDark + missionController: _missionController + visible: singleVehicleView.checked && !QGroundControl.videoManager.fullScreen + } + + FlightDisplayWimaMenu { + id: wimaMenu + z: _panel.z + 4 + anchors.left: parent.left + anchors.bottom: parent.bottom + anchors.leftMargin: ScreenTools.defaultFontPixelHeight * 0.25 + anchors.bottomMargin: ScreenTools.defaultFontPixelHeight * 0.25 + height: 300 + width: 300 + + + wimaController: wimaController } //------------------------------------------------------------------------- diff --git a/src/FlightDisplay/FlightDisplayWimaMenu.qml b/src/FlightDisplay/FlightDisplayWimaMenu.qml index 1d90c0f09..776a28ef8 100644 --- a/src/FlightDisplay/FlightDisplayWimaMenu.qml +++ b/src/FlightDisplay/FlightDisplayWimaMenu.qml @@ -18,15 +18,70 @@ import QGroundControl.Airmap 1.0 Item { id: _root + property var wimaController // must be provided by the user + + // box containing all items Rectangle { - anchors.top: parent.top - color: "white" - height: 100 - width: 200 + anchors.left: parent.left + anchors.bottom: parent.bottom + height: enableWima.checked ? parent.height : enableWima.height + width: enableWima.checked ? parent.width : enableWima.width + color: "black" + // checkbox to enable/ disable wima QGCCheckBox { id: enableWima + checked: true + anchors.horizontalCenter: parent.horizontalCenter + anchors.top: parent.top text: qsTr("WiMA") } + + // horizonal line + Rectangle { + id: horizontalLine + anchors.left: parent.left + anchors.right: parent.right + anchors.top: enableWima.bottom + anchors.leftMargin: ScreenTools.defaultFontPixelHeight * 0.5 + anchors.rightMargin: ScreenTools.defaultFontPixelHeight * 0.5 + anchors.topMargin: ScreenTools.defaultFontPixelHeight * 0.25 + height: 1 + + color: "white" + } + + ColumnLayout { + id : mainColumn + anchors.left: parent.left + anchors.right: parent.right + anchors.top: horizontalLine.bottom + anchors.bottom: parent.bottom + anchors.topMargin: ScreenTools.defaultFontPixelHeight * 0.5 + anchors.bottomMargin: ScreenTools.defaultFontPixelHeight * 0.5 + anchors.rightMargin: ScreenTools.defaultFontPixelHeight * 0.5 + anchors.leftMargin: ScreenTools.defaultFontPixelHeight * 0.5 + spacing: ScreenTools.defaultFontPixelHeight * 0.5 + + QGCButton { + id: buttonNextMissionPhase + text: "Next Phase" + onClicked: wimaController.nextPhase(); + } + + QGCButton { + id: buttonResetPhase + text: "Reset Phase" + onClicked: wimaController.resetPhase(); + } + + QGCButton { + id: buttonUpload + text: "Upload" + onClicked: wimaController.uploadToVehicle(); + } + } + + } } diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc index 435f93909..1124b98bd 100644 --- a/src/Wima/WimaController.cc +++ b/src/Wima/WimaController.cc @@ -103,6 +103,22 @@ void WimaController::nextPhase() updateCurrentPath(); } +void WimaController::previousPhase() +{ + +} + +void WimaController::resetPhase() +{ + _startWaypointIndex = 1; + nextPhase(); +} + +void WimaController::uploadToVehicle() +{ + +} + void WimaController::startMission() { @@ -294,18 +310,42 @@ void WimaController::containerDataValidChanged(bool valid) // create mission items _missionController->removeAll(); QmlObjectListModel* missionControllerVisualItems = _missionController->visualItems(); - bool copyON = false; - for ( int i = 0; i < tempMissionItems.size(); i++) { + + + // find takeoff command + int begin = -1; + for (int i = 0; i < tempMissionItems.size(); i++) { const MissionItem *missionItem = tempMissionItems[i]; - if (copyON || missionItem->command() == MAV_CMD_NAV_VTOL_TAKEOFF - || missionItem->command() == MAV_CMD_NAV_TAKEOFF) { + if ( missionItem->command() == MAV_CMD_NAV_VTOL_TAKEOFF + || missionItem->command() == MAV_CMD_NAV_TAKEOFF) { + _missionController->insertSimpleMissionItem(*missionItem, missionControllerVisualItems->count()); - copyON = true; + + QGeoCoordinate coordinate = missionItem->coordinate(); + _takeoffLandPostion.setAltitude(coordinate.altitude()); + _takeoffLandPostion.setLatitude(coordinate.latitude()); + _takeoffLandPostion.setLongitude(coordinate.longitude()); + + begin = i + 1; + break; } + } + + // check if takeoff command found + if (begin < 0) { + qWarning("WimaController::containerDataValidChanged(): No takeoff found."); + return; + } + + // copy mission items and create SimpleMissionItem by using _missionController + for ( int i = begin; i < tempMissionItems.size(); i++) { + const MissionItem *missionItem = tempMissionItems[i]; + _missionController->insertSimpleMissionItem(*missionItem, missionControllerVisualItems->count()); if ( missionItem->command() == MAV_CMD_NAV_VTOL_LAND - || missionItem->command() == MAV_CMD_NAV_LAND) - break; + || missionItem->command() == MAV_CMD_NAV_LAND) + break; + } @@ -327,15 +367,14 @@ void WimaController::containerDataValidChanged(bool valid) _missionItems.append(visualItemCopy); } - updateWaypointPath(); + if (areaCounter == numAreas) + _localPlanDataValid = true; + updateWaypointPath(); _startWaypointIndex = 1; updateCurrentMissionItems(); updateCurrentPath(); - if (areaCounter == numAreas) - _localPlanDataValid = true; - } else { _localPlanDataValid = false; _visualItems.clear(); @@ -353,17 +392,12 @@ void WimaController::containerDataValidChanged(bool valid) void WimaController::updateCurrentMissionItems() { + if (_missionItems.count() < 1 || !_localPlanDataValid) + return; + int numberWaypoints = 30; // the number of waypoints currentMissionItems must not exceed int overlapping = 2; // number of overlapping waypoints of consecutive mission phases - SimpleMissionItem *homeItem = _missionItems.value(0); - if (homeItem == nullptr) { - qWarning("WimaController::updateCurrentMissionItems(): nullptr"); - _currentMissionItems.clear(); - return; - } - QGeoCoordinate homeCoordinate(homeItem->coordinate()); - QList geoCoordinateList; // list with potential waypoints (from _missionItems), for _currentMissionItems _endWaypointIndex = std::min(_startWaypointIndex + numberWaypoints - 1, _missionItems.count()-2); // -2 -> last item is land item if (!extractCoordinateList(_missionItems, geoCoordinateList, _startWaypointIndex, _endWaypointIndex)) { @@ -375,7 +409,7 @@ void WimaController::updateCurrentMissionItems() // calculate path from home to first waypoint QList path; - if ( !calcShortestPath(homeCoordinate, geoCoordinateList[0], path) ) { + if ( !calcShortestPath(_takeoffLandPostion, geoCoordinateList[0], path) ) { qWarning("WimaController::updateCurrentMissionItems(): Not able to calc path from home to first waypoint."); _currentMissionItems.clear(); return; @@ -386,7 +420,7 @@ void WimaController::updateCurrentMissionItems() // calculate path from last waypoint to home path.clear(); - if ( !calcShortestPath(geoCoordinateList.last(), homeCoordinate, path) ) { + if ( !calcShortestPath(geoCoordinateList.last(), _takeoffLandPostion, path) ) { qWarning("WimaController::updateCurrentMissionItems(): Not able to calc path from home to first waypoint."); _currentMissionItems.clear(); return; @@ -394,11 +428,33 @@ void WimaController::updateCurrentMissionItems() path.removeFirst(); // first coordinate already in geoCoordinateList geoCoordinateList.append(path); + // create Mission Items _missionController->removeAll(); QmlObjectListModel* missionControllerVisuals = _missionController->visualItems(); - for (auto coordinate : geoCoordinateList) + + // set homeposition of settingsItem + MissionSettingsItem* settingsItem = missionControllerVisuals->value(0); + if (settingsItem == nullptr) { + qWarning("WimaController::updateCurrentMissionItems(): nullptr"); + _currentMissionItems.clear(); + return; + } + settingsItem->setCoordinate(_takeoffLandPostion); + + for (auto coordinate : geoCoordinateList) { _missionController->insertSimpleMissionItem(coordinate, missionControllerVisuals->count()); + } + + // set homeposition for take off item (somehow not working with insertSimpleMissionItem) + SimpleMissionItem* takeoff = missionControllerVisuals->value(1); + if (takeoff == nullptr) { + qWarning("WimaController::updateCurrentMissionItems(): nullptr"); + _currentMissionItems.clear(); + return; + } + takeoff->setCoordinate(_takeoffLandPostion); + // set land command for last mission item SimpleMissionItem *landItem = missionControllerVisuals->value(missionControllerVisuals->count()-1); @@ -417,14 +473,7 @@ void WimaController::updateCurrentMissionItems() return; } - // copy mission items to _currentMissionItems -// MissionSettingsItem *settingsItem = qobject_cast((*missionControllerVisuals)[0]); -// if (settingsItem == nullptr) { -// qWarning("WimaController::containerDataValidChanged(): Nullptr at MissionSettingsItem!"); -// return; -// } -// _missionItems.append(settingsItem); - + // copy to _currentMissionItems _currentMissionItems.clear(); for ( int i = 1; i < missionControllerVisuals->count(); i++) { SimpleMissionItem *visualItem = missionControllerVisuals->value(i); @@ -433,6 +482,7 @@ void WimaController::updateCurrentMissionItems() _currentMissionItems.clear(); return; } + SimpleMissionItem *visualItemCopy = new SimpleMissionItem(*visualItem, true, this); _currentMissionItems.append(visualItemCopy); } @@ -443,16 +493,14 @@ void WimaController::updateCurrentMissionItems() void WimaController::updateWaypointPath() { _waypointPath.clear(); - if (!extractCoordinateList(_missionItems, _waypointPath, 0, _missionItems.count()-1)) - return; + extractCoordinateList(_missionItems, _waypointPath, 0, _missionItems.count()-1); emit waypointPathChanged(); } void WimaController::updateCurrentPath() { _currentWaypointPath.clear(); - if (!extractCoordinateList(_currentMissionItems, _currentWaypointPath, 0, _currentMissionItems.count()-1)) - return; + extractCoordinateList(_currentMissionItems, _currentWaypointPath, 0, _currentMissionItems.count()-1); emit currentWaypointPathChanged(); } diff --git a/src/Wima/WimaController.h b/src/Wima/WimaController.h index 28b8c1d42..39c4321a5 100644 --- a/src/Wima/WimaController.h +++ b/src/Wima/WimaController.h @@ -69,6 +69,9 @@ public: // Member Methodes Q_INVOKABLE void nextPhase(); + Q_INVOKABLE void previousPhase(); + Q_INVOKABLE void resetPhase(); + Q_INVOKABLE void uploadToVehicle(); Q_INVOKABLE void startMission(); Q_INVOKABLE void abortMission(); Q_INVOKABLE void pauseMission(); @@ -134,6 +137,9 @@ private: QVariantList _waypointPath; // path connecting the items in _missionItems QVariantList _currentWaypointPath; // path connecting the items in _currentMissionItems int _startWaypointIndex; // index to the mission item stored in _missionItems defining the first element of _currentMissionItems + QList _startWaypointIndexList; int _endWaypointIndex; // index to the mission item stored in _missionItems defining the last element of _currentMissionItems + + QGeoCoordinate _takeoffLandPostion; }; diff --git a/src/Wima/WimaPlaner.cc b/src/Wima/WimaPlaner.cc index cac04e064..bb4daaa55 100644 --- a/src/Wima/WimaPlaner.cc +++ b/src/Wima/WimaPlaner.cc @@ -323,11 +323,7 @@ bool WimaPlaner::updateMission() qgcApp()->showMessage(QString(tr("Not able to calculate the path from measurement area to landing position.")).toLocal8Bit().data()); return false; } -// path.clear(); -// path.append(end); -// path.append(start); -// path.append(end); -// path.append(end); + for (int i = 1; i < path.count()-1; i++) { sequenceNumber = _missionController->insertSimpleMissionItem(path.value(i), missionItems->count()); _missionController->setCurrentPlanViewIndex(sequenceNumber, true); -- 2.22.0