diff --git a/src/FlightDisplay/FlightDisplayViewMap.qml b/src/FlightDisplay/FlightDisplayViewMap.qml index b7e97f785aee5ef7c6db2904dd4f83e47e13689a..6dfa0590ba60a4671726fb24fd8a6d720d5b88a6 100644 --- a/src/FlightDisplay/FlightDisplayViewMap.qml +++ b/src/FlightDisplay/FlightDisplayViewMap.qml @@ -215,7 +215,6 @@ FlightMap { largeMapView: _mainIsMap wimaController: flightMap.wimaController z: QGroundControl.zOrderTrajectoryLines-1 - color: "#B4808080" // gray with alpha #AARRGGBB } // Add trajectory points to the map diff --git a/src/FlightMap/MapItems/WimaPlanMapItems.qml b/src/FlightMap/MapItems/WimaPlanMapItems.qml index bfbe89cc57754b16f7e7582f901e7ee16d0a5db7..7c9f1ec66ba260e3e60cfa03d504f0e7b8c4d518 100644 --- a/src/FlightMap/MapItems/WimaPlanMapItems.qml +++ b/src/FlightMap/MapItems/WimaPlanMapItems.qml @@ -19,14 +19,17 @@ import QGroundControl.FlightMap 1.0 Item { id: _root - property var map ///< Map control to show items on - property bool largeMapView ///< true: map takes up entire view, false: map is in small window - property var wimaController - property var color - property var lineColor: color + property var map ///< Map control to show items on + property bool largeMapView ///< true: map takes up entire view, false: map is in small window + property var wimaController + property string mIColor: "#B4808080" // Mission Items Color, gray with alpha 0.7 #AARRGGBB + property string mIlineColor: mIColor + property string cMIColor: "orangered" // Current Mission Items Color + property string cMIlineColor: cMIColor property var _map: map property var _missionLineViewComponent + property var _currentMissionLineViewComponent // Add the mission item visuals to the map Repeater { @@ -34,7 +37,7 @@ Item { delegate: WimaMissionItemMapVisual { map: _map - color: _root.color + color: _root.mIColor // onClicked: { // if (isActiveVehicle) { // // Only active vehicle supports click to change current mission item @@ -52,16 +55,32 @@ Item { }*/ } + // Add the current mission item visuals to the map + Repeater { + model: largeMapView ? wimaController.currentMissionItems : 0 + + delegate: WimaMissionItemMapVisual { + map: _map + color: _root.cMIColor + } + } + Component.onCompleted: { _missionLineViewComponent = missionLineViewComponent.createObject(map) if (_missionLineViewComponent.status === Component.Error) console.log(_missionLineViewComponent.errorString()) map.addMapItem(_missionLineViewComponent) + + _currentMissionLineViewComponent = currentMissionLineViewComponent.createObject(map) + if (_currentMissionLineViewComponent.status === Component.Error) + console.log(_currentMissionLineViewComponent.errorString()) + map.addMapItem(_currentMissionLineViewComponent) } Component.onDestruction: { _missionLineViewComponent.destroy() + _currentMissionLineViewComponent.destroy() } Component { @@ -69,9 +88,20 @@ Item { MapPolyline { line.width: 3 - line.color: lineColor // Hack, can't get palette to work in here + line.color: mIlineColor z: QGroundControl.zOrderWaypointLines path: wimaController.waypointPath } } + + Component { + id: currentMissionLineViewComponent + + MapPolyline { + line.width: 3 + line.color: cMIlineColor + z: QGroundControl.zOrderWaypointLines + path: wimaController.currentWaypointPath + } + } } diff --git a/src/Wima/WimaAreaData.cc b/src/Wima/WimaAreaData.cc index e8c8b686f8aa85ba69859391057c97dc00423759..cdf2fbc73e866df194312b49750a50bf55ab8932 100644 --- a/src/Wima/WimaAreaData.cc +++ b/src/Wima/WimaAreaData.cc @@ -26,6 +26,16 @@ QVariantList WimaAreaData::path() const return _path; } +QList WimaAreaData::coordinateList() const +{ + QList coordinateList; + + for ( auto coorinate : _path) + coordinateList.append(coorinate.value()); + + return coordinateList; +} + /*! * \fn void WimaAreaData::setMaxAltitude(double maxAltitude) * diff --git a/src/Wima/WimaAreaData.h b/src/Wima/WimaAreaData.h index 79231757a1701ff5cdcbedf27159954e8fab1995..f3774a73ceae68f2785d83c8eb8bf52487006488 100644 --- a/src/Wima/WimaAreaData.h +++ b/src/Wima/WimaAreaData.h @@ -19,8 +19,9 @@ public: //WimaAreaData(const WimaArea &other, QObject *parent = nullptr); WimaAreaData& operator=(const WimaAreaData& otherData) = delete; // avoid slicing - double maxAltitude() const; - QVariantList path() const; + double maxAltitude() const; + QVariantList path() const; + QList coordinateList() const; virtual QString type() const = 0; diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc index f9fbd676b94803f83669bd2a5971e997be4d475f..66d59914e8eaa7a348343ddc5b3c6ad8cd0d8ae5 100644 --- a/src/Wima/WimaController.cc +++ b/src/Wima/WimaController.cc @@ -12,7 +12,8 @@ WimaController::WimaController(QObject *parent) , _serviceArea (this) , _corridor (this) , _localPlanDataValid (false) - , _firstWaypointIndex (2) // starts with 2, 0 is home position, 1 is takeoff + , _startWaypointIndex (0) + , _endWaypointIndex (0) { } @@ -49,11 +50,21 @@ QmlObjectListModel *WimaController::missionItems() return &_missionItems; } +QmlObjectListModel *WimaController::currentMissionItems() +{ + return &_currentMissionItems; +} + QVariantList WimaController::waypointPath() { return _waypointPath; } +QVariantList WimaController::currentWaypointPath() +{ + return _currentWaypointPath; +} + void WimaController::setMasterController(PlanMasterController *masterC) { _masterController = masterC; @@ -157,12 +168,12 @@ bool WimaController::calcShortestPath(const QGeoCoordinate &start, const QGeoCoo return retVal; } -bool WimaController::extractCoordinateList(const QmlObjectListModel &missionItems, QList &coordinateList) +bool WimaController::extractCoordinateList(QmlObjectListModel &missionItems, QList &coordinateList) { return extractCoordinateList(missionItems, coordinateList, 0, missionItems.count()-1); } -bool WimaController::extractCoordinateList(const QmlObjectListModel &missionItems, QList &coordinateList, int startIndex, int endIndex) +bool WimaController::extractCoordinateList(QmlObjectListModel &missionItems, QList &coordinateList, int startIndex, int endIndex) { if ( startIndex >= 0 && startIndex < missionItems.count() @@ -175,7 +186,7 @@ bool WimaController::extractCoordinateList(const QmlObjectListModel &missionItem return false; } else { for (int i = startIndex; i <= endIndex; i++) { - const MissionItem *mItem = qobject_cast(missionItems[i]); + SimpleMissionItem *mItem = missionItems.value(i); if (mItem == nullptr) { coordinateList.clear(); @@ -190,12 +201,12 @@ bool WimaController::extractCoordinateList(const QmlObjectListModel &missionItem return true; } -bool WimaController::extractCoordinateList(const QmlObjectListModel &missionItems, QVariantList &coordinateList) +bool WimaController::extractCoordinateList(QmlObjectListModel &missionItems, QVariantList &coordinateList) { return extractCoordinateList(missionItems, coordinateList, 0 , missionItems.count()-1); } -bool WimaController::extractCoordinateList(const QmlObjectListModel &missionItems, QVariantList &coordinateList, int startIndex, int endIndex) +bool WimaController::extractCoordinateList(QmlObjectListModel &missionItems, QVariantList &coordinateList, int startIndex, int endIndex) { QList geoCoordintateList; @@ -279,18 +290,22 @@ void WimaController::containerDataValidChanged(bool valid) QList tempMissionItems = planData.missionItems(); + // create mission items _missionController->removeAll(); QmlObjectListModel* missionControllerVisualItems = _missionController->visualItems(); - for ( auto missionItem : tempMissionItems) { + for ( int i = 1; i < tempMissionItems.size(); i++) { + const MissionItem *missionItem = tempMissionItems[i]; _missionController->insertSimpleMissionItem(*missionItem, missionControllerVisualItems->count()); } - MissionSettingsItem *settingsItem = qobject_cast((*missionControllerVisualItems)[0]); - if (settingsItem == nullptr) { - qWarning("WimaController::containerDataValidChanged(): Nullptr at MissionSettingsItem!"); - return; - } - //_missionItems.append(settingsItem); + + // copy mission items to _missionItems +// MissionSettingsItem *settingsItem = qobject_cast((*missionControllerVisualItems)[0]); +// if (settingsItem == nullptr) { +// qWarning("WimaController::containerDataValidChanged(): Nullptr at MissionSettingsItem!"); +// return; +// } +// _missionItems.append(settingsItem); for ( int i = 1; i < missionControllerVisualItems->count(); i++) { SimpleMissionItem *visualItem = qobject_cast((*missionControllerVisualItems)[i]); @@ -303,7 +318,8 @@ void WimaController::containerDataValidChanged(bool valid) } updateWaypointPath(); - _missionController->removeAll(); + updateCurrentMissionItems(); + updateCurrentPath(); if (areaCounter == numAreas) _localPlanDataValid = true; @@ -327,28 +343,94 @@ void WimaController::updateCurrentMissionItems() { int numberWaypoints = 30; // the number of waypoints currentMissionItems must not exceed + SimpleMissionItem *homeItem = _missionItems.value(0); + if (homeItem == nullptr) { + qWarning("WimaController::updateCurrentMissionItems(): nullptr"); + return; + } + QGeoCoordinate homeCoordinate(homeItem->coordinate()); + QList geoCoordinateList; // list with potential waypoints (from _missionItems), for _currentMissionItems - if (!extractCoordinateList(_missionItems, geoCoordinateList, _firstWaypointIndex, - std::min(_firstWaypointIndex + numberWaypoints, _missionItems.count()-2))) // -2 -> last item is land item + if (!extractCoordinateList(_missionItems, geoCoordinateList, _startWaypointIndex, + std::min(_startWaypointIndex + numberWaypoints - 1, _missionItems.count()-2))) // -2 -> last item is land item { qWarning("WimaController::updateCurrentMissionItems(): error on waypoint extraction."); return; } + _currentMissionItems.clear(); + + // calculate path from home to first waypoint + QList path; + if ( !calcShortestPath(homeCoordinate, geoCoordinateList[0], path) ) { + qWarning("WimaController::updateCurrentMissionItems(): Not able to calc path from home to first waypoint."); + return; + } + // prepend to geoCoordinateList + for (int i = path.size()-2; i >= 0; i--) // i = path.size()-2 : last coordinate already in geoCoordinateList + geoCoordinateList.prepend(path[i]); + + // calculate path from last waypoint to home + path.clear(); + if ( !calcShortestPath(geoCoordinateList.last(), homeCoordinate, path) ) { + qWarning("WimaController::updateCurrentMissionItems(): Not able to calc path from home to first waypoint."); + return; + } + path.removeFirst(); // first coordinate already in geoCoordinateList + geoCoordinateList.append(path); + + // create Mission Items + _missionController->removeAll(); + QmlObjectListModel* missionControllerVisuals = _missionController->visualItems(); + for (auto coordinate : geoCoordinateList) + _missionController->insertSimpleMissionItem(coordinate, missionControllerVisuals->count()); + + // set land command for last mission item + SimpleMissionItem *landItem = missionControllerVisuals->value(missionControllerVisuals->count()-1); + if (landItem == nullptr) { + qWarning("WimaController::updateCurrentMissionItems(): nullptr"); + return; + } + // check vehicle type, before setting land command + Vehicle* controllerVehicle = _masterController->controllerVehicle(); + MAV_CMD landCmd = controllerVehicle->vtol() ? MAV_CMD_NAV_VTOL_LAND : MAV_CMD_NAV_LAND; + if (controllerVehicle->firmwarePlugin()->supportedMissionCommands().contains(landCmd)) { + landItem->setCommand(landCmd); + } else + 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); + + for ( int i = 1; i < missionControllerVisuals->count(); i++) { + SimpleMissionItem *visualItem = missionControllerVisuals->value(i); + if (visualItem == nullptr) { + qWarning("WimaController::updateCurrentMissionItems(): Nullptr at SimpleMissionItem!"); + return; + } + SimpleMissionItem *visualItemCopy = new SimpleMissionItem(*visualItem, true, this); + _currentMissionItems.append(visualItemCopy); + } + emit currentMissionItemsChanged(); } void WimaController::updateWaypointPath() { _waypointPath.clear(); - extractCoordinateList(_missionItems, _waypointPath, 1, _missionItems.count()-1); + extractCoordinateList(_missionItems, _waypointPath, 0, _missionItems.count()-1); emit waypointPathChanged(); } void WimaController::updateCurrentPath() { _currentWaypointPath.clear(); - extractCoordinateList(_currentMissionItems, _currentWaypointPath, 1, _currentMissionItems.count()-1); + extractCoordinateList(_currentMissionItems, _currentWaypointPath, 0, _currentMissionItems.count()-1); emit currentWaypointPathChanged(); } diff --git a/src/Wima/WimaController.h b/src/Wima/WimaController.h index ea975fa426d8a60e95ea3b9f82b96dc7d76581da..dcb00852c400f519ca9a3c65cd1a0fc198576263 100644 --- a/src/Wima/WimaController.h +++ b/src/Wima/WimaController.h @@ -41,7 +41,9 @@ public: Q_PROPERTY(QString fileExtension READ fileExtension CONSTANT) Q_PROPERTY(WimaDataContainer* dataContainer READ dataContainer WRITE setDataContainer NOTIFY dataContainerChanged) Q_PROPERTY(QmlObjectListModel* missionItems READ missionItems NOTIFY missionItemsChanged) + Q_PROPERTY(QmlObjectListModel* currentMissionItems READ currentMissionItems NOTIFY currentMissionItemsChanged) Q_PROPERTY(QVariantList waypointPath READ waypointPath NOTIFY waypointPathChanged) + Q_PROPERTY(QVariantList currentWaypointPath READ currentWaypointPath NOTIFY currentWaypointPathChanged) // Property accessors @@ -55,7 +57,9 @@ public: QGCMapPolygon joinedArea (void) const; WimaDataContainer* dataContainer (void); QmlObjectListModel* missionItems (void); + QmlObjectListModel* currentMissionItems (void); QVariantList waypointPath (void); + QVariantList currentWaypointPath (void); // Property setters @@ -84,6 +88,16 @@ public: // Member Methodes QJsonDocument saveToJson(FileType fileType); + bool calcShortestPath(const QGeoCoordinate &start, const QGeoCoordinate &destination, QList &path); + /// extracts the coordinates stored in missionItems (list of MissionItems) and stores them in coordinateList + bool extractCoordinateList(QmlObjectListModel &missionItems, QList &coordinateList); + /// extracts the coordinates (between startIndex and endIndex) stored in missionItems (list of MissionItems) and stores them in coordinateList. + bool extractCoordinateList(QmlObjectListModel &missionItems, QList &coordinateList, int startIndex, int endIndex); + /// extracts the coordinates stored in missionItems (list of MissionItems) and stores them in coordinateList + bool extractCoordinateList(QmlObjectListModel &missionItems, QVariantList &coordinateList); + /// extracts the coordinates (between startIndex and endIndex) stored in missionItems (list of MissionItems) and stores them in coordinateList. + bool extractCoordinateList(QmlObjectListModel &missionItems, QVariantList &coordinateList, int startIndex, int endIndex); + signals: void masterControllerChanged (void); void missionControllerChanged (void); @@ -92,11 +106,16 @@ signals: void dataContainerChanged (); void readyForSaveSendChanged (bool ready); void missionItemsChanged (void); + void currentMissionItemsChanged (void); void waypointPathChanged (void); + void currentWaypointPathChanged (void); private slots: void containerDataValidChanged (bool valid); + void updateCurrentMissionItems (void); void updateWaypointPath (void); + void updateCurrentPath (void); + private: PlanMasterController *_masterController; @@ -109,7 +128,12 @@ private: WimaServiceAreaData _serviceArea; // area for supplying WimaCorridorData _corridor; // corridor connecting opArea and serArea bool _localPlanDataValid; - QmlObjectListModel _missionItems; // all mission itmes generaded by wimaPlaner, displayed in flightView - QVariantList _waypointPath; + QmlObjectListModel _missionItems; // all mission itmes (Mission Items) generaded by wimaPlaner, displayed in flightView + QmlObjectListModel _currentMissionItems; // contains the current mission items, which are a sub set of _missionItems, + // _currentMissionItems contains a number of mission items which can be worked off with a single battery chrage + 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 + int _endWaypointIndex; // index to the mission item stored in _missionItems defining the last element of _currentMissionItems }; diff --git a/src/Wima/WimaPlaner.h b/src/Wima/WimaPlaner.h index 69d1b40be710306a437ca380ceb2b048fac6d24b..e06625d084f2bf41fdc6ca41572415da91a475aa 100644 --- a/src/Wima/WimaPlaner.h +++ b/src/Wima/WimaPlaner.h @@ -109,6 +109,8 @@ public: QJsonDocument saveToJson(FileType fileType); + bool calcShortestPath(const QGeoCoordinate &start, const QGeoCoordinate &destination, QList &path); + // static Members static const char* wimaFileExtension; static const char* areaItemsName; @@ -127,7 +129,6 @@ private slots: void recalcPolygonInteractivity (int index); bool recalcJoinedArea (QString &errorString); void pushToContainer (); - bool calcShortestPath (const QGeoCoordinate &start, const QGeoCoordinate &destination, QList &path); private: // Member Functions