diff --git a/qgroundcontrol.qrc b/qgroundcontrol.qrc
index c0ddbc6d91d7de554bc1b78c604cb9e9c7479e2d..da2990b35bfd716a576079d480aabb3da73177f7 100644
--- a/qgroundcontrol.qrc
+++ b/qgroundcontrol.qrc
@@ -228,6 +228,7 @@
src/WimaView/WimaJoinedAreaMapVisual.qml
src/WimaView/WimaCorridorEditor.qml
src/FlightMap/MapItems/WimaPlanMapItems.qml
+ src/PlanView/WimaMissionItemMapVisual.qml
src/Settings/APMMavlinkStreamRate.SettingsGroup.json
diff --git a/src/FlightDisplay/FlightDisplayViewMap.qml b/src/FlightDisplay/FlightDisplayViewMap.qml
index e03096290fff2519fe2a616701829af40ad07230..852af22d07f6749d5c3d69f826671948238e124b 100644
--- a/src/FlightDisplay/FlightDisplayViewMap.qml
+++ b/src/FlightDisplay/FlightDisplayViewMap.qml
@@ -205,7 +205,7 @@ FlightMap {
: object.type === "WimaMeasurementAreaData" ? "green"
: "transparent"
opacity: 0.25
- z: QGroundControl.zOrderTrajectoryLines-1
+ z: QGroundControl.zOrderTrajectoryLines-2
}
}
@@ -214,7 +214,8 @@ FlightMap {
map: flightMap
largeMapView: _mainIsMap
wimaController: flightMap.wimaController
- z: 1000
+ z: QGroundControl.zOrderTrajectoryLines-1
+ color: "gray"
}
// Add trajectory points to the map
diff --git a/src/FlightMap/MapItems/MissionItemIndicator.qml b/src/FlightMap/MapItems/MissionItemIndicator.qml
index d4f3f70d47d4885821e206031427fdea8893ead7..71cbaf99a5744392001d1778995cd22695c4ea2d 100644
--- a/src/FlightMap/MapItems/MissionItemIndicator.qml
+++ b/src/FlightMap/MapItems/MissionItemIndicator.qml
@@ -21,6 +21,7 @@ MapQuickItem {
property var missionItem
property int sequenceNumber
+ property var color
signal clicked
@@ -37,6 +38,7 @@ MapQuickItem {
vehicleYaw: missionItem.missionVehicleYaw
showGimbalYaw: !isNaN(missionItem.missionGimbalYaw)
onClicked: _item.clicked()
+ color: _item.color
property bool _isCurrentItem: missionItem ? missionItem.isCurrentItem : false
}
diff --git a/src/FlightMap/MapItems/WimaPlanMapItems.qml b/src/FlightMap/MapItems/WimaPlanMapItems.qml
index 18dcbd21a17a83f36e5264a549c75a81a7c296e7..15cb9c434ac8296d6e4b54e644c620ce7c654218 100644
--- a/src/FlightMap/MapItems/WimaPlanMapItems.qml
+++ b/src/FlightMap/MapItems/WimaPlanMapItems.qml
@@ -22,6 +22,7 @@ Item {
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 _map: map
property var _missionLineViewComponent
@@ -30,14 +31,15 @@ Item {
Repeater {
model: largeMapView ? wimaController.missionItems : 0
- delegate: MissionItemMapVisual {
+ delegate: WimaMissionItemMapVisual {
map: _map
- onClicked: {
- if (isActiveVehicle) {
- // Only active vehicle supports click to change current mission item
- guidedActionsController.confirmAction(guidedActionsController.actionSetWaypoint, Math.max(object.sequenceNumber, 1))
- }
- }
+ color: _root.color
+// onClicked: {
+// if (isActiveVehicle) {
+// // Only active vehicle supports click to change current mission item
+// guidedActionsController.confirmAction(guidedActionsController.actionSetWaypoint, Math.max(object.sequenceNumber, 1))
+// }
+// }
}
/*onItemAdded: {
@@ -66,9 +68,9 @@ Item {
MapPolyline {
line.width: 3
- line.color: "#be781c" // Hack, can't get palette to work in here
+ line.color: "gray" // Hack, can't get palette to work in here
z: QGroundControl.zOrderWaypointLines
- path: undefined //wimaController.waypointPath
+ path: wimaController.waypointPath
}
}
}
diff --git a/src/MissionManager/SimpleMissionItem.cc b/src/MissionManager/SimpleMissionItem.cc
index 74a9e7f5b6da288e17aed91e53c8c5485a74dbf6..53cacddef8eeec37d0c9e67784440ad388d15c6d 100644
--- a/src/MissionManager/SimpleMissionItem.cc
+++ b/src/MissionManager/SimpleMissionItem.cc
@@ -148,7 +148,7 @@ SimpleMissionItem::SimpleMissionItem(Vehicle* vehicle, bool flyView, const Missi
SimpleMissionItem::SimpleMissionItem(const SimpleMissionItem& other, bool flyView, QObject* parent)
: VisualMissionItem (other, flyView, parent)
- , _missionItem (other._vehicle)
+ , _missionItem (other._missionItem, this)
, _rawEdit (false)
, _dirty (false)
, _ignoreDirtyChangeSignals (false)
diff --git a/src/PlanView/WimaMissionItemMapVisual.qml b/src/PlanView/WimaMissionItemMapVisual.qml
new file mode 100644
index 0000000000000000000000000000000000000000..bd26bb902ad34518268b0af8e5c1c8fff6f0dfcc
--- /dev/null
+++ b/src/PlanView/WimaMissionItemMapVisual.qml
@@ -0,0 +1,118 @@
+/****************************************************************************
+ *
+ * (c) 2009-2016 QGROUNDCONTROL PROJECT
+ *
+ * QGroundControl is licensed according to the terms in the file
+ * COPYING.md in the root of the source code directory.
+ *
+ ****************************************************************************/
+
+import QtQuick 2.3
+import QtQuick.Controls 1.2
+import QtLocation 5.3
+import QtPositioning 5.3
+
+import QGroundControl 1.0
+import QGroundControl.ScreenTools 1.0
+import QGroundControl.Palette 1.0
+import QGroundControl.Controls 1.0
+import QGroundControl.FlightMap 1.0
+
+/// Simple Mission Item visuals
+Item {
+ id: _root
+
+ property var map ///< Map control to place item in
+ property var qgcView ///< QGCView to use for popping dialogs
+ property var color
+
+ property var _missionItem: object
+ property var _itemVisual
+ property var _dragArea
+ property bool _itemVisualShowing: false
+ property bool _dragAreaShowing: false
+
+ signal clicked(int sequenceNumber)
+
+ function hideItemVisuals() {
+ if (_itemVisualShowing) {
+ _itemVisual.destroy()
+ _itemVisualShowing = false
+ }
+ }
+
+ function showItemVisuals() {
+ if (!_itemVisualShowing) {
+ _itemVisual = indicatorComponent.createObject(map)
+ map.addMapItem(_itemVisual)
+ _itemVisualShowing = true
+ }
+ }
+
+ function hideDragArea() {
+ if (_dragAreaShowing) {
+ _dragArea.destroy()
+ _dragAreaShowing = false
+ }
+ }
+
+ function showDragArea() {
+ if (!_dragAreaShowing && _missionItem.specifiesCoordinate) {
+ _dragArea = dragAreaComponent.createObject(map)
+ _dragAreaShowing = true
+ }
+ }
+
+ Component.onCompleted: {
+ showItemVisuals()
+ if (_missionItem.isCurrentItem && map.planView) {
+ showDragArea()
+ }
+ }
+
+ Component.onDestruction: {
+ hideDragArea()
+ hideItemVisuals()
+ }
+
+
+ Connections {
+ target: _missionItem
+
+ onIsCurrentItemChanged: {
+ if (_missionItem.isCurrentItem) {
+ showDragArea()
+ } else {
+ hideDragArea()
+ }
+ }
+ }
+
+ // Control which is used to drag items
+ Component {
+ id: dragAreaComponent
+
+ MissionItemIndicatorDrag {
+ mapControl: _root.map
+ itemIndicator: _itemVisual
+ itemCoordinate: _missionItem.coordinate
+
+ onItemCoordinateChanged: _missionItem.coordinate = itemCoordinate
+ }
+ }
+
+ Component {
+ id: indicatorComponent
+
+ MissionItemIndicator {
+ coordinate: _missionItem.coordinate
+ visible: _missionItem.specifiesCoordinate
+ z: QGroundControl.zOrderMapItems
+ missionItem: _missionItem
+ sequenceNumber: _missionItem.sequenceNumber
+ color: _root.color
+
+ onClicked: _root.clicked(_missionItem.sequenceNumber)
+ }
+ }
+}
diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc
index 64f375f9c978ebfe754550951d20655b375695a1..d8b6bd0c06f17ff21c4f40597284f822be6601d4 100644
--- a/src/Wima/WimaController.cc
+++ b/src/Wima/WimaController.cc
@@ -38,7 +38,7 @@ QStringList WimaController::saveNameFilters() const
return filters;
}
-WimaDataContainer *WimaController::dataContainer() const
+WimaDataContainer *WimaController::dataContainer()
{
return _container;
}
@@ -48,6 +48,11 @@ QmlObjectListModel *WimaController::missionItems()
return &_missionItems;
}
+QVariantList WimaController::waypointPath()
+{
+ return _waypointPath;
+}
+
void WimaController::setMasterController(PlanMasterController *masterC)
{
_masterController = masterC;
@@ -159,14 +164,11 @@ void WimaController::containerDataValidChanged(bool valid)
int areaCounter = 0;
int numAreas = 4; // extract only numAreas Areas, if there are more they are invalid and ignored
- qWarning() << QString("List Size: %1").arg(areaList.size());
for (int i = 0; i < areaList.size(); i++) {
const WimaAreaData *areaData = areaList[i];
- qWarning() << areaData->type();
if (areaData->type() == WimaServiceAreaData::typeString) { // is it a service area?
_serviceArea = *qobject_cast(areaData);
-// qWarning("Service area, wuhuuu!");
areaCounter++;
_visualItems.append(&_serviceArea);
@@ -175,7 +177,6 @@ void WimaController::containerDataValidChanged(bool valid)
if (areaData->type() == WimaMeasurementAreaData::typeString) { // is it a measurement area?
_measurementArea = *qobject_cast(areaData);
-// qWarning("Measurement area, wuhuuu!");
areaCounter++;
_visualItems.append(&_measurementArea);
@@ -184,7 +185,6 @@ void WimaController::containerDataValidChanged(bool valid)
if (areaData->type() == WimaCorridorData::typeString) { // is it a corridor?
_corridor = *qobject_cast(areaData);
-// qWarning("WimaCorridorData, wuhuuu!");
areaCounter++;
//_visualItems.append(&_corridor); // not needed
@@ -193,7 +193,6 @@ void WimaController::containerDataValidChanged(bool valid)
if (areaData->type() == WimaJoinedAreaData::typeString) { // is it a corridor?
_joinedArea = *qobject_cast(areaData);
-// qWarning("WimaJoinedAreaData, wuhuuu!");
areaCounter++;
_visualItems.append(&_joinedArea);
@@ -213,8 +212,9 @@ void WimaController::containerDataValidChanged(bool valid)
_missionController->removeAll();
QmlObjectListModel* missionControllerVisualItems = _missionController->visualItems();
- for ( auto missionItem : tempMissionItems)
+ for ( auto missionItem : tempMissionItems) {
_missionController->insertSimpleMissionItem(*missionItem, missionControllerVisualItems->count());
+ }
MissionSettingsItem *settingsItem = qobject_cast((*missionControllerVisualItems)[0]);
if (settingsItem == nullptr) {
@@ -233,10 +233,7 @@ void WimaController::containerDataValidChanged(bool valid)
_missionItems.append(visualItemCopy);
}
- // remove warnings if you read this
- qWarning("WimaController::containerDataValidChanged(): Mission Item count:");
- qWarning() << _missionItems.count();
-
+ updateWaypointPath();
_missionController->removeAll();
if (areaCounter == numAreas)
@@ -257,6 +254,22 @@ void WimaController::containerDataValidChanged(bool valid)
#endif
}
+void WimaController::updateWaypointPath()
+{
+ _waypointPath.clear();
+
+ for ( int i = 1; i < _missionItems.count(); i++) {
+ SimpleMissionItem *item = qobject_cast(_missionItems[i]);
+ if (item == nullptr) {
+ qWarning("WimaController::updateWaypointPath(): nullptr");
+ return;
+ }
+ _waypointPath.append(QVariant::fromValue(item->coordinate()));
+ }
+
+ emit waypointPathChanged();
+}
+
diff --git a/src/Wima/WimaController.h b/src/Wima/WimaController.h
index 95079d50383eec8a51c42f5d1808f19df180fbf2..ea975fa426d8a60e95ea3b9f82b96dc7d76581da 100644
--- a/src/Wima/WimaController.h
+++ b/src/Wima/WimaController.h
@@ -39,21 +39,23 @@ public:
Q_PROPERTY(QStringList loadNameFilters READ loadNameFilters CONSTANT)
Q_PROPERTY(QStringList saveNameFilters READ saveNameFilters CONSTANT)
Q_PROPERTY(QString fileExtension READ fileExtension CONSTANT)
- Q_PROPERTY(WimaDataContainer* dataContainer READ dataContainer WRITE setDataContainer NOTIFY dataContainerChanged)
+ Q_PROPERTY(WimaDataContainer* dataContainer READ dataContainer WRITE setDataContainer NOTIFY dataContainerChanged)
Q_PROPERTY(QmlObjectListModel* missionItems READ missionItems NOTIFY missionItemsChanged)
+ Q_PROPERTY(QVariantList waypointPath READ waypointPath NOTIFY waypointPathChanged)
// Property accessors
PlanMasterController* masterController (void) { return _masterController; }
MissionController* missionController (void) { return _missionController; }
- QmlObjectListModel* visualItems (void) ;
+ QmlObjectListModel* visualItems (void);
QString currentFile (void) const { return _currentFile; }
QStringList loadNameFilters (void) const;
QStringList saveNameFilters (void) const;
QString fileExtension (void) const { return wimaFileExtension; }
QGCMapPolygon joinedArea (void) const;
- WimaDataContainer* dataContainer (void) const;
+ WimaDataContainer* dataContainer (void);
QmlObjectListModel* missionItems (void);
+ QVariantList waypointPath (void);
// Property setters
@@ -90,9 +92,11 @@ signals:
void dataContainerChanged ();
void readyForSaveSendChanged (bool ready);
void missionItemsChanged (void);
+ void waypointPathChanged (void);
private slots:
void containerDataValidChanged (bool valid);
+ void updateWaypointPath (void);
private:
PlanMasterController *_masterController;
@@ -106,5 +110,6 @@ private:
WimaCorridorData _corridor; // corridor connecting opArea and serArea
bool _localPlanDataValid;
QmlObjectListModel _missionItems; // all mission itmes generaded by wimaPlaner, displayed in flightView
+ QVariantList _waypointPath;
};
diff --git a/src/Wima/WimaPlaner.cc b/src/Wima/WimaPlaner.cc
index 6ff0434e1f44f4757e877a4f999d3ef41b81ac8a..cac04e0644d6e02f8cb676eabf9191e071a5ff0c 100644
--- a/src/Wima/WimaPlaner.cc
+++ b/src/Wima/WimaPlaner.cc
@@ -242,7 +242,6 @@ bool WimaPlaner::updateMission()
qWarning("WimaPlaner::updateMission(): settingsItem == nullptr");
return false;
}
-
// set altitudes, temporary measure to solve bugs
QGeoCoordinate center = _serviceArea.center();
center.setAltitude(0);
@@ -253,15 +252,20 @@ bool WimaPlaner::updateMission()
center = _corridor.center();
center.setAltitude(0);
_corridor.setCenter(center);
-
-
// set HomePos. to serArea center
settingsItem->setCoordinate(_serviceArea.center());
// create take off position item
int sequenceNumber = _missionController->insertSimpleMissionItem(_serviceArea.center(), missionItems->count());
- _missionController->setCurrentPlanViewIndex(sequenceNumber, true);
+ _missionController->setCurrentPlanViewIndex(sequenceNumber, true);
+ SimpleMissionItem* takeOffItem = qobject_cast(missionItems->get(missionItems->count()-1));
+ if (takeOffItem == nullptr){
+ qWarning("WimaPlaner::updateMission(): takeOffItem == nullptr");
+ return false;
+ } else {
+ takeOffItem->setCoordinate(_serviceArea.center()); //necessary, insertSimpleMissionItem does not copy coordinate (why?)
+ }
// create survey item, will be extened with more()-> mission types in the future
_missionController->insertComplexMissionItem(_missionController->circularSurveyComplexItemName(), _measurementArea.center(), missionItems->count());
@@ -280,13 +284,9 @@ bool WimaPlaner::updateMission()
survey->setAutoGenerated(true); // prevents reinitialisation from gui
survey->surveyAreaPolygon()->clear();
survey->surveyAreaPolygon()->appendVertices(_measurementArea.coordinateList());
-
}
-
-
-
// calculate path from take off to opArea
if (survey->visualTransectPoints().size() == 0) {
qWarning("WimaPlaner::updateMission(): survey no points.");
@@ -345,6 +345,8 @@ bool WimaPlaner::updateMission()
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 false;
}
}
@@ -588,26 +590,6 @@ bool WimaPlaner::recalcJoinedArea(QString &errorString)
return false; // this happens if all areas are pairwise disjoint
}
// join service area, op area and corridor
-
-
-// // remove if debugging finished
-// WimaServiceArea *test = new WimaServiceArea(this);
-
-// WimaServiceArea *test1 = new WimaServiceArea(this);
-// WimaServiceArea *test2 = new WimaServiceArea(this);
-// WimaServiceArea *test3 = new WimaServiceArea(this);
-// Circle circle(25, this);
-// using namespace GeoUtilities;
-
-// test->setPath(toGeo(circle.approximateSektor(0.5, 1, 3), _joinedArea.center()));
-// _visualItems.append(test);
-// test1->setPath(toGeo(circle.approximateSektor(0.5, 1, -1), _joinedArea.center()));
-// _visualItems.append(test1);
-// test2->setPath(toGeo(circle.approximateSektor(0.5,-1, 3), _joinedArea.center()));
-// _visualItems.append(test2);
-// test3->setPath(toGeo(circle.approximateSektor(0.5, -1, -3), _joinedArea.center()));
-// _visualItems.append(test3);
-
return true;
}
@@ -682,6 +664,21 @@ WimaPlanData WimaPlaner::toPlanData()
// convert mission items to mavlink commands
QList rgMissionItems;
MissionController::convertToMissionItems(_missionController->visualItems(), rgMissionItems, this);
+
+ // remove warnings if you read this
+// qWarning("WimaPlaner::toPlanData(): rgMissionItems");
+// for (auto item : rgMissionItems) {
+// qWarning() << item->command();
+// qWarning() << item->param1();
+// qWarning() << item->param2();
+// qWarning() << item->param3();
+// qWarning() << item->param4();
+// qWarning() << item->param5();
+// qWarning() << item->param6();
+// qWarning() << item->param7();
+// qWarning(" ");
+// }
+
QList rgMissionItemsConst;
for (auto missionItem : rgMissionItems)
rgMissionItemsConst.append(missionItem);