diff --git a/QGCApplication.pro b/QGCApplication.pro index cd9af36b4629a1ea59f200caaa469f9e02538e81..fb0c0999f7dffc98590331cd86c2718a1d1a7cfb 100644 --- a/QGCApplication.pro +++ b/QGCApplication.pro @@ -141,6 +141,7 @@ INCLUDEPATH += \ src/AutoPilotPlugins \ src/comm \ src/FlightDisplay \ + src/FlightMap \ src/input \ src/Joystick \ src/lib/qmapcontrol \ @@ -239,6 +240,7 @@ HEADERS += \ src/comm/UDPLink.h \ src/FlightDisplay/FlightDisplayWidget.h \ src/FlightDisplay/FlightDisplayView.h \ + src/FlightMap/FlightMapSettings.h \ src/GAudioOutput.h \ src/HomePositionManager.h \ src/Joystick/Joystick.h \ @@ -374,6 +376,7 @@ SOURCES += \ src/comm/UDPLink.cc \ src/FlightDisplay/FlightDisplayWidget.cc \ src/FlightDisplay/FlightDisplayView.cc \ + src/FlightMap/FlightMapSettings.cc \ src/GAudioOutput.cc \ src/HomePositionManager.cc \ src/Joystick/Joystick.cc \ @@ -508,40 +511,44 @@ INCLUDEPATH += \ src/qgcunittest HEADERS += \ - src/qgcunittest/FlightGearTest.h \ - src/qgcunittest/MultiSignalSpy.h \ - src/qgcunittest/TCPLinkTest.h \ - src/qgcunittest/TCPLoopBackServer.h \ src/FactSystem/FactSystemTestBase.h \ src/FactSystem/FactSystemTestGeneric.h \ src/FactSystem/FactSystemTestPX4.h \ + src/MissionItemTest.h \ + src/MissionManager/MissionManagerTest.h \ src/qgcunittest/FileDialogTest.h \ + src/qgcunittest/FileManagerTest.h \ + src/qgcunittest/FlightGearTest.h \ src/qgcunittest/LinkManagerTest.h \ src/qgcunittest/MainWindowTest.h \ src/qgcunittest/MavlinkLogTest.h \ src/qgcunittest/MessageBoxTest.h \ + src/qgcunittest/MultiSignalSpy.h \ + src/qgcunittest/PX4RCCalibrationTest.h \ + src/qgcunittest/TCPLinkTest.h \ + src/qgcunittest/TCPLoopBackServer.h \ src/qgcunittest/UnitTest.h \ src/VehicleSetup/SetupViewTest.h \ - src/qgcunittest/FileManagerTest.h \ - src/qgcunittest/PX4RCCalibrationTest.h \ SOURCES += \ - src/qgcunittest/FlightGearTest.cc \ - src/qgcunittest/MultiSignalSpy.cc \ - src/qgcunittest/TCPLinkTest.cc \ - src/qgcunittest/TCPLoopBackServer.cc \ src/FactSystem/FactSystemTestBase.cc \ src/FactSystem/FactSystemTestGeneric.cc \ src/FactSystem/FactSystemTestPX4.cc \ + src/MissionItemTest.cc \ + src/MissionManager/MissionManagerTest.cc \ src/qgcunittest/FileDialogTest.cc \ + src/qgcunittest/FileManagerTest.cc \ + src/qgcunittest/FlightGearTest.cc \ src/qgcunittest/LinkManagerTest.cc \ src/qgcunittest/MainWindowTest.cc \ src/qgcunittest/MavlinkLogTest.cc \ src/qgcunittest/MessageBoxTest.cc \ + src/qgcunittest/MultiSignalSpy.cc \ + src/qgcunittest/PX4RCCalibrationTest.cc \ + src/qgcunittest/TCPLinkTest.cc \ + src/qgcunittest/TCPLoopBackServer.cc \ src/qgcunittest/UnitTest.cc \ src/VehicleSetup/SetupViewTest.cc \ - src/qgcunittest/FileManagerTest.cc \ - src/qgcunittest/PX4RCCalibrationTest.cc \ } # DebugBuild|WindowsDebugAndRelease } # MobileBuild diff --git a/qgroundcontrol.qrc b/qgroundcontrol.qrc index 545f877164c8bb935b8ec2634ea861931d9c5b02..f304cc54b1261f5cc1355bd21976b2032c25595d 100644 --- a/qgroundcontrol.qrc +++ b/qgroundcontrol.qrc @@ -43,6 +43,7 @@ src/AutoPilotPlugins/PX4/Images/PowerComponentBattery_04cell.svg src/AutoPilotPlugins/PX4/Images/PowerComponentBattery_05cell.svg src/AutoPilotPlugins/PX4/Images/PowerComponentBattery_06cell.svg + src/FlightMap/Images/attitudeDial.svg src/FlightMap/Images/attitudeInstrument.svg src//FlightMap/Images/attitudePointer.svg @@ -61,6 +62,10 @@ src/FlightMap/Images/scale_end.png src/FlightMap/Images/airplaneOutline.svg src/FlightMap/Images/airplaneOpaque.svg + src/FlightMap/Images/MapType.svg + src/FlightMap/Images/MapCenter.svg + src/FlightMap/Images/ZoomPlus.svg + src/FlightMap/Images/ZoomMinus.svg src/test.qml @@ -102,6 +107,7 @@ src/QmlControls/MissionItemSummary.qml src/QmlControls/MissionItemEditor.qml src/QmlControls/DropButton.qml + src/QmlControls/QGCCanvas.qml src/VehicleSetup/SetupView.qml diff --git a/src/FlightDisplay/FlightDisplayView.cc b/src/FlightDisplay/FlightDisplayView.cc index 34cfa13e84c0935cbeca98de1dfea13f5b977613..8b19e357ec4a743635991f74e44426d28db4cf2c 100644 --- a/src/FlightDisplay/FlightDisplayView.cc +++ b/src/FlightDisplay/FlightDisplayView.cc @@ -91,19 +91,3 @@ FlightDisplayView::FlightDisplayView(QWidget *parent) FlightDisplayView::~FlightDisplayView() { } - -void FlightDisplayView::saveSetting(const QString &name, const QString& value) -{ - QSettings settings; - QString key(kMainFlightDisplayViewGroup); - key += "/" + name; - settings.setValue(key, value); -} - -QString FlightDisplayView::loadSetting(const QString &name, const QString& defaultValue) -{ - QSettings settings; - QString key(kMainFlightDisplayViewGroup); - key += "/" + name; - return settings.value(key, defaultValue).toString(); -} diff --git a/src/FlightDisplay/FlightDisplayView.h b/src/FlightDisplay/FlightDisplayView.h index e97c290015370a173bd7eaec8ea700aec1fc4080..7fe374b2ed7847c7bedb7be05ef721386d9d9f95 100644 --- a/src/FlightDisplay/FlightDisplayView.h +++ b/src/FlightDisplay/FlightDisplayView.h @@ -38,9 +38,6 @@ public: Q_PROPERTY(bool hasVideo READ hasVideo CONSTANT) - Q_INVOKABLE void saveSetting (const QString &key, const QString& value); - Q_INVOKABLE QString loadSetting (const QString &key, const QString& defaultValue); - #if defined(QGC_GST_STREAMING) bool hasVideo () { return true; } #else diff --git a/src/FlightDisplay/FlightDisplayView.qml b/src/FlightDisplay/FlightDisplayView.qml index 70bc9d13645b30c65cb38f1339bf578267f95c58..d9e5dcfebf920aaf58dbb67214088cb2233e1023 100644 --- a/src/FlightDisplay/FlightDisplayView.qml +++ b/src/FlightDisplay/FlightDisplayView.qml @@ -26,7 +26,9 @@ import QtQuick.Controls 1.3 import QtQuick.Controls.Styles 1.2 import QtQuick.Dialogs 1.2 import QtLocation 5.3 +import QtPositioning 5.2 +import QGroundControl 1.0 import QGroundControl.FlightMap 1.0 import QGroundControl.ScreenTools 1.0 import QGroundControl.Controls 1.0 @@ -66,7 +68,7 @@ Item { property real _airSpeed: _activeVehicle ? _activeVehicle.airSpeed : _defaultAirSpeed property real _climbRate: _activeVehicle ? _activeVehicle.climbRate : _defaultClimbRate - property bool _showMap: getBool(multiVehicleManager.loadSetting(_mapName + _showMapBackgroundKey, "1")) + property bool _showMap: getBool(QGroundControl.flightMapSettings.loadMapSetting(flightMap.mapName, _showMapBackgroundKey, "1")) // Validate _showMap setting Component.onCompleted: _setShowMap(_showMap) @@ -81,7 +83,7 @@ Item { function _setShowMap(showMap) { _showMap = flightDisplay.hasVideo ? showMap : true - multiVehicleManager.saveSetting(_mapName + _showMapBackgroundKey, setBool(_showMap)) + QGroundControl.flightMapSettings.saveMapSetting(flightMap.mapName, _showMapBackgroundKey, setBool(_showMap)) } FlightMap { @@ -92,6 +94,13 @@ Item { longitude: parent._longitude visible: _showMap + // Home position + MissionItemIndicator { + label: "H" + coordinate: (_activeVehicle && _activeVehicle.homePositionAvailable) ? _activeVehicle.homePosition : QtPositioning.coordinate(0, 0) + visible: _activeVehicle ? _activeVehicle.homePositionAvailable : false + } + // Add the vehicles to the map MapItemView { model: multiVehicleManager.vehicles @@ -141,7 +150,6 @@ Item { size: ScreenTools.defaultFontPixelSize * (13.3) heading: _heading active: multiVehicleManager.activeVehicleAvailable - z: flightMap.z + 2 } QGCAttitudeWidget { @@ -152,8 +160,39 @@ Item { rollAngle: _roll pitchAngle: _pitch active: multiVehicleManager.activeVehicleAvailable - z: flightMap.z + 2 } + + DropButton { + id: mapTypeButton + anchors.margins: ScreenTools.defaultFontPixelHeight + anchors.top: parent.top + anchors.right: parent.right + dropDirection: dropDown + buttonImage: "/qmlimages/MapType.svg" + viewportMargins: ScreenTools.defaultFontPixelWidth / 2 + + dropDownComponent: Component { + Row { + spacing: ScreenTools.defaultFontPixelWidth + + Repeater { + model: QGroundControl.flightMapSettings.mapTypes + + QGCButton { + checkable: true + checked: flightMap.mapType == text + text: modelData + + onClicked: { + flightMap.mapType = text + mapTypeButton.hideDropDown() + } + } + } + } + } + } + } // Flight Map QGCVideoBackground { @@ -280,8 +319,6 @@ Item { MenuSeparator { visible: flightDisplay.hasVideo && _showMap } - - Component.onCompleted: flightMap.addMapMenuItems(optionsMenu) } } } diff --git a/src/FlightMap/FlightMap.qml b/src/FlightMap/FlightMap.qml index a764bb8e72e33296b237476db01a873a20152c0e..f7acec53931266d1aa4afd5911ea79bab14fb62e 100644 --- a/src/FlightMap/FlightMap.qml +++ b/src/FlightMap/FlightMap.qml @@ -32,6 +32,7 @@ import QtQuick.Controls 1.3 import QtLocation 5.3 import QtPositioning 5.3 +import QGroundControl 1.0 import QGroundControl.Controls 1.0 import QGroundControl.FlightMap 1.0 import QGroundControl.ScreenTools 1.0 @@ -47,6 +48,7 @@ Map { property real heading: 0 property bool interactive: true property string mapName: 'defaultMap' + property string mapType: QGroundControl.flightMapSettings.mapTypeForMapName(mapName) property alias mapWidgets: controlWidgets property bool isSatelliteMap: false @@ -61,45 +63,20 @@ Map { plugin: Plugin { name: "QGroundControl" } ExclusiveGroup { id: mapTypeGroup } - - // Map type selection MenuItem - Component { - id: menuItemComponent - - MenuItem { - checkable: true - checked: text == _map.activeMapType.name - exclusiveGroup: mapTypeGroup - visible: _map.visible - - onTriggered: setCurrentMap(text) - } - } - - // Set the current map type to the specified type name - function setCurrentMap(name) { + + Component.onCompleted: onMapTypeChanged + + onMapTypeChanged: { + QGroundControl.flightMapSettings.setMapTypeForMapName(mapName, mapType) + var fullMapName = QGroundControl.flightMapSettings.mapProvider + " " + mapType for (var i = 0; i < _map.supportedMapTypes.length; i++) { - if (name === _map.supportedMapTypes[i].name) { + if (fullMapName === _map.supportedMapTypes[i].name) { _map.activeMapType = _map.supportedMapTypes[i] - multiVehicleManager.saveSetting(_map.mapName + "/currentMapType", name); - return; + return } } } - - // Add menu map types to the specified menu and sets the current map type from settings - function addMapMenuItems(menu) { - var savedMapName = multiVehicleManager.loadSetting(_map.mapName + "/currentMapType", "") - - setCurrentMap(savedMapName) - - for (var i = 0; i < _map.supportedMapTypes.length; i++) { - var menuItem = menuItemComponent.createObject() - menuItem.text = _map.supportedMapTypes[i].name - menu.insertItem(menu.items.length, menuItem) - } - } - + /// Map control widgets Column { id: controlWidgets @@ -137,6 +114,7 @@ Map { QGCButton { width: parent._buttonWidth + //iconSource: "/qmlimages/ZoomPlus.svg" text: "+" onClicked: { @@ -152,6 +130,7 @@ Map { QGCButton { width: parent._buttonWidth + //iconSource: "/qmlimages/ZoomMinus.svg" text: "-" onClicked: { diff --git a/src/FlightMap/FlightMapSettings.cc b/src/FlightMap/FlightMapSettings.cc new file mode 100644 index 0000000000000000000000000000000000000000..a27e5978fca1e969e16470795ec051fb5777e6bc --- /dev/null +++ b/src/FlightMap/FlightMapSettings.cc @@ -0,0 +1,140 @@ +/*===================================================================== + + QGroundControl Open Source Ground Control Station + + (c) 2009 - 2015 QGROUNDCONTROL PROJECT + + This file is part of the QGROUNDCONTROL project + + QGROUNDCONTROL is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + QGROUNDCONTROL is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with QGROUNDCONTROL. If not, see . + + ======================================================================*/ + +#include "FlightMapSettings.h" + +#include +#include + +IMPLEMENT_QGC_SINGLETON(FlightMapSettings, FlightMapSettings) + +const char* FlightMapSettings::_defaultMapProvider = "Bing"; // Bing is default since it support full street/satellite/hybrid set +const char* FlightMapSettings::_settingsGroup = "FlightMapSettings"; +const char* FlightMapSettings::_mapProviderKey = "MapProvider"; +const char* FlightMapSettings::_mapTypeKey = "MapType"; + +FlightMapSettings::FlightMapSettings(QObject* parent) + : QObject(parent) + , _mapProvider(_defaultMapProvider) +{ + qmlRegisterUncreatableType ("QGroundControl", 1, 0, "FlightMapSetting", "Reference only"); + + _supportedMapProviders << "Bing" << "Google" << "Open"; + + _loadSettings(); +} + +FlightMapSettings::~FlightMapSettings() +{ + +} + +void FlightMapSettings::_storeSettings(void) +{ + QSettings settings; + + settings.beginGroup(_settingsGroup); + settings.setValue(_mapProviderKey, _supportedMapProviders.contains(_mapProvider) ? _mapProvider : _defaultMapProvider); +} + +void FlightMapSettings::_loadSettings(void) +{ + QSettings settings; + + settings.beginGroup(_settingsGroup); + _mapProvider = settings.value(_mapProviderKey, _defaultMapProvider).toString(); + + if (!_supportedMapProviders.contains(_mapProvider)) { + _mapProvider = _defaultMapProvider; + } + + _setMapTypesForCurrentProvider(); +} + +QString FlightMapSettings::mapProvider(void) +{ + return _mapProvider; +} + +void FlightMapSettings::setMapProvider(const QString& mapProvider) +{ + if (_supportedMapProviders.contains(mapProvider)) { + _mapProvider = mapProvider; + _storeSettings(); + _setMapTypesForCurrentProvider(); + emit mapProviderChanged(mapProvider); + } +} + +void FlightMapSettings::_setMapTypesForCurrentProvider(void) +{ + _mapTypes.clear(); + + if (_mapProvider == "Bing") { + _mapTypes << "Street Map" << "Satellite Map" << "Hybrid Map"; + } else if (_mapProvider == "Google") { + _mapTypes << "Street Map" << "Satellite Map" << "Terrain Map"; + } else if (_mapProvider == "Open") { + _mapTypes << "Street Map"; + } + + emit mapTypesChanged(_mapTypes); +} + +QString FlightMapSettings::mapTypeForMapName(const QString& mapName) +{ + QSettings settings; + + settings.beginGroup(_settingsGroup); + settings.beginGroup(mapName); + settings.beginGroup(_mapProvider); + return settings.value(_mapTypeKey, "Street Map").toString(); +} + +void FlightMapSettings::setMapTypeForMapName(const QString& mapName, const QString& mapType) +{ + QSettings settings; + + settings.beginGroup(_settingsGroup); + settings.beginGroup(mapName); + settings.beginGroup(_mapProvider); + settings.setValue(_mapTypeKey, mapType); +} + +void FlightMapSettings::saveMapSetting (const QString &mapName, const QString& key, const QString& value) +{ + QSettings settings; + + settings.beginGroup(_settingsGroup); + settings.beginGroup(mapName); + settings.setValue(key, value); +} + +QString FlightMapSettings::loadMapSetting (const QString &mapName, const QString& key, const QString& defaultValue) +{ + QSettings settings; + + settings.beginGroup(_settingsGroup); + settings.beginGroup(mapName); + return settings.value(key, defaultValue).toString(); +} diff --git a/src/FlightMap/FlightMapSettings.h b/src/FlightMap/FlightMapSettings.h new file mode 100644 index 0000000000000000000000000000000000000000..8de843e7cc2a80c86df5cbf0db14ab8884caab43 --- /dev/null +++ b/src/FlightMap/FlightMapSettings.h @@ -0,0 +1,79 @@ +/*===================================================================== + +QGroundControl Open Source Ground Control Station + +(c) 2009 - 2015 QGROUNDCONTROL PROJECT + +This file is part of the QGROUNDCONTROL project + + QGROUNDCONTROL is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + QGROUNDCONTROL is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with QGROUNDCONTROL. If not, see . + +======================================================================*/ + +#ifndef FlightMapSettings_H +#define FlightMapSettings_H + +#include "QGCSingleton.h" + +#include + +class FlightMapSettings : public QObject +{ + Q_OBJECT + + DECLARE_QGC_SINGLETON(FlightMapSettings, FlightMapSettings) + +public: + /// mapProvider is either Bing, Google or Open to specify to set of maps available + Q_PROPERTY(QString mapProvider READ mapProvider WRITE setMapProvider NOTIFY mapProviderChanged) + + /// Map types associated with current map provider + Q_PROPERTY(QStringList mapTypes MEMBER _mapTypes NOTIFY mapTypesChanged) + + Q_INVOKABLE QString mapTypeForMapName(const QString& mapName); + Q_INVOKABLE void setMapTypeForMapName(const QString& mapName, const QString& mapType); + + Q_INVOKABLE void saveMapSetting (const QString &mapName, const QString& key, const QString& value); + Q_INVOKABLE QString loadMapSetting (const QString &mapName, const QString& key, const QString& defaultValue); + + // Property accessors + + QString mapProvider(void); + void setMapProvider(const QString& mapProvider); + +signals: + void mapProviderChanged(const QString& mapProvider); + void mapTypesChanged(const QStringList& mapTypes); + +private: + /// @brief All access to FlightMapSettings singleton is through FlightMapSettings::instance + FlightMapSettings(QObject* parent = NULL); + ~FlightMapSettings(); + + void _storeSettings(void); + void _loadSettings(void); + + void _setMapTypesForCurrentProvider(void); + + QString _mapProvider; ///< Current map provider + QStringList _supportedMapProviders; + QStringList _mapTypes; ///< Map types associated with current map provider + + static const char* _defaultMapProvider; + static const char* _settingsGroup; + static const char* _mapProviderKey; + static const char* _mapTypeKey; +}; + +#endif diff --git a/src/FlightMap/Images/MapCenter.svg b/src/FlightMap/Images/MapCenter.svg index fc60536ce3766d40e9b72bb09be731961b3c4cd0..02ca06b79aa2efe6b432a7a5a1652c6237bad937 100644 --- a/src/FlightMap/Images/MapCenter.svg +++ b/src/FlightMap/Images/MapCenter.svg @@ -2,12 +2,12 @@ - + - - - - - + + + + + diff --git a/src/FlightMap/Images/MapType.svg b/src/FlightMap/Images/MapType.svg index 52822bb5889e0e732c481cb5d5637665f4d099ad..19e55e7109f88cc0793804d7eb85d8eeb485a68d 100644 --- a/src/FlightMap/Images/MapType.svg +++ b/src/FlightMap/Images/MapType.svg @@ -2,9 +2,9 @@ - + - - + + diff --git a/src/FlightMap/Images/PiP.svg b/src/FlightMap/Images/PiP.svg new file mode 100644 index 0000000000000000000000000000000000000000..a14ec2bbc73ddfedf32f1d79ceb5dcb007f70d1f --- /dev/null +++ b/src/FlightMap/Images/PiP.svg @@ -0,0 +1,10 @@ + + + + + + + + + diff --git a/src/FlightMap/Images/ZoomMinus.svg b/src/FlightMap/Images/ZoomMinus.svg new file mode 100644 index 0000000000000000000000000000000000000000..62392cdae1fb3f1317211e35d7915e1945ffd86e --- /dev/null +++ b/src/FlightMap/Images/ZoomMinus.svg @@ -0,0 +1,9 @@ + + + + + + + + diff --git a/src/FlightMap/Images/ZoomPlus.svg b/src/FlightMap/Images/ZoomPlus.svg new file mode 100644 index 0000000000000000000000000000000000000000..ed0b1674a6bd082eba55a9bc880c35b11cd3bb71 --- /dev/null +++ b/src/FlightMap/Images/ZoomPlus.svg @@ -0,0 +1,8 @@ + + + + + + + diff --git a/src/HomePositionManager.cc b/src/HomePositionManager.cc index d8ca76e011d501c7a218243e6b79cf04acccde63..5c07db78c234ec23200cf78e5c8163c38483be03 100644 --- a/src/HomePositionManager.cc +++ b/src/HomePositionManager.cc @@ -25,6 +25,7 @@ #include #include #include +#include #include "UAS.h" #include "UASInterface.h" @@ -51,6 +52,8 @@ HomePositionManager::HomePositionManager(QObject* parent) , homeLon(8.549444) , homeAlt(470.0) { + qmlRegisterUncreatableType ("QGroundControl", 1, 0, "HomePositionManager", "Reference only"); + _loadSettings(); } diff --git a/src/MissionEditor/MissionEditor.cc b/src/MissionEditor/MissionEditor.cc index 4b04ade0f4328456f08a772d5fa6d24d2f6a6a6d..565f0a7cea8d5b5e3ddf62fa50e0ef95765b5b88 100644 --- a/src/MissionEditor/MissionEditor.cc +++ b/src/MissionEditor/MissionEditor.cc @@ -104,7 +104,8 @@ int MissionEditor::addMissionItem(QGeoCoordinate coordinate) qWarning() << "addMissionItem called with _canEdit == false"; } - MissionItem * newItem = new MissionItem(this, _missionItems->count(), coordinate); + MissionItem * newItem = new MissionItem(this, _missionItems->count(), coordinate, MAV_CMD_NAV_WAYPOINT); + newItem->setAltitude(30); if (_missionItems->count() == 0) { newItem->setCommand(MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF); } diff --git a/src/MissionEditor/MissionEditor.qml b/src/MissionEditor/MissionEditor.qml index 5ac9325dbf77e8b73338facf29fd57f89ec1888e..2293d27406686315b1c0fcd0b12b5b249e9174a5 100644 --- a/src/MissionEditor/MissionEditor.qml +++ b/src/MissionEditor/MissionEditor.qml @@ -104,7 +104,7 @@ QGCView { anchors.right: mapTypeButton.left anchors.top: mapTypeButton.top dropDirection: dropDown - label: "C" + buttonImage: "/qmlimages/MapCenter.svg" viewportMargins: ScreenTools.defaultFontPixelWidth / 2 dropDownComponent: Component { @@ -114,14 +114,48 @@ QGCView { QGCButton { text: "Home" - onClicked: centerMapButton.hideDropDown() + onClicked: { + centerMapButton.hideDropDown() + editorMap.center = QtPositioning.coordinate(_homePositionCoordinate.latitude, _homePositionCoordinate.longitude) + _showHomePositionManager = true + } } +/* + +This code will need to wait for Qml 5.5 support since Map.visibleRegion is only in Qt 5.5 + QGCButton { text: "All Items" - onClicked: centerMapButton.hideDropDown() + onClicked: { + centerMapButton.hideDropDown() + + // Begin with only the home position in the region + var region = QtPositioning.rectangle(QtPositioning.coordinate(_homePositionCoordinate.latitude, _homePositionCoordinate.longitude), + QtPositioning.coordinate(_homePositionCoordinate.latitude, _homePositionCoordinate.longitude)) + + // Now expand the region to include all mission items + for (var i=0; i<_missionItems.count; i++) { + var missionItem = _missionItems.get(i) + + region.topLeft.latitude = Math.max(missionItem.coordinate.latitude, region.topLeft.latitude) + region.topLeft.longitude = Math.min(missionItem.coordinate.longitude, region.topLeft.longitude) + + region.topRight.latitude = Math.max(missionItem.coordinate.latitude, region.topRight.latitude) + region.topRight.longitude = Math.max(missionItem.coordinate.longitude, region.topRight.longitude) + + region.bottomLeft.latitude = Math.min(missionItem.coordinate.latitude, region.bottomLeft.latitude) + region.bottomLeft.longitude = Math.min(missionItem.coordinate.longitude, region.bottomLeft.longitude) + + region.bottomRight.latitude = Math.min(missionItem.coordinate.latitude, region.bottomRight.latitude) + region.bottomRight.longitude = Math.max(missionItem.coordinate.longitude, region.bottomRight.longitude) + } + + editorMap.visibleRegion = region + } } +*/ } } } @@ -132,29 +166,26 @@ QGCView { anchors.top: parent.top anchors.right: parent.right dropDirection: dropDown - label: "M" + buttonImage: "/qmlimages/MapType.svg" viewportMargins: ScreenTools.defaultFontPixelWidth / 2 dropDownComponent: Component { Row { spacing: ScreenTools.defaultFontPixelWidth - QGCButton { - text: "Street" - - onClicked: mapTypeButton.hideDropDown() - } - - QGCButton { - text: "Satellite" - - onClicked: mapTypeButton.hideDropDown() - } + Repeater { + model: QGroundControl.flightMapSettings.mapTypes - QGCButton { - text: "Hybrid" + QGCButton { + checkable: true + checked: editorMap.mapType == text + text: modelData - onClicked: mapTypeButton.hideDropDown() + onClicked: { + editorMap.mapType = text + mapTypeButton.hideDropDown() + } + } } } } diff --git a/src/MissionItem.cc b/src/MissionItem.cc index f633bb357e3a377027fc2151a39924c397fbd260..eb47d3d8f67e9306889ac233bb36515a22269816 100644 --- a/src/MissionItem.cc +++ b/src/MissionItem.cc @@ -67,16 +67,17 @@ const MissionItem::MavCmd2Name_t MissionItem::_rgMavCmd2Name[_cMavCmd2Name] = { MissionItem::MissionItem(QObject* parent, int sequenceNumber, QGeoCoordinate coordinate, + int command, double param1, double param2, double param3, double param4, bool autocontinue, bool isCurrentItem, - int frame, - int command) + int frame) : QObject(parent) , _sequenceNumber(sequenceNumber) + , _frame(-1) // Forces set of _altitudeRelativeToHomeFact , _command((MavlinkQmlSingleton::Qml_MAV_CMD)command) , _autocontinue(autocontinue) , _isCurrentItem(isCurrentItem) @@ -219,7 +220,7 @@ void MissionItem::save(QTextStream &saveStream) position = position.arg(y(), 0, 'g', 18); position = position.arg(z(), 0, 'g', 18); QString parameters("%1\t%2\t%3\t%4"); - parameters = parameters.arg(param2(), 0, 'g', 18).arg(param2(), 0, 'g', 18).arg(loiterOrbitRadius(), 0, 'g', 18).arg(yawRadians(), 0, 'g', 18); + parameters = parameters.arg(param1(), 0, 'g', 18).arg(param2(), 0, 'g', 18).arg(loiterOrbitRadius(), 0, 'g', 18).arg(yawRadians(), 0, 'g', 18); // FORMAT: // as documented here: http://qgroundcontrol.org/waypoint_protocol saveStream << this->sequenceNumber() << "\t" << this->isCurrentItem() << "\t" << this->frame() << "\t" << this->command() << "\t" << parameters << "\t" << position << "\t" << this->autoContinue() << "\r\n"; //"\t" << this->getDescription() << "\r\n"; @@ -280,7 +281,7 @@ void MissionItem::setZ(double z) void MissionItem::setLatitude(double lat) { - if (_latitudeFact->value().toDouble() != lat && ((_frame == MAV_FRAME_GLOBAL) || (_frame == MAV_FRAME_GLOBAL_RELATIVE_ALT))) + if (_latitudeFact->value().toDouble() != lat) { _latitudeFact->setValue(lat); emit changed(this); @@ -290,7 +291,7 @@ void MissionItem::setLatitude(double lat) void MissionItem::setLongitude(double lon) { - if (_longitudeFact->value().toDouble() != lon && ((_frame == MAV_FRAME_GLOBAL) || (_frame == MAV_FRAME_GLOBAL_RELATIVE_ALT))) + if (_longitudeFact->value().toDouble() != lon) { _longitudeFact->setValue(lon); emit changed(this); @@ -300,7 +301,7 @@ void MissionItem::setLongitude(double lon) void MissionItem::setAltitude(double altitude) { - if (_altitudeFact->value().toDouble() != altitude && ((_frame == MAV_FRAME_GLOBAL) || (_frame == MAV_FRAME_GLOBAL_RELATIVE_ALT))) + if (_altitudeFact->value().toDouble() != altitude) { _altitudeFact->setValue(altitude); emit changed(this); @@ -349,7 +350,7 @@ int MissionItem::frame(void) const void MissionItem::setFrame(int /*MAV_FRAME*/ frame) { if (_frame != frame) { - _altitudeRelativeToHomeFact->setValue(_frame == MAV_FRAME_GLOBAL_RELATIVE_ALT); + _altitudeRelativeToHomeFact->setValue(frame == MAV_FRAME_GLOBAL_RELATIVE_ALT); _frame = frame; emit changed(this); } diff --git a/src/MissionItem.h b/src/MissionItem.h index aae348b95c8dfaa5a0923ec093c2c8b1bce6ceca..46610a79421cf6f1bcc7276a05eb0a4372e797b7 100644 --- a/src/MissionItem.h +++ b/src/MissionItem.h @@ -47,14 +47,14 @@ public: MissionItem(QObject *parent = 0, int sequenceNumber = 0, QGeoCoordinate coordiante = QGeoCoordinate(), + int action = MAV_CMD_NAV_WAYPOINT, double param1 = 0.0, double param2 = 0.0, double param3 = 0.0, double param4 = 0.0, bool autocontinue = true, bool isCurrentItem = false, - int frame = MAV_FRAME_GLOBAL, - int action = MAV_CMD_NAV_WAYPOINT); + int frame = MAV_FRAME_GLOBAL_RELATIVE_ALT); MissionItem(const MissionItem& other, QObject* parent = NULL); ~MissionItem(); diff --git a/src/MissionItemTest.cc b/src/MissionItemTest.cc new file mode 100644 index 0000000000000000000000000000000000000000..a1ee32afa723e40cec99b930b56cc6df1e34ac0a --- /dev/null +++ b/src/MissionItemTest.cc @@ -0,0 +1,182 @@ +/*===================================================================== + + QGroundControl Open Source Ground Control Station + + (c) 2009 - 2014 QGROUNDCONTROL PROJECT + + This file is part of the QGROUNDCONTROL project + + QGROUNDCONTROL is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + QGROUNDCONTROL is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with QGROUNDCONTROL. If not, see . + + ======================================================================*/ + +#include "MissionItemTest.h" + +UT_REGISTER_TEST(MissionItemTest) + +const MissionItemTest::ItemInfo_t MissionItemTest::_rgItemInfo[] = { + { 1, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_NAV_WAYPOINT, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_GLOBAL_RELATIVE_ALT }, + { 1, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_NAV_LOITER_UNLIM, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_GLOBAL_RELATIVE_ALT }, + { 1, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_NAV_LOITER_TURNS, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_GLOBAL_RELATIVE_ALT }, + { 1, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_NAV_LOITER_TIME, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_GLOBAL_RELATIVE_ALT }, + { 1, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_NAV_LAND, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_GLOBAL_RELATIVE_ALT }, + { 1, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_NAV_TAKEOFF, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_GLOBAL_RELATIVE_ALT }, + { 1, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_CONDITION_DELAY, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_MISSION }, + { 1, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_DO_JUMP, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_MISSION }, +}; + +const MissionItemTest::FactValue_t MissionItemTest::_rgFactValuesWaypoint[] = { + { "Latitude:", -10.0 }, + { "Longitude:", -20.0 }, + { "Altitude:", -30.0 }, + { "Heading:", 40.0 }, + { "Radius:", 20.0 }, + { "Hold:", 10.0 }, +}; + +const MissionItemTest::FactValue_t MissionItemTest::_rgFactValuesLoiterUnlim[] = { + { "Latitude:", -10.0 }, + { "Longitude:", -20.0 }, + { "Altitude:", -30.0 }, + { "Heading:", 40.0 }, + { "Radius:", 30.0 }, +}; + +const MissionItemTest::FactValue_t MissionItemTest::_rgFactValuesLoiterTurns[] = { + { "Latitude:", -10.0 }, + { "Longitude:", -20.0 }, + { "Altitude:", -30.0 }, + { "Heading:", 40.0 }, + { "Radius:", 30.0 }, + { "Turns:", 10.0 }, +}; + +const MissionItemTest::FactValue_t MissionItemTest::_rgFactValuesLoiterTime[] = { + { "Latitude:", -10.0 }, + { "Longitude:", -20.0 }, + { "Altitude:", -30.0 }, + { "Heading:", 40.0 }, + { "Radius:", 30.0 }, + { "Seconds:", 10.0 }, +}; + +const MissionItemTest::FactValue_t MissionItemTest::_rgFactValuesLand[] = { + { "Latitude:", -10.0 }, + { "Longitude:", -20.0 }, + { "Altitude:", -30.0 }, + { "Heading:", 40.0 }, +}; + +const MissionItemTest::FactValue_t MissionItemTest::_rgFactValuesTakeoff[] = { + { "Latitude:", -10.0 }, + { "Longitude:", -20.0 }, + { "Altitude:", -30.0 }, + { "Heading:", 40.0 }, + { "Pitch:", 10.0 }, +}; + +const MissionItemTest::FactValue_t MissionItemTest::_rgFactValuesConditionDelay[] = { + { "Seconds:", 10.0 }, +}; + +const MissionItemTest::FactValue_t MissionItemTest::_rgFactValuesDoJump[] = { + { "Seq #:", 10.0 }, + { "Repeat:", 20.0 }, +}; + +const MissionItemTest::ItemExpected_t MissionItemTest::_rgItemExpected[] = { + { "1\t0\t3\t16\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", sizeof(MissionItemTest::_rgFactValuesWaypoint)/sizeof(MissionItemTest::_rgFactValuesWaypoint[0]), MissionItemTest::_rgFactValuesWaypoint }, + { "1\t0\t3\t17\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", sizeof(MissionItemTest::_rgFactValuesLoiterUnlim)/sizeof(MissionItemTest::_rgFactValuesLoiterUnlim[0]), MissionItemTest::_rgFactValuesLoiterUnlim }, + { "1\t0\t3\t18\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", sizeof(MissionItemTest::_rgFactValuesLoiterTurns)/sizeof(MissionItemTest::_rgFactValuesLoiterTurns[0]), MissionItemTest::_rgFactValuesLoiterTurns }, + { "1\t0\t3\t19\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", sizeof(MissionItemTest::_rgFactValuesLoiterTime)/sizeof(MissionItemTest::_rgFactValuesLoiterTime[0]), MissionItemTest::_rgFactValuesLoiterTime }, + { "1\t0\t3\t21\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", sizeof(MissionItemTest::_rgFactValuesLand)/sizeof(MissionItemTest::_rgFactValuesLand[0]), MissionItemTest::_rgFactValuesLand }, + { "1\t0\t3\t22\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", sizeof(MissionItemTest::_rgFactValuesTakeoff)/sizeof(MissionItemTest::_rgFactValuesTakeoff[0]), MissionItemTest::_rgFactValuesTakeoff }, + { "1\t0\t2\t112\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", sizeof(MissionItemTest::_rgFactValuesConditionDelay)/sizeof(MissionItemTest::_rgFactValuesConditionDelay[0]), MissionItemTest::_rgFactValuesConditionDelay }, + { "1\t0\t2\t177\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", sizeof(MissionItemTest::_rgFactValuesDoJump)/sizeof(MissionItemTest::_rgFactValuesDoJump[0]), MissionItemTest::_rgFactValuesDoJump }, +}; + +MissionItemTest::MissionItemTest(void) +{ + +} + +void MissionItemTest::_test(void) +{ + for (size_t i=0; icommand; + + MissionItem* item = new MissionItem(NULL, + info->sequenceNumber, + info->coordinate, + info->command, + info->param1, + info->param2, + info->param3, + info->param4, + info->autocontinue, + info->isCurrentItem, + info->frame); + + // Validate the saving is working correctly + QString savedItemString; + QTextStream saveStream(&savedItemString, QIODevice::WriteOnly); + item->save(saveStream); + QCOMPARE(savedItemString, QString(expected->streamString)); + + // Validate that the fact values are correctly returned + size_t factCount = 0; + for (int i=0; itextFieldFacts()->count(); i++) { + Fact* fact = qobject_cast(item->textFieldFacts()->get(i)); + + bool found = false; + for (size_t j=0; jcFactValues; j++) { + const FactValue_t* factValue = &expected->rgFactValues[j]; + + if (factValue->name == fact->name()) { + qDebug() << factValue->name; + QCOMPARE(fact->value().toDouble(), factValue->value); + factCount ++; + found = true; + break; + } + } + + QVERIFY(found); + } + QCOMPARE(factCount, expected->cFactValues); + + // Validate that loading is working correctly + MissionItem* loadedItem = new MissionItem(); + QTextStream loadStream(&savedItemString, QIODevice::ReadOnly); + QVERIFY(loadedItem->load(loadStream)); + //qDebug() << savedItemString; + QCOMPARE(loadedItem->coordinate().latitude(), item->coordinate().latitude()); + QCOMPARE(loadedItem->coordinate().longitude(), item->coordinate().longitude()); + QCOMPARE(loadedItem->coordinate().altitude(), item->coordinate().altitude()); + QCOMPARE(loadedItem->command(), item->command()); + QCOMPARE(loadedItem->param1(), item->param1()); + QCOMPARE(loadedItem->param2(), item->param2()); + QCOMPARE(loadedItem->param3(), item->param3()); + QCOMPARE(loadedItem->param4(), item->param4()); + QCOMPARE(loadedItem->autoContinue(), item->autoContinue()); + QCOMPARE(loadedItem->isCurrentItem(), item->isCurrentItem()); + QCOMPARE(loadedItem->frame(), item->frame()); + + delete item; + delete loadedItem; + } +} diff --git a/src/MissionItemTest.h b/src/MissionItemTest.h new file mode 100644 index 0000000000000000000000000000000000000000..643df0e2af70563965befca0f3da9a964d500f26 --- /dev/null +++ b/src/MissionItemTest.h @@ -0,0 +1,83 @@ +/*===================================================================== + + QGroundControl Open Source Ground Control Station + + (c) 2009 - 2015 QGROUNDCONTROL PROJECT + + This file is part of the QGROUNDCONTROL project + + QGROUNDCONTROL is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + QGROUNDCONTROL is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with QGROUNDCONTROL. If not, see . + + ======================================================================*/ + +#ifndef MissionItemTest_H +#define MissionItemTest_H + +#include "UnitTest.h" +#include "TCPLink.h" +#include "MultiSignalSpy.h" + +/// @file +/// @brief FlightGear HIL Simulation unit tests +/// +/// @author Don Gagne + +class MissionItemTest : public UnitTest +{ + Q_OBJECT + +public: + MissionItemTest(void); + +private slots: + void _test(void); + +private: + typedef struct { + int sequenceNumber; + QGeoCoordinate coordinate; + int command; + double param1; + double param2; + double param3; + double param4; + bool autocontinue; + bool isCurrentItem; + int frame; + } ItemInfo_t; + + typedef struct { + const char* name; + double value; + } FactValue_t; + + typedef struct { + const char* streamString; + size_t cFactValues; + const FactValue_t* rgFactValues; + } ItemExpected_t; + + static const ItemInfo_t _rgItemInfo[]; + static const ItemExpected_t _rgItemExpected[]; + static const FactValue_t _rgFactValuesWaypoint[]; + static const FactValue_t _rgFactValuesLoiterUnlim[]; + static const FactValue_t _rgFactValuesLoiterTurns[]; + static const FactValue_t _rgFactValuesLoiterTime[]; + static const FactValue_t _rgFactValuesLand[]; + static const FactValue_t _rgFactValuesTakeoff[]; + static const FactValue_t _rgFactValuesConditionDelay[]; + static const FactValue_t _rgFactValuesDoJump[]; +}; + +#endif diff --git a/src/MissionManager/MissionManager.cc b/src/MissionManager/MissionManager.cc index 55038190f2abb2b2d79e58606865013ce70c9c69..51d71675c342e019bc959f13a24d2ab4579f302f 100644 --- a/src/MissionManager/MissionManager.cc +++ b/src/MissionManager/MissionManager.cc @@ -79,11 +79,12 @@ void MissionManager::writeMissionItems(const QmlObjectListModel& missionItems) _vehicle->sendMessage(message); _startAckTimeout(AckMissionRequest, message); + emit inProgressChanged(true); } void MissionManager::requestMissionItems(void) { - qCDebug(MissionManagerLog) << "_requestMissionItems"; + qCDebug(MissionManagerLog) << "requestMissionItems read sequence"; mavlink_message_t message; mavlink_mission_request_list_t request; @@ -97,6 +98,7 @@ void MissionManager::requestMissionItems(void) _vehicle->sendMessage(message); _startAckTimeout(AckMissionCount, message); + emit inProgressChanged(true); } void MissionManager::_ackTimeout(void) @@ -157,6 +159,7 @@ void MissionManager::_sendTransactionComplete(void) _vehicle->sendMessage(message); emit newMissionItemsAvailable(); + emit inProgressChanged(false); } void MissionManager::_handleMissionCount(const mavlink_message_t& message) @@ -174,6 +177,7 @@ void MissionManager::_handleMissionCount(const mavlink_message_t& message) if (_cMissionItems == 0) { emit newMissionItemsAvailable(); + emit inProgressChanged(false); } else { _requestNextMissionItem(0); } @@ -222,14 +226,14 @@ void MissionManager::_handleMissionItem(const mavlink_message_t& message) MissionItem* item = new MissionItem(this, missionItem.seq, QGeoCoordinate(missionItem.x, missionItem.y, missionItem.z), + missionItem.command, missionItem.param1, missionItem.param2, missionItem.param3, - missionItem.param3, + missionItem.param4, missionItem.autocontinue, missionItem.current, - missionItem.frame, - missionItem.command); + missionItem.frame); _missionItems.append(item); if (!item->canEdit()) { @@ -308,8 +312,11 @@ void MissionManager::_handleMissionAck(const mavlink_message_t& message) if (missionAck.type == MAV_MISSION_ACCEPTED) { qCDebug(MissionManagerLog) << "_handleMissionAck write sequence complete"; } else { + _missionItems.clear(); + emit newMissionItemsAvailable(); qCDebug(MissionManagerLog) << "_handleMissionAck ack error:" << missionAck.type; } + emit inProgressChanged(false); } /// Called when a new mavlink message for out vehicle is received diff --git a/src/MissionManager/MissionManager.h b/src/MissionManager/MissionManager.h index f9e57d3cee6ef42acf57f6d884e8bd5c27f8459f..d039f463086b8f2e5eec0c08cba3028394c06140 100644 --- a/src/MissionManager/MissionManager.h +++ b/src/MissionManager/MissionManager.h @@ -47,7 +47,7 @@ public: MissionManager(Vehicle* vehicle); ~MissionManager(); - Q_PROPERTY(bool inProgress READ inProgress CONSTANT) + Q_PROPERTY(bool inProgress READ inProgress NOTIFY inProgressChanged) Q_PROPERTY(QmlObjectListModel* missionItems READ missionItems CONSTANT) Q_PROPERTY(bool canEdit READ canEdit NOTIFY canEditChanged) @@ -72,6 +72,7 @@ signals: // Public signals void canEditChanged(bool canEdit); void newMissionItemsAvailable(void); + void inProgressChanged(bool inProgress); private slots: void _mavlinkMessageReceived(const mavlink_message_t& message); diff --git a/src/MissionManager/MissionManagerTest.cc b/src/MissionManager/MissionManagerTest.cc new file mode 100644 index 0000000000000000000000000000000000000000..991d674ae8eb8e939de7909e4b30c90bb2233035 --- /dev/null +++ b/src/MissionManager/MissionManagerTest.cc @@ -0,0 +1,214 @@ +/*===================================================================== + + QGroundControl Open Source Ground Control Station + + (c) 2009 - 2014 QGROUNDCONTROL PROJECT + + This file is part of the QGROUNDCONTROL project + + QGROUNDCONTROL is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + QGROUNDCONTROL is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with QGROUNDCONTROL. If not, see . + + ======================================================================*/ + +#include "MissionManagerTest.h" +#include "LinkManager.h" +#include "MultiVehicleManager.h" + +UT_REGISTER_TEST(MissionManagerTest) + +const MissionManagerTest::TestCase_t MissionManagerTest::_rgTestCases[] = { + { "1\t0\t3\t16\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", { 1, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_NAV_WAYPOINT, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_GLOBAL_RELATIVE_ALT } }, + { "1\t0\t3\t17\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", { 1, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_NAV_LOITER_UNLIM, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_GLOBAL_RELATIVE_ALT } }, + { "1\t0\t3\t18\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", { 1, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_NAV_LOITER_TURNS, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_GLOBAL_RELATIVE_ALT } }, + { "1\t0\t3\t19\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", { 1, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_NAV_LOITER_TIME, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_GLOBAL_RELATIVE_ALT } }, + { "1\t0\t3\t21\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", { 1, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_NAV_LAND, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_GLOBAL_RELATIVE_ALT } }, + { "1\t0\t3\t22\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", { 1, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_NAV_TAKEOFF, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_GLOBAL_RELATIVE_ALT } }, + { "1\t0\t2\t112\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", { 1, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_CONDITION_DELAY, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_MISSION } }, + { "1\t0\t2\t177\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", { 1, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_DO_JUMP, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_MISSION } }, +}; + +MissionManagerTest::MissionManagerTest(void) + : _mockLink(NULL) +{ + +} + +void MissionManagerTest::init(void) +{ + UnitTest::init(); + + LinkManager* linkMgr = LinkManager::instance(); + Q_CHECK_PTR(linkMgr); + + _mockLink = new MockLink(); + Q_CHECK_PTR(_mockLink); + LinkManager::instance()->_addLink(_mockLink); + + linkMgr->connectLink(_mockLink); + + // Wait for the Vehicle to work it's way through the various threads + + QSignalSpy spyVehicle(MultiVehicleManager::instance(), SIGNAL(activeVehicleChanged(Vehicle*))); + QCOMPARE(spyVehicle.wait(5000), true); + + // Wait for the Mission Manager to finish it's initial load + + _missionManager = MultiVehicleManager::instance()->activeVehicle()->missionManager(); + QVERIFY(_missionManager); + + _rgSignals[canEditChangedSignalIndex] = SIGNAL(canEditChanged(bool)); + _rgSignals[newMissionItemsAvailableSignalIndex] = SIGNAL(newMissionItemsAvailable(void)); + _rgSignals[inProgressChangedSignalIndex] = SIGNAL(inProgressChanged(bool)); + + _multiSpy = new MultiSignalSpy(); + Q_CHECK_PTR(_multiSpy); + QCOMPARE(_multiSpy->init(_missionManager, _rgSignals, _cSignals), true); + + if (_missionManager->inProgress()) { + _multiSpy->waitForSignalByIndex(newMissionItemsAvailableSignalIndex, 1000); + _multiSpy->waitForSignalByIndex(inProgressChangedSignalIndex, 1000); + QCOMPARE(_multiSpy->checkSignalByMask(newMissionItemsAvailableSignalMask | inProgressChangedSignalMask), true); + QCOMPARE(_multiSpy->checkNoSignalByMask(canEditChangedSignalIndex), true); + } + + QVERIFY(!_missionManager->inProgress()); + QCOMPARE(_missionManager->missionItems()->count(), 0); + _multiSpy->clearAllSignals(); +} + +void MissionManagerTest::cleanup(void) +{ + delete _multiSpy; + _multiSpy = NULL; + + LinkManager::instance()->disconnectLink(_mockLink); + _mockLink = NULL; + QTest::qWait(1000); // Need to allow signals to move between threads + + UnitTest::cleanup(); +} + +/// Checks the state of the inProgress value and signal to match the specified value +void MissionManagerTest::_checkInProgressValues(bool inProgress) +{ + QCOMPARE(_missionManager->inProgress(), inProgress); + QSignalSpy* spy = _multiSpy->getSpyByIndex(inProgressChangedSignalIndex); + QList signalArgs = spy->takeFirst(); + QCOMPARE(signalArgs.count(), 1); + QCOMPARE(signalArgs[0].toBool(), inProgress); +} + +void MissionManagerTest::_readEmptyVehicle(void) +{ + _missionManager->requestMissionItems(); + + // requestMissionItems should emit inProgressChanged signal before returning so no need to wait for it + QVERIFY(_missionManager->inProgress()); + QCOMPARE(_multiSpy->checkOnlySignalByMask(inProgressChangedSignalMask), true); + _checkInProgressValues(true); + + _multiSpy->clearAllSignals(); + + // Now wait for read sequence to complete. We should get both a newMissionItemsAvailable and a + // inProgressChanged signal to signal completion. + _multiSpy->waitForSignalByIndex(newMissionItemsAvailableSignalIndex, 1000); + _multiSpy->waitForSignalByIndex(inProgressChangedSignalIndex, 1000); + QCOMPARE(_multiSpy->checkSignalByMask(newMissionItemsAvailableSignalMask | inProgressChangedSignalMask), true); + QCOMPARE(_multiSpy->checkNoSignalByMask(canEditChangedSignalMask), true); + _checkInProgressValues(false); + + // Vehicle should have no items at this point + QCOMPARE(_missionManager->missionItems()->count(), 0); + QCOMPARE(_missionManager->canEdit(), true); +} + +void MissionManagerTest::_roundTripItems(void) +{ + // Setup our test case data + const size_t cTestCases = sizeof(_rgTestCases)/sizeof(_rgTestCases[0]); + QmlObjectListModel* list = new QmlObjectListModel(); + + for (size_t i=0; iitemStream, QIODevice::ReadOnly); + QVERIFY(item->load(loadStream)); + + list->append(item); + } + + // Send the items to the vehicle + _missionManager->writeMissionItems(*list); + + // writeMissionItems should emit inProgressChanged signal before returning so no need to wait for it + QVERIFY(_missionManager->inProgress()); + QCOMPARE(_multiSpy->checkOnlySignalByMask(inProgressChangedSignalMask), true); + _checkInProgressValues(true); + + _multiSpy->clearAllSignals(); + + // Now wait for write sequence to complete. We should only get an inProgressChanged signal to signal completion. + _multiSpy->waitForSignalByIndex(inProgressChangedSignalIndex, 1000); + QCOMPARE(_multiSpy->checkOnlySignalByMask(inProgressChangedSignalMask), true); + _checkInProgressValues(false); + + QCOMPARE(_missionManager->missionItems()->count(), (int)cTestCases); + QCOMPARE(_missionManager->canEdit(), true); + + delete list; + list = NULL; + _multiSpy->clearAllSignals(); + + // Read the items back from the vehicle + _missionManager->requestMissionItems(); + + // requestMissionItems should emit inProgressChanged signal before returning so no need to wait for it + QVERIFY(_missionManager->inProgress()); + QCOMPARE(_multiSpy->checkOnlySignalByMask(inProgressChangedSignalMask), true); + _checkInProgressValues(true); + + _multiSpy->clearAllSignals(); + + // Now wait for read sequence to complete. We should get both a newMissionItemsAvailable and a + // inProgressChanged signal to signal completion. + _multiSpy->waitForSignalByIndex(newMissionItemsAvailableSignalIndex, 1000); + _multiSpy->waitForSignalByIndex(inProgressChangedSignalIndex, 1000); + QCOMPARE(_multiSpy->checkSignalByMask(newMissionItemsAvailableSignalMask | inProgressChangedSignalMask), true); + QCOMPARE(_multiSpy->checkNoSignalByMask(canEditChangedSignalMask), true); + _checkInProgressValues(false); + + QCOMPARE(_missionManager->missionItems()->count(), (int)cTestCases); + QCOMPARE(_missionManager->canEdit(), true); + + // Validate the returned items against our test data + + for (size_t i=0; i(_missionManager->missionItems()->get(i)); + + qDebug() << "Test case" << i; + QCOMPARE(actual->coordinate().latitude(), testCase->expectedItem.coordinate.latitude()); + QCOMPARE(actual->coordinate().longitude(), testCase->expectedItem.coordinate.longitude()); + QCOMPARE(actual->coordinate().altitude(), testCase->expectedItem.coordinate.altitude()); + QCOMPARE((int)actual->command(), (int)testCase->expectedItem.command); + QCOMPARE(actual->param1(), testCase->expectedItem.param1); + QCOMPARE(actual->param2(), testCase->expectedItem.param2); + QCOMPARE(actual->param3(), testCase->expectedItem.param3); + QCOMPARE(actual->param4(), testCase->expectedItem.param4); + QCOMPARE(actual->autoContinue(), testCase->expectedItem.autocontinue); + QCOMPARE(actual->frame(), testCase->expectedItem.frame); + } +} diff --git a/src/MissionManager/MissionManagerTest.h b/src/MissionManager/MissionManagerTest.h new file mode 100644 index 0000000000000000000000000000000000000000..22ee3903f2097e3dd40a79b7b31449832b4d063f --- /dev/null +++ b/src/MissionManager/MissionManagerTest.h @@ -0,0 +1,90 @@ +/*===================================================================== + + QGroundControl Open Source Ground Control Station + + (c) 2009 - 2015 QGROUNDCONTROL PROJECT + + This file is part of the QGROUNDCONTROL project + + QGROUNDCONTROL is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + QGROUNDCONTROL is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with QGROUNDCONTROL. If not, see . + + ======================================================================*/ + +#ifndef MissionManagerTest_H +#define MissionManagerTest_H + +#include "UnitTest.h" +#include "MockLink.h" +#include "MissionManager.h" +#include "MultiSignalSpy.h" + +class MissionManagerTest : public UnitTest +{ + Q_OBJECT + +public: + MissionManagerTest(void); + +private slots: + void init(void); + void cleanup(void); + + void _readEmptyVehicle(void); + void _roundTripItems(void); + +private: + void _checkInProgressValues(bool inProgress); + + MockLink* _mockLink; + MissionManager* _missionManager; + + enum { + canEditChangedSignalIndex = 0, + newMissionItemsAvailableSignalIndex, + inProgressChangedSignalIndex, + maxSignalIndex + }; + + enum { + canEditChangedSignalMask = 1 << canEditChangedSignalIndex, + newMissionItemsAvailableSignalMask = 1 << newMissionItemsAvailableSignalIndex, + inProgressChangedSignalMask = 1 << inProgressChangedSignalIndex, + }; + + MultiSignalSpy* _multiSpy; + static const size_t _cSignals = maxSignalIndex; + const char* _rgSignals[_cSignals]; + + typedef struct { + int sequenceNumber; + QGeoCoordinate coordinate; + int command; + double param1; + double param2; + double param3; + double param4; + bool autocontinue; + bool isCurrentItem; + int frame; + } ItemInfo_t; + + typedef struct { + const char* itemStream; + const ItemInfo_t expectedItem; + } TestCase_t; + + static const TestCase_t _rgTestCases[]; +}; + +#endif diff --git a/src/QGCApplication.cc b/src/QGCApplication.cc index c795fe0282a5b244fc96bcc72e19d6954266c981..d96ba0bcd26d7aab5ce21ea4153a99736b3eb09c 100644 --- a/src/QGCApplication.cc +++ b/src/QGCApplication.cc @@ -83,6 +83,7 @@ #include "MissionManager.h" #include "QGroundControlQmlGlobal.h" #include "HomePositionManager.h" +#include "FlightMapSettings.h" #ifndef __ios__ #include "SerialLink.h" @@ -161,7 +162,9 @@ QGCApplication::QGCApplication(int &argc, char* argv[], bool unitTesting) , _runningUnitTests(unitTesting) , _styleIsDark(true) , _fakeMobile(false) - , _useNewMissionEditor(false) +#ifdef UNITTEST_BUILD + , _useNewMissionEditor(true) // Unit Tests run new mission editor +#endif #ifdef QT_DEBUG , _testHighDPI(false) #endif @@ -313,7 +316,6 @@ void QGCApplication::_initCommon(void) qmlRegisterUncreatableType ("QGroundControl.JoystickManager", 1, 0, "JoystickManager", "Reference only"); qmlRegisterUncreatableType ("QGroundControl.JoystickManager", 1, 0, "Joystick", "Reference only"); qmlRegisterUncreatableType ("QGroundControl", 1, 0, "QmlObjectListModel", "Reference only"); - qmlRegisterUncreatableType ("QGroundControl", 1, 0, "HomePositionManager", "Reference only"); qmlRegisterType ("QGroundControl.Controllers", 1, 0, "ViewWidgetController"); qmlRegisterType ("QGroundControl.Controllers", 1, 0, "ParameterEditorController"); @@ -550,6 +552,16 @@ void QGCApplication::_createSingletons(void) { // The order here is important since the singletons reference each other + // No dependencies + FlightMapSettings* flightMapSettings = FlightMapSettings::_createSingleton(); + Q_UNUSED(flightMapSettings); + Q_ASSERT(flightMapSettings); + + // No dependencies + HomePositionManager* homePositionManager = HomePositionManager::_createSingleton(); + Q_UNUSED(homePositionManager); + Q_ASSERT(homePositionManager); + // No dependencies FirmwarePlugin* firmwarePlugin = GenericFirmwarePlugin::_createSingleton(); Q_UNUSED(firmwarePlugin); @@ -584,22 +596,17 @@ void QGCApplication::_createSingletons(void) Q_UNUSED(linkManager); Q_ASSERT(linkManager); - // Needs LinkManager - HomePositionManager* uasManager = HomePositionManager::_createSingleton(); - Q_UNUSED(uasManager); - Q_ASSERT(uasManager); - - // Need HomePositionManager + // Need MultiVehicleManager AutoPilotPluginManager* pluginManager = AutoPilotPluginManager::_createSingleton(); Q_UNUSED(pluginManager); Q_ASSERT(pluginManager); - // Need HomePositionManager + // Need MultiVehicleManager UASMessageHandler* messageHandler = UASMessageHandler::_createSingleton(); Q_UNUSED(messageHandler); Q_ASSERT(messageHandler); - // Needs HomePositionManager + // Needs MultiVehicleManager FactSystem* factSystem = FactSystem::_createSingleton(); Q_UNUSED(factSystem); Q_ASSERT(factSystem); @@ -631,7 +638,6 @@ void QGCApplication::_destroySingletons(void) FactSystem::_deleteSingleton(); UASMessageHandler::_deleteSingleton(); AutoPilotPluginManager::_deleteSingleton(); - HomePositionManager::_deleteSingleton(); LinkManager::_deleteSingleton(); GAudioOutput::_deleteSingleton(); JoystickManager::_deleteSingleton(); @@ -640,6 +646,8 @@ void QGCApplication::_destroySingletons(void) GenericFirmwarePlugin::_deleteSingleton(); PX4FirmwarePlugin::_deleteSingleton(); APMFirmwarePlugin::_deleteSingleton(); + HomePositionManager::_deleteSingleton(); + FlightMapSettings::_deleteSingleton(); } void QGCApplication::informationMessageBoxOnMainThread(const QString& title, const QString& msg) diff --git a/src/QmlControls/DropButton.qml b/src/QmlControls/DropButton.qml index 7a2acd1858ebdbe6f02ea87df54264107f5e12db..2943cae3ce0d42547ea5cd406620fa54f0458eca 100644 --- a/src/QmlControls/DropButton.qml +++ b/src/QmlControls/DropButton.qml @@ -1,4 +1,4 @@ -import QtQuick 2.2 +import QtQuick 2.4 import QtQuick.Controls 1.2 import QtQuick.Controls.Styles 1.2 @@ -8,8 +8,8 @@ import QGroundControl.Palette 1.0 Item { id: _root - property alias label: buttonLabel.text - property alias radius: button.radius + property alias buttonImage: button.source + property real radius: (ScreenTools.defaultFontPixelHeight * 3) / 2 property int dropDirection: dropDown property alias dropDownComponent: dropDownLoader.sourceComponent property real viewportMargins: 0 @@ -144,33 +144,25 @@ Item { } // Button - Rectangle { + Image { id: button anchors.fill: parent - radius: (ScreenTools.defaultFontPixelHeight * 3) / 2 - color: qgcPal.button + fillMode: Image.PreserveAspectFit opacity: _showDropDown ? 1.0 : 0.75 - - QGCLabel { - id: buttonLabel - anchors.fill: parent - horizontalAlignment: Text.AlignHCenter - verticalAlignment: Text.AlignVCenter - color: "white" - } - + mipmap: true + smooth: true MouseArea { anchors.fill: parent onClicked: _showDropDown = !_showDropDown } - } // Rectangle - button + } // Image - button Item { id: dropDownItem visible: _showDropDown - Canvas { + QGCCanvas { id: arrowCanvas anchors.fill: parent @@ -224,6 +216,13 @@ Item { id: dropDownLoader x: _dropMargin y: _dropMargin + + Connections { + target: dropDownLoader.item + + onWidthChanged: _calcPositions() + onHeightChanged: _calcPositions() + } } } } // Item - dropDownItem diff --git a/src/QmlControls/QGCCanvas.qml b/src/QmlControls/QGCCanvas.qml new file mode 100644 index 0000000000000000000000000000000000000000..1b7d6e3b9808e49e9ce3c3e351bb201ac3393e9d --- /dev/null +++ b/src/QmlControls/QGCCanvas.qml @@ -0,0 +1,17 @@ +import QtQuick 2.2 +import QtQuick.Controls 1.2 +import QtQuick.Controls.Styles 1.2 + +import QGroundControl.Palette 1.0 +import QGroundControl.ScreenTools 1.0 + +/// Canvas has some sort of bug in it which can cause it to not paint when top level Views +/// are switched. In order to fix this we ahve a signal hacked into ScreenTools to force +/// a repaint. +Canvas { + Connections { + target: ScreenTools + + onRepaintRequested: arrowCanvas.requestPaint() + } +} diff --git a/src/QmlControls/QGroundControl.Controls.qmldir b/src/QmlControls/QGroundControl.Controls.qmldir index cc9b94af863eb80dbf383a5ef39b88434e96cf10..5d72b54cccfab741ef85519db4002ba3a19598ce 100644 --- a/src/QmlControls/QGroundControl.Controls.qmldir +++ b/src/QmlControls/QGroundControl.Controls.qmldir @@ -9,6 +9,7 @@ QGCComboBox 1.0 QGCComboBox.qml QGCColoredImage 1.0 QGCColoredImage.qml QGCToolBarButton 1.0 QGCToolBarButton.qml QGCMovableItem 1.0 QGCMovableItem.qml +QGCCanvas 1.0 QGCCanvas.qml SubMenuButton 1.0 SubMenuButton.qml IndicatorButton 1.0 IndicatorButton.qml diff --git a/src/QmlControls/QGroundControlQmlGlobal.cc b/src/QmlControls/QGroundControlQmlGlobal.cc index 1261e059b0c20632d00e3f633dfef4926db13c59..8912f375e19db8e1567fc4ef248d9a16f6f50530 100644 --- a/src/QmlControls/QGroundControlQmlGlobal.cc +++ b/src/QmlControls/QGroundControlQmlGlobal.cc @@ -29,6 +29,7 @@ QGroundControlQmlGlobal::QGroundControlQmlGlobal(QObject* parent) : QObject(parent) , _homePositionManager(HomePositionManager::instance()) + , _flightMapSettings(FlightMapSettings::instance()) { } diff --git a/src/QmlControls/QGroundControlQmlGlobal.h b/src/QmlControls/QGroundControlQmlGlobal.h index 72fcfff8d83af3681fd6a1ee8e27ee7ff0b9ce45..5091dae4661d6a7d8f16da6ef337b87f8adce9dd 100644 --- a/src/QmlControls/QGroundControlQmlGlobal.h +++ b/src/QmlControls/QGroundControlQmlGlobal.h @@ -30,6 +30,7 @@ #include #include "HomePositionManager.h" +#include "FlightMapSettings.h" class QGroundControlQmlGlobal : public QObject { @@ -39,14 +40,17 @@ public: QGroundControlQmlGlobal(QObject* parent = NULL); ~QGroundControlQmlGlobal(); - Q_PROPERTY(HomePositionManager* homePositionManager READ homePositionManager CONSTANT) + Q_PROPERTY(HomePositionManager* homePositionManager READ homePositionManager CONSTANT) + Q_PROPERTY(FlightMapSettings* flightMapSettings READ flightMapSettings CONSTANT) // Property accesors - HomePositionManager* homePositionManager(void) { return _homePositionManager; } + HomePositionManager* homePositionManager(void) { return _homePositionManager; } + FlightMapSettings* flightMapSettings(void) { return _flightMapSettings; } private: HomePositionManager* _homePositionManager; + FlightMapSettings* _flightMapSettings; }; #endif diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 7e973712534db6e4f31a29750a9765abde127c4e..6bcc7a9b25796c386c487da512642c8511f6aeb5 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -51,6 +51,7 @@ Vehicle::Vehicle(LinkInterface* link, int vehicleId, MAV_AUTOPILOT firmwareType) , _joystickMode(JoystickModeRC) , _joystickEnabled(false) , _uas(NULL) + , _homePositionAvailable(false) , _mav(NULL) , _currentMessageCount(0) , _messageCount(0) @@ -203,6 +204,20 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes // Give the plugin a change to adjust the message contents _firmwarePlugin->adjustMavlinkMessage(&message); + if (message.msgid == MAVLINK_MSG_ID_HOME_POSITION) { + mavlink_home_position_t homePos; + + mavlink_msg_home_position_decode(&message, &homePos); + + _homePosition.setLatitude(homePos.latitude / 10000000.0); + _homePosition.setLongitude(homePos.longitude / 10000000.0); + _homePosition.setAltitude(homePos.altitude / 1000.0); + _homePositionAvailable = true; + + emit homePositionAvailableChanged(true); + emit homePositionChanged(_homePosition); + } + emit mavlinkMessageReceived(message); _uas->receiveMessage(message); @@ -945,3 +960,17 @@ QmlObjectListModel* Vehicle::missionItemsModel(void) return &_missionItems; } } + +bool Vehicle::homePositionAvailable(void) +{ + return _homePositionAvailable; +} + +QGeoCoordinate Vehicle::homePosition(void) +{ + if (!_homePositionAvailable) { + qWarning() << "Call to homePosition while _homePositionAvailable == false"; + } + + return _homePosition; +} diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 1c30ed9bc221eca2cdcd47cd8f1bd428b9cacda6..cffb42170b2aa18ecac90614ca78da28d2e2c4a0 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -59,6 +59,9 @@ public: Q_PROPERTY(QGeoCoordinate coordinate MEMBER _geoCoordinate NOTIFY coordinateChanged) Q_PROPERTY(MissionManager* missionManager MEMBER _missionManager CONSTANT) + Q_PROPERTY(bool homePositionAvailable READ homePositionAvailable NOTIFY homePositionAvailableChanged) + Q_PROPERTY(QGeoCoordinate homePosition READ homePosition NOTIFY homePositionChanged) + Q_INVOKABLE QString getMavIconColor(); //-- System Messages @@ -154,6 +157,9 @@ public: MissionManager* missionManager(void) { return _missionManager; } + bool homePositionAvailable(void); + QGeoCoordinate homePosition(void); + typedef enum { MessageNone, MessageNormal, @@ -221,6 +227,8 @@ signals: void joystickEnabledChanged(bool enabled); void activeChanged(bool active); void mavlinkMessageReceived(const mavlink_message_t& message); + void homePositionAvailableChanged(bool homePositionAvailable); + void homePositionChanged(const QGeoCoordinate& homePosition); /// Used internally to move sendMessage call to main thread void _sendMessageOnThread(mavlink_message_t message); @@ -318,6 +326,9 @@ private: QGeoCoordinate _geoCoordinate; + bool _homePositionAvailable; + QGeoCoordinate _homePosition; + UASInterface* _mav; int _currentMessageCount; int _messageCount; diff --git a/src/VehicleSetup/PX4FirmwareUpgradeThread.cc b/src/VehicleSetup/PX4FirmwareUpgradeThread.cc index 177bbf0f23d657dd807f16c07b28c2b35ea52dfe..58d29e385509cc7cb9fc94608296e09a73d123b9 100644 --- a/src/VehicleSetup/PX4FirmwareUpgradeThread.cc +++ b/src/VehicleSetup/PX4FirmwareUpgradeThread.cc @@ -183,6 +183,10 @@ bool PX4FirmwareUpgradeThreadWorker::_findBoardFromPorts(QSerialPortInfo& portIn qCDebug(FirmwareUpgradeLog) << "Found PX4 FMU V1 (by name matching fallback)"; type = FoundBoardPX4FMUV1; found = true; + } else if (info.description().startsWith("PX4 FMU")) { + qCDebug(FirmwareUpgradeLog) << "Found PX4 FMU, assuming V2 (by name matching fallback)"; + type = FoundBoardPX4FMUV2; + found = true; } } } diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc index 4ad64ba3149c114eee6a5bcf48ea9399225b465f..a847ad7646151a7d495814b18558f368fd44c0d2 100644 --- a/src/comm/MockLink.cc +++ b/src/comm/MockLink.cc @@ -88,7 +88,7 @@ MockLink::MockLink(MockConfiguration* config) : _fileServer = new MockLinkFileServer(_vehicleSystemId, _vehicleComponentId, this); Q_CHECK_PTR(_fileServer); - _missionItemHandler = new MockLinkMissionItemHandler(_vehicleSystemId, this); + _missionItemHandler = new MockLinkMissionItemHandler(this); Q_CHECK_PTR(_missionItemHandler); moveToThread(this); @@ -302,9 +302,11 @@ void MockLink::_handleIncomingMavlinkBytes(const uint8_t* bytes, int cBytes) if (!mavlink_parse_char(getMavlinkChannel(), bytes[i], &msg, &comm)) { continue; } - + Q_ASSERT(_missionItemHandler); - _missionItemHandler->handleMessage(msg); + if (_missionItemHandler->handleMessage(msg)) { + continue; + } switch (msg.msgid) { case MAVLINK_MSG_ID_HEARTBEAT: @@ -326,24 +328,6 @@ void MockLink::_handleIncomingMavlinkBytes(const uint8_t* bytes, int cBytes) case MAVLINK_MSG_ID_PARAM_REQUEST_READ: _handleParamRequestRead(msg); break; - - case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: - _handleMissionRequestList(msg); - break; - - case MAVLINK_MSG_ID_MISSION_REQUEST: - _handleMissionRequest(msg); - break; - - case MAVLINK_MSG_ID_MISSION_ITEM: - _handleMissionItem(msg); - break; - -#if 0 - case MAVLINK_MSG_ID_MISSION_COUNT: - _handleMissionCount(msg); - break; -#endif case MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL: _handleFTP(msg); @@ -627,67 +611,6 @@ void MockLink::_handleParamRequestRead(const mavlink_message_t& msg) respondWithMavlinkMessage(responseMsg); } -void MockLink::_handleMissionRequestList(const mavlink_message_t& msg) -{ - mavlink_mission_request_list_t request; - - mavlink_msg_mission_request_list_decode(&msg, &request); - - Q_ASSERT(request.target_system == _vehicleSystemId); - - mavlink_message_t responseMsg; - - mavlink_msg_mission_count_pack(_vehicleSystemId, - _vehicleComponentId, - &responseMsg, // Outgoing message - msg.sysid, // Target is original sender - msg.compid, // Target is original sender - _missionItems.count()); // Number of mission items - respondWithMavlinkMessage(responseMsg); -} - -void MockLink::_handleMissionRequest(const mavlink_message_t& msg) -{ - mavlink_mission_request_t request; - - mavlink_msg_mission_request_decode(&msg, &request); - - Q_ASSERT(request.target_system == _vehicleSystemId); - Q_ASSERT(request.seq < _missionItems.count()); - - mavlink_message_t responseMsg; - - mavlink_mission_item_t item = _missionItems[request.seq]; - - mavlink_msg_mission_item_pack(_vehicleSystemId, - _vehicleComponentId, - &responseMsg, // Outgoing message - msg.sysid, // Target is original sender - msg.compid, // Target is original sender - request.seq, // Index of mission item being sent - item.frame, - item.command, - item.current, - item.autocontinue, - item.param1, item.param2, item.param3, item.param4, - item.x, item.y, item.z); - respondWithMavlinkMessage(responseMsg); -} - -void MockLink::_handleMissionItem(const mavlink_message_t& msg) -{ - mavlink_mission_item_t request; - - mavlink_msg_mission_item_decode(&msg, &request); - - Q_ASSERT(request.target_system == _vehicleSystemId); - - // FIXME: What do you do with duplication sequence numbers? - Q_ASSERT(!_missionItems.contains(request.seq)); - - _missionItems[request.seq] = request; -} - void MockLink::emitRemoteControlChannelRawChanged(int channel, uint16_t raw) { uint16_t chanRaw[18]; diff --git a/src/comm/MockLink.h b/src/comm/MockLink.h index 7ca8f179d7c768f83c192e22b9fa9bf923e23453..6c7950cbc88bcec60899e57e908834d46462f35e 100644 --- a/src/comm/MockLink.h +++ b/src/comm/MockLink.h @@ -61,14 +61,8 @@ public: MockLink(MockConfiguration* config = NULL); ~MockLink(void); - // Virtuals from LinkInterface - virtual QString getName(void) const { return _name; } - virtual void requestReset(void){ } - virtual bool isConnected(void) const { return _connected; } - virtual qint64 getConnectionSpeed(void) const { return 100000000; } - virtual qint64 bytesAvailable(void) { return 0; } - // MockLink methods + int vehicleId(void) { return _vehicleSystemId; } MAV_AUTOPILOT getAutopilotType(void) { return _autopilotType; } void setAutopilotType(MAV_AUTOPILOT autopilot) { _autopilotType = autopilot; } void emitRemoteControlChannelRawChanged(int channel, uint16_t raw); @@ -78,6 +72,13 @@ public: MockLinkFileServer* getFileServer(void) { return _fileServer; } + // Virtuals from LinkInterface + virtual QString getName(void) const { return _name; } + virtual void requestReset(void){ } + virtual bool isConnected(void) const { return _connected; } + virtual qint64 getConnectionSpeed(void) const { return 100000000; } + virtual qint64 bytesAvailable(void) { return 0; } + // These are left unimplemented in order to cause linker errors which indicate incorrect usage of // connect/disconnect on link directly. All connect/disconnect calls should be made through LinkManager. bool connect(void); @@ -120,9 +121,6 @@ private: void _handleParamRequestList(const mavlink_message_t& msg); void _handleParamSet(const mavlink_message_t& msg); void _handleParamRequestRead(const mavlink_message_t& msg); - void _handleMissionRequestList(const mavlink_message_t& msg); - void _handleMissionRequest(const mavlink_message_t& msg); - void _handleMissionItem(const mavlink_message_t& msg); void _handleFTP(const mavlink_message_t& msg); void _handleCommandLong(const mavlink_message_t& msg); float _floatUnionForParam(int componentId, const QString& paramName); @@ -142,9 +140,6 @@ private: QMap > _mapParamName2Value; QMap _mapParamName2MavParamType; - typedef QMap MissionList_t; - MissionList_t _missionItems; - uint8_t _mavBaseMode; uint32_t _mavCustomMode; uint8_t _mavState; diff --git a/src/comm/MockLinkMissionItemHandler.cc b/src/comm/MockLinkMissionItemHandler.cc index 6e3a4abd3259698f031e04160b084e3f28f79951..59bf013318baa13e30e8ded63761e89984bc40ed 100644 --- a/src/comm/MockLinkMissionItemHandler.cc +++ b/src/comm/MockLinkMissionItemHandler.cc @@ -21,1158 +21,187 @@ ======================================================================*/ -/** -* @file -* @brief a program to manage waypoints and exchange them with the ground station -* -* @author Petri Tanskanen -* @author Benjamin Knecht -* @author Christian Schluchter -*/ - -// FIXME: This file is a work in progress - #include "MockLinkMissionItemHandler.h" +#include "MockLink.h" -#include - -#include "QGC.h" #include +QGC_LOGGING_CATEGORY(MockLinkMissionItemHandlerLog, "MockLinkMissionItemHandlerLog") -MockLinkMissionItemHandler::MockLinkMissionItemHandler(uint16_t systemId, QObject* parent) : - QObject(parent), - _vehicleSystemId(systemId) +MockLinkMissionItemHandler::MockLinkMissionItemHandler(MockLink* mockLink) + : QObject(mockLink) + , _mockLink(mockLink) { - + Q_ASSERT(mockLink); } -void MockLinkMissionItemHandler::handleMessage(const mavlink_message_t& msg) +bool MockLinkMissionItemHandler::handleMessage(const mavlink_message_t& msg) { switch (msg.msgid) { - case MAVLINK_MSG_ID_MISSION_ACK: - // Acks are received back for each MISSION_ITEM message - break; - - case MAVLINK_MSG_ID_MISSION_SET_CURRENT: - // Sets the currently active mission item - break; - case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: - // Signals the start of requesting the full mission list. Subsequent MISSION_REQUEST message should be received for - // each mission item. + _handleMissionRequestList(msg); break; - + case MAVLINK_MSG_ID_MISSION_REQUEST: - // Request the specified mission item. Requests should be in order. + _handleMissionRequest(msg); break; - + + case MAVLINK_MSG_ID_MISSION_ITEM: + _handleMissionItem(msg); + break; + case MAVLINK_MSG_ID_MISSION_COUNT: - // Return the current number of mission items + _handleMissionCount(msg); break; - - case MAVLINK_MSG_ID_MISSION_ITEM: - // FIXME: Figure out + + case MAVLINK_MSG_ID_MISSION_ACK: + // Acks are received back for each MISSION_ITEM message + break; + + case MAVLINK_MSG_ID_MISSION_SET_CURRENT: + // Sets the currently active mission item break; case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: // Delete all mission items + _missionItems.clear(); break; + + default: + return false; } + + return true; } -#if 0 -#ifndef M_PI -#define M_PI 3.14159265358979323846 -#endif - -class PxMatrix3x3; - - -/** - * @brief Pixhawk 3D vector class, can be cast to a local OpenCV CvMat. - * - */ -class PxVector3 +void MockLinkMissionItemHandler::_handleMissionRequestList(const mavlink_message_t& msg) { -public: - /** @brief standard constructor */ - PxVector3(void) {} - /** @brief copy constructor */ - PxVector3(const PxVector3 &v) { - for (int i=0; i < 3; i++) { - m_vec[i] = v.m_vec[i]; - } - } - /** @brief x,y,z constructor */ - PxVector3(const float _x, const float _y, const float _z) { - m_vec[0] = _x; - m_vec[1] = _y; - m_vec[2] = _z; - } - /** @brief broadcast constructor */ - PxVector3(const float _f) { - for (int i=0; i < 3; i++) { - m_vec[i] = _f; - } - } - -private: - /** @brief private constructor (not used here, for SSE compatibility) */ - PxVector3(const float (&_vec)[3]) { - for (int i=0; i < 3; i++) { - m_vec[i] = _vec[i]; - } - } - -public: - /** @brief assignment operator */ - void operator= (const PxVector3 &r) { - for (int i=0; i < 3; i++) { - m_vec[i] = r.m_vec[i]; - } - } - /** @brief const element access */ - float operator[] (const int i) const { - return m_vec[i]; - } - /** @brief element access */ - float &operator[] (const int i) { - return m_vec[i]; - } - - // === arithmetic operators === - /** @brief element-wise negation */ - friend PxVector3 operator- (const PxVector3 &v) { - PxVector3 ret; - for (int i=0; i < 3; i++) { - ret.m_vec[i] = -v.m_vec[i]; - } - return ret; - } - friend PxVector3 operator+ (const PxVector3 &l, const PxVector3 &r) { - PxVector3 ret; - for (int i=0; i < 3; i++) { - ret.m_vec[i] = l.m_vec[i] + r.m_vec[i]; - } - return ret; - } - friend PxVector3 operator- (const PxVector3 &l, const PxVector3 &r) { - PxVector3 ret; - for (int i=0; i < 3; i++) { - ret.m_vec[i] = l.m_vec[i] - r.m_vec[i]; - } - return ret; - } - friend PxVector3 operator* (const PxVector3 &l, const PxVector3 &r) { - PxVector3 ret; - for (int i=0; i < 3; i++) { - ret.m_vec[i] = l.m_vec[i] * r.m_vec[i]; - } - return ret; - } - friend PxVector3 operator/ (const PxVector3 &l, const PxVector3 &r) { - PxVector3 ret; - for (int i=0; i < 3; i++) { - ret.m_vec[i] = l.m_vec[i] / r.m_vec[i]; - } - return ret; - } + qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequestList read sequence"; - friend void operator+= (PxVector3 &l, const PxVector3 &r) { - for (int i=0; i < 3; i++) { - l.m_vec[i] = l.m_vec[i] + r.m_vec[i]; - } - } - friend void operator-= (PxVector3 &l, const PxVector3 &r) { - for (int i=0; i < 3; i++) { - l.m_vec[i] = l.m_vec[i] - r.m_vec[i]; - } - } - friend void operator*= (PxVector3 &l, const PxVector3 &r) { - for (int i=0; i < 3; i++) { - l.m_vec[i] = l.m_vec[i] * r.m_vec[i]; - } - } - friend void operator/= (PxVector3 &l, const PxVector3 &r) { - for (int i=0; i < 3; i++) { - l.m_vec[i] = l.m_vec[i] / r.m_vec[i]; - } - } + mavlink_mission_request_list_t request; - friend PxVector3 operator+ (const PxVector3 &l, float f) { - PxVector3 ret; - for (int i=0; i < 3; i++) { - ret.m_vec[i] = l.m_vec[i] + f; - } - return ret; - } - friend PxVector3 operator- (const PxVector3 &l, float f) { - PxVector3 ret; - for (int i=0; i < 3; i++) { - ret.m_vec[i] = l.m_vec[i] - f; - } - return ret; - } - friend PxVector3 operator* (const PxVector3 &l, float f) { - PxVector3 ret; - for (int i=0; i < 3; i++) { - ret.m_vec[i] = l.m_vec[i] * f; - } - return ret; - } - friend PxVector3 operator/ (const PxVector3 &l, float f) { - PxVector3 ret; - float inv = 1.f/f; - for (int i=0; i < 3; i++) { - ret.m_vec[i] = l.m_vec[i] * inv; - } - return ret; - } + mavlink_msg_mission_request_list_decode(&msg, &request); - friend void operator+= (PxVector3 &l, float f) { - for (int i=0; i < 3; i++) { - l.m_vec[i] = l.m_vec[i] + f; - } - } - friend void operator-= (PxVector3 &l, float f) { - for (int i=0; i < 3; i++) { - l.m_vec[i] = l.m_vec[i] - f; - } - } - friend void operator*= (PxVector3 &l, float f) { - for (int i=0; i < 3; i++) { - l.m_vec[i] = l.m_vec[i] * f; - } - } - friend void operator/= (PxVector3 &l, float f) { - float inv = 1.f/f; - for (int i=0; i < 3; i++) { - l.m_vec[i] = l.m_vec[i] * inv; - } - } + Q_ASSERT(request.target_system == _mockLink->vehicleId()); - // === vector operators === - /** @brief dot product */ - float dot(const PxVector3 &v) const { - return m_vec[0]*v.m_vec[0] + m_vec[1]*v.m_vec[1] + m_vec[2]*v.m_vec[2]; - } - /** @brief length squared of the vector */ - float lengthSquared(void) const { - return m_vec[0]*m_vec[0] + m_vec[1]*m_vec[1] + m_vec[2]*m_vec[2]; - } - /** @brief length of the vector */ - float length(void) const { - return sqrt(lengthSquared()); - } - /** @brief cross product */ - PxVector3 cross(const PxVector3 &v) const { - return PxVector3(m_vec[1]*v.m_vec[2] - m_vec[2]*v.m_vec[1], m_vec[2]*v.m_vec[0] - m_vec[0]*v.m_vec[2], m_vec[0]*v.m_vec[1] - m_vec[1]*v.m_vec[0]); - } - /** @brief normalizes the vector */ - PxVector3 &normalize(void) { - const float l = 1.f / length(); - for (int i=0; i < 3; i++) { - m_vec[i] *= l; - } - return *this; - } + mavlink_message_t responseMsg; - friend class PxMatrix3x3; -protected: - float m_vec[3]; -}; + mavlink_msg_mission_count_pack(_mockLink->vehicleId(), + MAV_COMP_ID_MISSIONPLANNER, + &responseMsg, // Outgoing message + msg.sysid, // Target is original sender + msg.compid, // Target is original sender + _missionItems.count()); // Number of mission items + _mockLink->respondWithMavlinkMessage(responseMsg); +} -/** - * @brief Pixhawk 3D vector class in double precision, can be cast to a local OpenCV CvMat. - * - */ -class PxVector3Double +void MockLinkMissionItemHandler::_handleMissionRequest(const mavlink_message_t& msg) { -public: - /** @brief standard constructor */ - PxVector3Double(void) {} - /** @brief copy constructor */ - PxVector3Double(const PxVector3Double &v) { - for (int i=0; i < 3; i++) { - m_vec[i] = v.m_vec[i]; - } - } - /** @brief x,y,z constructor */ - PxVector3Double(const double _x, const double _y, const double _z) { - m_vec[0] = _x; - m_vec[1] = _y; - m_vec[2] = _z; - } - /** @brief broadcast constructor */ - PxVector3Double(const double _f) { - for (int i=0; i < 3; i++) { - m_vec[i] = _f; - } - } + qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequest read sequence"; -private: - /** @brief private constructor (not used here, for SSE compatibility) */ - PxVector3Double(const double (&_vec)[3]) { - for (int i=0; i < 3; i++) { - m_vec[i] = _vec[i]; - } - } + mavlink_mission_request_t request; -public: - /** @brief assignment operator */ - void operator= (const PxVector3Double &r) { - for (int i=0; i < 3; i++) { - m_vec[i] = r.m_vec[i]; - } - } - /** @brief const element access */ - double operator[] (const int i) const { - return m_vec[i]; - } - /** @brief element access */ - double &operator[] (const int i) { - return m_vec[i]; - } + mavlink_msg_mission_request_decode(&msg, &request); - // === arithmetic operators === - /** @brief element-wise negation */ - friend PxVector3Double operator- (const PxVector3Double &v) { - PxVector3Double ret; - for (int i=0; i < 3; i++) { - ret.m_vec[i] = -v.m_vec[i]; - } - return ret; - } - friend PxVector3Double operator+ (const PxVector3Double &l, const PxVector3Double &r) { - PxVector3Double ret; - for (int i=0; i < 3; i++) { - ret.m_vec[i] = l.m_vec[i] + r.m_vec[i]; - } - return ret; - } - friend PxVector3Double operator- (const PxVector3Double &l, const PxVector3Double &r) { - PxVector3Double ret; - for (int i=0; i < 3; i++) { - ret.m_vec[i] = l.m_vec[i] - r.m_vec[i]; - } - return ret; - } - friend PxVector3Double operator* (const PxVector3Double &l, const PxVector3Double &r) { - PxVector3Double ret; - for (int i=0; i < 3; i++) { - ret.m_vec[i] = l.m_vec[i] * r.m_vec[i]; - } - return ret; - } - friend PxVector3Double operator/ (const PxVector3Double &l, const PxVector3Double &r) { - PxVector3Double ret; - for (int i=0; i < 3; i++) { - ret.m_vec[i] = l.m_vec[i] / r.m_vec[i]; - } - return ret; - } + Q_ASSERT(request.target_system == _mockLink->vehicleId()); + Q_ASSERT(request.seq < _missionItems.count()); - friend void operator+= (PxVector3Double &l, const PxVector3Double &r) { - for (int i=0; i < 3; i++) { - l.m_vec[i] = l.m_vec[i] + r.m_vec[i]; - } - } - friend void operator-= (PxVector3Double &l, const PxVector3Double &r) { - for (int i=0; i < 3; i++) { - l.m_vec[i] = l.m_vec[i] - r.m_vec[i]; - } - } - friend void operator*= (PxVector3Double &l, const PxVector3Double &r) { - for (int i=0; i < 3; i++) { - l.m_vec[i] = l.m_vec[i] * r.m_vec[i]; - } - } - friend void operator/= (PxVector3Double &l, const PxVector3Double &r) { - for (int i=0; i < 3; i++) { - l.m_vec[i] = l.m_vec[i] / r.m_vec[i]; - } - } + mavlink_message_t responseMsg; - friend PxVector3Double operator+ (const PxVector3Double &l, double f) { - PxVector3Double ret; - for (int i=0; i < 3; i++) { - ret.m_vec[i] = l.m_vec[i] + f; - } - return ret; - } - friend PxVector3Double operator- (const PxVector3Double &l, double f) { - PxVector3Double ret; - for (int i=0; i < 3; i++) { - ret.m_vec[i] = l.m_vec[i] - f; - } - return ret; - } - friend PxVector3Double operator* (const PxVector3Double &l, double f) { - PxVector3Double ret; - for (int i=0; i < 3; i++) { - ret.m_vec[i] = l.m_vec[i] * f; - } - return ret; - } - friend PxVector3Double operator/ (const PxVector3Double &l, double f) { - PxVector3Double ret; - double inv = 1.f/f; - for (int i=0; i < 3; i++) { - ret.m_vec[i] = l.m_vec[i] * inv; - } - return ret; - } - - friend void operator+= (PxVector3Double &l, double f) { - for (int i=0; i < 3; i++) { - l.m_vec[i] = l.m_vec[i] + f; - } - } - friend void operator-= (PxVector3Double &l, double f) { - for (int i=0; i < 3; i++) { - l.m_vec[i] = l.m_vec[i] - f; - } - } - friend void operator*= (PxVector3Double &l, double f) { - for (int i=0; i < 3; i++) { - l.m_vec[i] = l.m_vec[i] * f; - } - } - friend void operator/= (PxVector3Double &l, double f) { - double inv = 1.f/f; - for (int i=0; i < 3; i++) { - l.m_vec[i] = l.m_vec[i] * inv; - } - } - - // === vector operators === - /** @brief dot product */ - double dot(const PxVector3Double &v) const { - return m_vec[0]*v.m_vec[0] + m_vec[1]*v.m_vec[1] + m_vec[2]*v.m_vec[2]; - } - /** @brief length squared of the vector */ - double lengthSquared(void) const { - return m_vec[0]*m_vec[0] + m_vec[1]*m_vec[1] + m_vec[2]*m_vec[2]; - } - /** @brief length of the vector */ - double length(void) const { - return sqrt(lengthSquared()); - } - /** @brief cross product */ - PxVector3Double cross(const PxVector3Double &v) const { - return PxVector3Double(m_vec[1]*v.m_vec[2] - m_vec[2]*v.m_vec[1], m_vec[2]*v.m_vec[0] - m_vec[0]*v.m_vec[2], m_vec[0]*v.m_vec[1] - m_vec[1]*v.m_vec[0]); - } - /** @brief normalizes the vector */ - PxVector3Double &normalize(void) { - const double l = 1.f / length(); - for (int i=0; i < 3; i++) { - m_vec[i] *= l; - } - return *this; - } + mavlink_mission_item_t item = _missionItems[request.seq]; - friend class PxMatrix3x3; -protected: - double m_vec[3]; -}; - - - link(parent), - idle(false), - current_active_wp_id(-1), - yawReached(false), - posReached(false), - timestamp_lastoutside_orbit(0), - timestamp_firstinside_orbit(0), - waypoints(&waypoints1), - waypoints_receive_buffer(&waypoints2), - current_state(PX_WPP_IDLE), - protocol_current_wp_id(0), - protocol_current_count(0), - protocol_current_partner_systemid(0), - protocol_current_partner_compid(0), - protocol_timestamp_lastaction(0), - protocol_timeout(1000), - timestamp_last_send_setpoint(0), - systemid(sysid), - compid(MAV_COMP_ID_MISSIONPLANNER), - setpointDelay(10), - yawTolerance(0.4f), - verbose(true), - debug(false), - silent(false) -{ - connect(parent, SIGNAL(messageReceived(mavlink_message_t)), this, SLOT(handleMessage(mavlink_message_t))); - qDebug() << "PLANNER FOR SYSTEM" << systemid << "INITIALIZED"; -} - - - -/* -* @brief Sends an waypoint ack message -*/ -void MAVLinkSimulationWaypointPlanner::send_waypoint_ack(uint8_t target_systemid, uint8_t target_compid, uint8_t type) -{ - mavlink_message_t msg; - mavlink_mission_ack_t wpa; - - wpa.target_system = target_systemid; - wpa.target_component = target_compid; - wpa.type = type; - - mavlink_msg_mission_ack_encode(systemid, compid, &msg, &wpa); - link->sendMAVLinkMessage(&msg); - - - - if (verbose) qDebug("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system); -} - -/* -* @brief Broadcasts the new target waypoint and directs the MAV to fly there -* -* This function broadcasts its new active waypoint sequence number and -* sends a message to the controller, advising it to fly to the coordinates -* of the waypoint with a given orientation -* -* @param seq The waypoint sequence number the MAV should fly to. -*/ -void MAVLinkSimulationWaypointPlanner::send_waypoint_current(uint16_t seq) -{ - if(seq < waypoints->size()) { - mavlink_mission_item_t *cur = waypoints->at(seq); - - mavlink_message_t msg; - mavlink_mission_current_t wpc; - - wpc.seq = cur->seq; - - mavlink_msg_mission_current_encode(systemid, compid, &msg, &wpc); - link->sendMAVLinkMessage(&msg); - - - - if (verbose) qDebug("Broadcasted new current waypoint %u\n", wpc.seq); - } -} - -/* -* @brief Directs the MAV to fly to a position -* -* Sends a message to the controller, advising it to fly to the coordinates -* of the waypoint with a given orientation -* -* @param seq The waypoint sequence number the MAV should fly to. -*/ -void MAVLinkSimulationWaypointPlanner::send_setpoint(uint16_t seq) -{ - Q_UNUSED(seq); + mavlink_msg_mission_item_pack(_mockLink->vehicleId(), + MAV_COMP_ID_MISSIONPLANNER, + &responseMsg, // Outgoing message + msg.sysid, // Target is original sender + msg.compid, // Target is original sender + request.seq, // Index of mission item being sent + item.frame, + item.command, + item.current, + item.autocontinue, + item.param1, item.param2, item.param3, item.param4, + item.x, item.y, item.z); + _mockLink->respondWithMavlinkMessage(responseMsg); } -void MAVLinkSimulationWaypointPlanner::send_waypoint_count(uint8_t target_systemid, uint8_t target_compid, uint16_t count) +void MockLinkMissionItemHandler::_handleMissionCount(const mavlink_message_t& msg) { - mavlink_message_t msg; - mavlink_mission_count_t wpc; - - wpc.target_system = target_systemid; - wpc.target_component = target_compid; - wpc.count = count; - - mavlink_msg_mission_count_encode(systemid, compid, &msg, &wpc); - link->sendMAVLinkMessage(&msg); - - if (verbose) qDebug("Sent waypoint count (%u) to ID %u\n", wpc.count, wpc.target_system); - - -} - -void MAVLinkSimulationWaypointPlanner::send_waypoint(uint8_t target_systemid, uint8_t target_compid, uint16_t seq) -{ - if (seq < waypoints->size()) { - mavlink_message_t msg; - mavlink_mission_item_t *wp = waypoints->at(seq); - wp->target_system = target_systemid; - wp->target_component = target_compid; - - if (verbose) qDebug("Sent waypoint %u (%u / %u / %u / %u / %u / %f / %f / %f / %u / %f / %f / %f / %f / %u)\n", wp->seq, wp->target_system, wp->target_component, wp->seq, wp->frame, wp->command, wp->param3, wp->param1, wp->param2, wp->current, wp->x, wp->y, wp->z, wp->param4, wp->autocontinue); - - mavlink_msg_mission_item_encode(systemid, compid, &msg, wp); - link->sendMAVLinkMessage(&msg); - if (verbose) qDebug("Sent waypoint %u to ID %u\n", wp->seq, wp->target_system); - - + qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionCount write sequence"; + + mavlink_mission_count_t missionCount; + + mavlink_msg_mission_count_decode(&msg, &missionCount); + Q_ASSERT(missionCount.target_system == _mockLink->vehicleId()); + + _writeSequenceCount = missionCount.count; + Q_ASSERT(_writeSequenceCount >= 0); + + // FIXME: Set up a timer for a failed write sequence + + _missionItems.clear(); + + if (_writeSequenceCount == 0) { + // FIXME: NYI } else { - if (verbose) qDebug("ERROR: index out of bounds\n"); + _writeSequenceIndex = 0; + _requestNextMissionItem(_writeSequenceIndex); } } -void MAVLinkSimulationWaypointPlanner::send_waypoint_request(uint8_t target_systemid, uint8_t target_compid, uint16_t seq) -{ - mavlink_message_t msg; - mavlink_mission_request_t wpr; - wpr.target_system = target_systemid; - wpr.target_component = target_compid; - wpr.seq = seq; - mavlink_msg_mission_request_encode(systemid, compid, &msg, &wpr); - link->sendMAVLinkMessage(&msg); - if (verbose) qDebug("Sent waypoint request %u to ID %u\n", wpr.seq, wpr.target_system); - - -} - -/* -* @brief emits a message that a waypoint reached -* -* This function broadcasts a message that a waypoint is reached. -* -* @param seq The waypoint sequence number the MAV has reached. -*/ -void MAVLinkSimulationWaypointPlanner::send_waypoint_reached(uint16_t seq) +void MockLinkMissionItemHandler::_requestNextMissionItem(int sequenceNumber) { - mavlink_message_t msg; - mavlink_mission_item_reached_t wp_reached; - - wp_reached.seq = seq; - - mavlink_msg_mission_item_reached_encode(systemid, compid, &msg, &wp_reached); - link->sendMAVLinkMessage(&msg); - - if (verbose) qDebug("Sent waypoint %u reached message\n", wp_reached.seq); - - -} - -float MAVLinkSimulationWaypointPlanner::distanceToSegment(uint16_t seq, float x, float y, float z) -{ - if (seq < waypoints->size()) { - mavlink_mission_item_t *cur = waypoints->at(seq); - - const PxVector3 A(cur->x, cur->y, cur->z); - const PxVector3 C(x, y, z); - - // seq not the second last waypoint - if ((uint16_t)(seq+1) < waypoints->size()) { - mavlink_mission_item_t *next = waypoints->at(seq+1); - const PxVector3 B(next->x, next->y, next->z); - const float r = (B-A).dot(C-A) / (B-A).lengthSquared(); - if (r >= 0 && r <= 1) { - const PxVector3 P(A + r*(B-A)); - return (P-C).length(); - } else if (r < 0.f) { - return (C-A).length(); - } else { - return (C-B).length(); - } - } else { - return (C-A).length(); - } - } - return -1.f; -} - -float MAVLinkSimulationWaypointPlanner::distanceToPoint(uint16_t seq, float x, float y, float z) -{ - if (seq < waypoints->size()) { - mavlink_mission_item_t *cur = waypoints->at(seq); - - const PxVector3 A(cur->x, cur->y, cur->z); - const PxVector3 C(x, y, z); - - return (C-A).length(); + qCDebug(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem write sequence sequenceNumber:" << sequenceNumber; + + if (sequenceNumber >= _writeSequenceCount) { + qCWarning(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem requested seqeuence number > write count sequenceNumber::_writeSequenceCount" << sequenceNumber << _writeSequenceCount; + return; } - return -1.f; -} - -float MAVLinkSimulationWaypointPlanner::distanceToPoint(uint16_t seq, float x, float y) -{ - if (seq < waypoints->size()) { - mavlink_mission_item_t *cur = waypoints->at(seq); - - const PxVector3 A(cur->x, cur->y, 0); - const PxVector3 C(x, y, 0); + + mavlink_message_t message; + mavlink_mission_request_t missionRequest; + + missionRequest.target_system = MAVLinkProtocol::instance()->getSystemId(); + missionRequest.target_component = MAVLinkProtocol::instance()->getComponentId(); + missionRequest.seq = sequenceNumber; + + mavlink_msg_mission_request_encode(_mockLink->vehicleId(), MAV_COMP_ID_MISSIONPLANNER, &message, &missionRequest); + _mockLink->respondWithMavlinkMessage(message); - return (C-A).length(); - } - return -1.f; + // FIXME: Timeouts } -void MAVLinkSimulationWaypointPlanner::handleMessage(const mavlink_message_t& msg) -{ - mavlink_handler(&msg); -} -void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* msg) +void MockLinkMissionItemHandler::_handleMissionItem(const mavlink_message_t& msg) { - // Handle param messages -// paramClient->handleMAVLinkPacket(msg); - - //check for timed-out operations - - //qDebug() << "MAV: %d WAYPOINTPLANNER GOT MESSAGE" << systemid; - - uint64_t now = QGC::groundTimeMilliseconds(); - if (now-protocol_timestamp_lastaction > protocol_timeout && current_state != PX_WPP_IDLE) { - if (verbose) qDebug() << "Last operation (state=%u) timed out, changing state to PX_WPP_IDLE" << current_state; - current_state = PX_WPP_IDLE; - protocol_current_count = 0; - protocol_current_partner_systemid = 0; - protocol_current_partner_compid = 0; - protocol_current_wp_id = -1; - - if(waypoints->size() == 0) { - current_active_wp_id = -1; - } - } - - if(now-timestamp_last_send_setpoint > setpointDelay) { - send_setpoint(current_active_wp_id); - } - - switch(msg->msgid) { - case MAVLINK_MSG_ID_ATTITUDE: { - if(msg->sysid == systemid && current_active_wp_id < waypoints->size()) { - mavlink_mission_item_t *wp = waypoints->at(current_active_wp_id); - if(wp->frame == 1) { - mavlink_attitude_t att; - mavlink_msg_attitude_decode(msg, &att); - float yaw_tolerance = yawTolerance; - //compare current yaw - if (att.yaw - yaw_tolerance >= 0.0f && att.yaw + yaw_tolerance < 2.f*M_PI) { - if (att.yaw - yaw_tolerance <= wp->param4 && att.yaw + yaw_tolerance >= wp->param4) - yawReached = true; - } else if(att.yaw - yaw_tolerance < 0.0f) { - float lowerBound = 360.0f + att.yaw - yaw_tolerance; - if (lowerBound < wp->param4 || wp->param4 < att.yaw + yaw_tolerance) - yawReached = true; - } else { - float upperBound = att.yaw + yaw_tolerance - 2.f*M_PI; - if (att.yaw - yaw_tolerance < wp->param4 || wp->param4 < upperBound) - yawReached = true; - } - - // FIXME HACK: Ignore yaw: - - yawReached = true; - } - } - break; - } - - case MAVLINK_MSG_ID_LOCAL_POSITION_NED: { - if(msg->sysid == systemid && current_active_wp_id < waypoints->size()) { - mavlink_mission_item_t *wp = waypoints->at(current_active_wp_id); - - if(wp->frame == 1) { - mavlink_local_position_ned_t pos; - mavlink_msg_local_position_ned_decode(msg, &pos); - //qDebug() << "Received new position: x:" << pos.x << "| y:" << pos.y << "| z:" << pos.z; - - posReached = false; - - // compare current position (given in message) with current waypoint - float orbit = wp->param1; - - float dist; - if (wp->param2 == 0) { - dist = distanceToSegment(current_active_wp_id, pos.x, pos.y, pos.z); - } else { - dist = distanceToPoint(current_active_wp_id, pos.x, pos.y, pos.z); - } - - if (dist >= 0.f && dist <= orbit && yawReached) { - posReached = true; - } - } - } - break; - } - - case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: { - if(msg->sysid == systemid && current_active_wp_id < waypoints->size()) { - mavlink_mission_item_t *wp = waypoints->at(current_active_wp_id); - - if(wp->frame == 0) { - mavlink_global_position_int_t pos; - mavlink_msg_global_position_int_decode(msg, &pos); - - float x = static_cast(pos.lat)/1E7; - float y = static_cast(pos.lon)/1E7; - //float z = static_cast(pos.alt)/1000; - - //qDebug() << "Received new position: x:" << x << "| y:" << y << "| z:" << z; - - posReached = false; - yawReached = true; - - // FIXME big hack for simulation! - //float oneDegreeOfLatMeters = 111131.745f; - float orbit = 0.00008f; - - // compare current position (given in message) with current waypoint - //float orbit = wp->param1; - - // Convert to degrees - - - float dist; - dist = distanceToPoint(current_active_wp_id, x, y); - - if (dist >= 0.f && dist <= orbit && yawReached) { - posReached = true; - qDebug() << "WP PLANNER: REACHED POSITION"; - } - } - } - break; - } - - case MAVLINK_MSG_ID_COMMAND_LONG: - { // special action from ground station - mavlink_command_long_t action; - mavlink_msg_command_long_decode(msg, &action); - if(action.target_system == systemid) { - if (verbose) qDebug("MissionItem: received message with action %d\n", action.command); -// switch (action.action) { -// case MAV_ACTION_LAUNCH: -// if (verbose) std::cerr << "Launch received" << std::endl; -// current_active_wp_id = 0; -// if (waypoints->size()>0) -// { -// setActive(waypoints[current_active_wp_id]); -// } -// else -// if (verbose) std::cerr << "No launch, waypointList empty" << std::endl; -// break; - -// case MAV_ACTION_CONTINUE: -// if (verbose) std::c -// err << "Continue received" << std::endl; -// idle = false; -// setActive(waypoints[current_active_wp_id]); -// break; - -// case MAV_ACTION_HALT: -// if (verbose) std::cerr << "Halt received" << std::endl; -// idle = true; -// break; - -// default: -// if (verbose) std::cerr << "Unknown action received with id " << action.action << ", no action taken" << std::endl; -// break; -// } - } - break; - } - - case MAVLINK_MSG_ID_MISSION_ACK: { - mavlink_mission_ack_t wpa; - mavlink_msg_mission_ack_decode(msg, &wpa); - - if((msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid) && (wpa.target_system == systemid && wpa.target_component == compid)) { - protocol_timestamp_lastaction = now; - - if (current_state == PX_WPP_SENDLIST || current_state == PX_WPP_SENDLIST_SENDWPS) { - if (protocol_current_wp_id == waypoints->size()-1) { - if (verbose) qDebug("Received Ack after having sent last waypoint, going to state PX_WPP_IDLE\n"); - current_state = PX_WPP_IDLE; - protocol_current_wp_id = 0; - } - } - } - break; - } - - case MAVLINK_MSG_ID_MISSION_SET_CURRENT: { - mavlink_mission_set_current_t wpc; - mavlink_msg_mission_set_current_decode(msg, &wpc); - - if(wpc.target_system == systemid && wpc.target_component == compid) { - protocol_timestamp_lastaction = now; - - if (current_state == PX_WPP_IDLE) { - if (wpc.seq < waypoints->size()) { - if (verbose) qDebug("Received MAVLINK_MSG_ID_MISSION_ITEM_SET_CURRENT\n"); - current_active_wp_id = wpc.seq; - uint32_t i; - for(i = 0; i < waypoints->size(); i++) { - if (i == current_active_wp_id) { - waypoints->at(i)->current = true; - } else { - waypoints->at(i)->current = false; - } - } - if (verbose) qDebug("New current waypoint %u\n", current_active_wp_id); - yawReached = false; - posReached = false; - send_waypoint_current(current_active_wp_id); - send_setpoint(current_active_wp_id); - timestamp_firstinside_orbit = 0; - } else { - if (verbose) qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM_SET_CURRENT: Index out of bounds\n"); - } - } - } else { - qDebug() << "SYSTEM / COMPONENT ID MISMATCH: target sys:" << wpc.target_system << "this system:" << systemid << "target comp:" << wpc.target_component << "this comp:" << compid; - } - break; - } - - case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: { - mavlink_mission_request_list_t wprl; - mavlink_msg_mission_request_list_decode(msg, &wprl); - if(wprl.target_system == systemid && wprl.target_component == compid) { - protocol_timestamp_lastaction = now; - - if (current_state == PX_WPP_IDLE || current_state == PX_WPP_SENDLIST) { - if (waypoints->size() > 0) { - if (verbose && current_state == PX_WPP_IDLE) qDebug("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST from %u changing state to PX_WPP_SENDLIST\n", msg->sysid); - if (verbose && current_state == PX_WPP_SENDLIST) qDebug("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST again from %u staying in state PX_WPP_SENDLIST\n", msg->sysid); - current_state = PX_WPP_SENDLIST; - protocol_current_wp_id = 0; - protocol_current_partner_systemid = msg->sysid; - protocol_current_partner_compid = msg->compid; - } else { - if (verbose) qDebug("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST from %u but have no waypoints, staying in \n", msg->sysid); - } - protocol_current_count = static_cast(waypoints->size()); - send_waypoint_count(msg->sysid,msg->compid, protocol_current_count); - } else { - if (verbose) qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST because i'm doing something else already (state=%i).\n", current_state); - } - } else { - if (verbose) qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST because not my systemid or compid.\n"); - } - break; - } - - case MAVLINK_MSG_ID_MISSION_REQUEST: { - mavlink_mission_request_t wpr; - mavlink_msg_mission_request_decode(msg, &wpr); - if(msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid && wpr.target_system == systemid && wpr.target_component == compid) { - protocol_timestamp_lastaction = now; - - //ensure that we are in the correct state and that the first request has id 0 and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint) - if ((current_state == PX_WPP_SENDLIST && wpr.seq == 0) || (current_state == PX_WPP_SENDLIST_SENDWPS && (wpr.seq == protocol_current_wp_id || wpr.seq == protocol_current_wp_id + 1) && wpr.seq < waypoints->size())) { - if (verbose && current_state == PX_WPP_SENDLIST) qDebug("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); - if (verbose && current_state == PX_WPP_SENDLIST_SENDWPS && wpr.seq == protocol_current_wp_id + 1) qDebug("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); - if (verbose && current_state == PX_WPP_SENDLIST_SENDWPS && wpr.seq == protocol_current_wp_id) qDebug("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); - - current_state = PX_WPP_SENDLIST_SENDWPS; - protocol_current_wp_id = wpr.seq; - send_waypoint(protocol_current_partner_systemid, protocol_current_partner_compid, wpr.seq); - } else { - if (verbose) { - if (!(current_state == PX_WPP_SENDLIST || current_state == PX_WPP_SENDLIST_SENDWPS)) { - qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).\n", current_state); - break; - } else if (current_state == PX_WPP_SENDLIST) { - if (wpr.seq != 0) qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the first requested waypoint ID (%u) was not 0.\n", wpr.seq); - } else if (current_state == PX_WPP_SENDLIST_SENDWPS) { - if (wpr.seq != protocol_current_wp_id && wpr.seq != protocol_current_wp_id + 1) qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).\n", wpr.seq, protocol_current_wp_id, protocol_current_wp_id+1); - else if (wpr.seq >= waypoints->size()) qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.\n", wpr.seq); - } else qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST - FIXME: missed error description\n"); - } - } - } else { - //we we're target but already communicating with someone else - if((wpr.target_system == systemid && wpr.target_component == compid) && !(msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid)) { - if (verbose) qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.\n", msg->sysid, protocol_current_partner_systemid); - } - } - break; - } - - case MAVLINK_MSG_ID_MISSION_COUNT: { - mavlink_mission_count_t wpc; - mavlink_msg_mission_count_decode(msg, &wpc); - if(wpc.target_system == systemid && wpc.target_component == compid) { - protocol_timestamp_lastaction = now; - - if (current_state == PX_WPP_IDLE || (current_state == PX_WPP_GETLIST && protocol_current_wp_id == 0)) { - if (wpc.count > 0) { - if (verbose && current_state == PX_WPP_IDLE) qDebug("Got MAVLINK_MSG_ID_MISSION_COUNT (%u) from %u changing state to PX_WPP_GETLIST\n", wpc.count, msg->sysid); - if (verbose && current_state == PX_WPP_GETLIST) qDebug("Got MAVLINK_MSG_ID_MISSION_COUNT (%u) again from %u\n", wpc.count, msg->sysid); - - current_state = PX_WPP_GETLIST; - protocol_current_wp_id = 0; - protocol_current_partner_systemid = msg->sysid; - protocol_current_partner_compid = msg->compid; - protocol_current_count = wpc.count; - - qDebug("clearing receive buffer and readying for receiving waypoints\n"); - while(waypoints_receive_buffer->size() > 0) { - delete waypoints_receive_buffer->back(); - waypoints_receive_buffer->pop_back(); - } - - send_waypoint_request(protocol_current_partner_systemid, protocol_current_partner_compid, protocol_current_wp_id); - } else { - if (verbose) qDebug("Ignoring MAVLINK_MSG_ID_MISSION_COUNT from %u with count of %u\n", msg->sysid, wpc.count); - } - } else { - if (verbose && !(current_state == PX_WPP_IDLE || current_state == PX_WPP_GETLIST)) qDebug("Ignored MAVLINK_MSG_ID_MISSION_COUNT because i'm doing something else already (state=%i).\n", current_state); - else if (verbose && current_state == PX_WPP_GETLIST && protocol_current_wp_id != 0) qDebug("Ignored MAVLINK_MSG_ID_MISSION_COUNT because i'm already receiving waypoint %u.\n", protocol_current_wp_id); - else qDebug("Ignored MAVLINK_MSG_ID_MISSION_COUNT - FIXME: missed error description\n"); - } - } - break; - } - - case MAVLINK_MSG_ID_MISSION_ITEM: { - mavlink_mission_item_t wp; - mavlink_msg_mission_item_decode(msg, &wp); - - if((msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid) && (wp.target_system == systemid && wp.target_component == compid)) { - protocol_timestamp_lastaction = now; - - //ensure that we are in the correct state and that the first waypoint has id 0 and the following waypoints have the correct ids - if ((current_state == PX_WPP_GETLIST && wp.seq == 0) || (current_state == PX_WPP_GETLIST_GETWPS && wp.seq == protocol_current_wp_id && wp.seq < protocol_current_count)) { - if (verbose && current_state == PX_WPP_GETLIST) qDebug("Got MAVLINK_MSG_ID_MISSION_ITEM %u from %u changing state to PX_WPP_GETLIST_GETWPS\n", wp.seq, msg->sysid); - if (verbose && current_state == PX_WPP_GETLIST_GETWPS && wp.seq == protocol_current_wp_id) qDebug("Got MAVLINK_MSG_ID_MISSION_ITEM %u from %u\n", wp.seq, msg->sysid); - if (verbose && current_state == PX_WPP_GETLIST_GETWPS && wp.seq-1 == protocol_current_wp_id) qDebug("Got MAVLINK_MSG_ID_MISSION_ITEM %u (again) from %u\n", wp.seq, msg->sysid); - - current_state = PX_WPP_GETLIST_GETWPS; - protocol_current_wp_id = wp.seq + 1; - mavlink_mission_item_t* newwp = new mavlink_mission_item_t; - memcpy(newwp, &wp, sizeof(mavlink_mission_item_t)); - waypoints_receive_buffer->push_back(newwp); - - if(protocol_current_wp_id == protocol_current_count && current_state == PX_WPP_GETLIST_GETWPS) { - if (verbose) qDebug("Got all %u waypoints, changing state to PX_WPP_IDLE\n", protocol_current_count); - - send_waypoint_ack(protocol_current_partner_systemid, protocol_current_partner_compid, 0); - - if (current_active_wp_id > waypoints_receive_buffer->size()-1) { - current_active_wp_id = static_cast(waypoints_receive_buffer->size()) - 1; - } - - // switch the waypoints list - std::vector* waypoints_temp = waypoints; - waypoints = waypoints_receive_buffer; - waypoints_receive_buffer = waypoints_temp; - - //get the new current waypoint - uint32_t i; - for(i = 0; i < waypoints->size(); i++) { - if (waypoints->at(i)->current == 1) { - current_active_wp_id = i; - //if (verbose) qDebug("New current waypoint %u\n", current_active_wp_id); - yawReached = false; - posReached = false; - send_waypoint_current(current_active_wp_id); - send_setpoint(current_active_wp_id); - timestamp_firstinside_orbit = 0; - break; - } - } - - if (i == waypoints->size()) { - current_active_wp_id = -1; - yawReached = false; - posReached = false; - timestamp_firstinside_orbit = 0; - } - - current_state = PX_WPP_IDLE; - } else { - send_waypoint_request(protocol_current_partner_systemid, protocol_current_partner_compid, protocol_current_wp_id); - } - } else { - if (current_state == PX_WPP_IDLE) { - //we're done receiving waypoints, answer with ack. - send_waypoint_ack(protocol_current_partner_systemid, protocol_current_partner_compid, 0); - qDebug("Received MAVLINK_MSG_ID_MISSION_ITEM while state=PX_WPP_IDLE, answered with WAYPOINT_ACK.\n"); - } - if (verbose) { - if (!(current_state == PX_WPP_GETLIST || current_state == PX_WPP_GETLIST_GETWPS)) { - qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u because i'm doing something else already (state=%i).\n", wp.seq, current_state); - break; - } else if (current_state == PX_WPP_GETLIST) { - if(!(wp.seq == 0)) qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the first waypoint ID (%u) was not 0.\n", wp.seq); - else qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq); - } else if (current_state == PX_WPP_GETLIST_GETWPS) { - if (!(wp.seq == protocol_current_wp_id)) qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was not the expected %u.\n", wp.seq, protocol_current_wp_id); - else if (!(wp.seq < protocol_current_count)) qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was out of bounds.\n", wp.seq); - else qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq); - } else qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq); - } - } - } else { - // We're target but already communicating with someone else - if((wp.target_system == systemid && wp.target_component == compid) && !(msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid) && current_state != PX_WPP_IDLE) { - if (verbose) qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u from ID %u because i'm already talking to ID %u.\n", wp.seq, msg->sysid, protocol_current_partner_systemid); - } else if(wp.target_system == systemid && wp.target_component == compid) { - if (verbose) qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u from ID %u because i have no idea what to do with it\n", wp.seq, msg->sysid); - } - } - break; - } - - case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: { - mavlink_mission_clear_all_t wpca; - mavlink_msg_mission_clear_all_decode(msg, &wpca); - - if(wpca.target_system == systemid && wpca.target_component == compid && current_state == PX_WPP_IDLE) { - protocol_timestamp_lastaction = now; - - if (verbose) qDebug("Got MAVLINK_MSG_ID_MISSION_ITEM_CLEAR_LIST from %u deleting all waypoints\n", msg->sysid); - while(waypoints->size() > 0) { - delete waypoints->back(); - waypoints->pop_back(); - } - current_active_wp_id = -1; - } else if (wpca.target_system == systemid && wpca.target_component == compid && current_state != PX_WPP_IDLE) { - if (verbose) qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, current_state); - } - break; - } - - default: { - if (debug) qDebug("MissionItem: received message of unknown type\n"); - break; - } - } - - //check if the current waypoint was reached - if ((posReached && /*yawReached &&*/ !idle)) { - if (current_active_wp_id < waypoints->size()) { - mavlink_mission_item_t *cur_wp = waypoints->at(current_active_wp_id); - - if (timestamp_firstinside_orbit == 0) { - // Announce that last waypoint was reached - if (verbose) qDebug("*** Reached waypoint %u ***\n", cur_wp->seq); - send_waypoint_reached(cur_wp->seq); - timestamp_firstinside_orbit = now; - } - - // check if the MAV was long enough inside the waypoint orbit - //if (now-timestamp_lastoutside_orbit > (cur_wp->hold_time*1000)) - if(now-timestamp_firstinside_orbit >= cur_wp->param2*1000) { - if (cur_wp->autocontinue) { - cur_wp->current = 0; - if (current_active_wp_id == waypoints->size() - 1 && waypoints->size() > 0) { - //the last waypoint was reached, if auto continue is - //activated restart the waypoint list from the beginning - current_active_wp_id = 0; - } else { - current_active_wp_id++; - } - - // Fly to next waypoint - timestamp_firstinside_orbit = 0; - send_waypoint_current(current_active_wp_id); - send_setpoint(current_active_wp_id); - waypoints->at(current_active_wp_id)->current = true; - posReached = false; - //yawReached = false; - if (verbose) qDebug("Set new waypoint (%u)\n", current_active_wp_id); - } - } - } + qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionItem write sequence"; + + mavlink_mission_item_t missionItem; + + mavlink_msg_mission_item_decode(&msg, &missionItem); + + Q_ASSERT(missionItem.target_system == _mockLink->vehicleId()); + + Q_ASSERT(!_missionItems.contains(missionItem.seq)); + Q_ASSERT(missionItem.seq == _writeSequenceIndex); + + _missionItems[missionItem.seq] = missionItem; + + // FIXME: Timeouts + + _writeSequenceIndex++; + if (_writeSequenceIndex < _writeSequenceCount) { + _requestNextMissionItem(_writeSequenceIndex); } else { - timestamp_lastoutside_orbit = now; + qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionItem sending final ack, write sequence complete"; + mavlink_message_t message; + mavlink_mission_ack_t missionAck; + + missionAck.target_system = MAVLinkProtocol::instance()->getSystemId(); + missionAck.target_component = MAVLinkProtocol::instance()->getComponentId(); + missionAck.type = MAV_MISSION_ACCEPTED; + + mavlink_msg_mission_ack_encode(_mockLink->vehicleId(), MAV_COMP_ID_MISSIONPLANNER, &message, &missionAck); + _mockLink->respondWithMavlinkMessage(message); } } - -#endif \ No newline at end of file diff --git a/src/comm/MockLinkMissionItemHandler.h b/src/comm/MockLinkMissionItemHandler.h index 801c80e53459f80ca3b8169fb09e5aa3f53cf404..2978b55ccf3fc7ffacdc637016a966a15ad76218 100644 --- a/src/comm/MockLinkMissionItemHandler.h +++ b/src/comm/MockLinkMissionItemHandler.h @@ -21,90 +21,46 @@ ======================================================================*/ -#ifndef MOCKLINKMISSIONITEMHANDLER_H -#define MOCKLINKMISSIONITEMHANDLER_H - -// FIXME: This file is a work in progress +#ifndef MockLinkMissionItemHandler_H +#define MockLinkMissionItemHandler_H #include -#include +#include #include "QGCMAVLink.h" +#include "QGCLoggingCategory.h" -/* Alreedy defined in MAVLinkSimulationLink.h above! -enum PX_WAYPOINTPLANNER_STATES { - PX_WPP_IDLE = 0, - PX_WPP_SENDLIST, - PX_WPP_SENDLIST_SENDWPS, - PX_WPP_GETLIST, - PX_WPP_GETLIST_GETWPS, - PX_WPP_GETLIST_GOTALL -}; -*/ +class MockLink; + +Q_DECLARE_LOGGING_CATEGORY(MockLinkMissionItemHandlerLog) class MockLinkMissionItemHandler : public QObject { Q_OBJECT public: - MockLinkMissionItemHandler(uint16_t systemId, QObject* parent = NULL); + MockLinkMissionItemHandler(MockLink* mockLink); /// @brief Called to handle mission item related messages. All messages should be passed to this method. /// It will handle the appropriate set. - void handleMessage(const mavlink_message_t& msg); - -#if 0 -signals: - void messageSent(const mavlink_message_t& msg); - -protected: - MAVLinkSimulationLink* link; - bool idle; ///< indicates if the system is following the waypoints or is waiting - uint16_t current_active_wp_id; ///< id of current waypoint - bool yawReached; ///< boolean for yaw attitude reached - bool posReached; ///< boolean for position reached - uint64_t timestamp_lastoutside_orbit;///< timestamp when the MAV was last outside the orbit or had the wrong yaw value - uint64_t timestamp_firstinside_orbit;///< timestamp when the MAV was the first time after a waypoint change inside the orbit and had the correct yaw value - - std::vector waypoints1; ///< vector1 that holds the waypoints - std::vector waypoints2; ///< vector2 that holds the waypoints - - std::vector* waypoints; ///< pointer to the currently active waypoint vector - std::vector* waypoints_receive_buffer; ///< pointer to the receive buffer waypoint vector - PX_WAYPOINTPLANNER_STATES current_state; - uint16_t protocol_current_wp_id; - uint16_t protocol_current_count; - uint8_t protocol_current_partner_systemid; - uint8_t protocol_current_partner_compid; - uint64_t protocol_timestamp_lastaction; - unsigned int protocol_timeout; - uint64_t timestamp_last_send_setpoint; - uint8_t systemid; - uint8_t compid; - unsigned int setpointDelay; - float yawTolerance; - bool verbose; - bool debug; - bool silent; - - void send_waypoint_ack(uint8_t target_systemid, uint8_t target_compid, uint8_t type); - void send_waypoint_current(uint16_t seq); - void send_setpoint(uint16_t seq); - void send_waypoint_count(uint8_t target_systemid, uint8_t target_compid, uint16_t count); - void send_waypoint(uint8_t target_systemid, uint8_t target_compid, uint16_t seq); - void send_waypoint_request(uint8_t target_systemid, uint8_t target_compid, uint16_t seq); - void send_waypoint_reached(uint16_t seq); - float distanceToSegment(uint16_t seq, float x, float y, float z); - float distanceToPoint(uint16_t seq, float x, float y, float z); - float distanceToPoint(uint16_t seq, float x, float y); - void mavlink_handler(const mavlink_message_t* msg); -#endif + /// @return true: message handled + bool handleMessage(const mavlink_message_t& msg); private: - uint16_t _vehicleSystemId; ///< System id of this vehicle - - QList _missionItems; ///< Current set of mission itemss + void _handleMissionRequestList(const mavlink_message_t& msg); + void _handleMissionRequest(const mavlink_message_t& msg); + void _handleMissionItem(const mavlink_message_t& msg); + void _handleMissionCount(const mavlink_message_t& msg); + void _requestNextMissionItem(int sequenceNumber); +private: + MockLink* _mockLink; + + int _writeSequenceCount; ///< Numbers of items about to be written + int _writeSequenceIndex; ///< Current index being reqested + + typedef QMap MissionList_t; + MissionList_t _missionItems; }; -#endif // MAVLINKSIMULATIONWAYPOINTPLANNER_H +#endif diff --git a/src/qgcunittest/MultiSignalSpy.cc b/src/qgcunittest/MultiSignalSpy.cc index d9f786f4f4641333ce49f831c26f4d24a7f2082b..9bcc3124c1d3d59894378a18d125b57302b23b1f 100644 --- a/src/qgcunittest/MultiSignalSpy.cc +++ b/src/qgcunittest/MultiSignalSpy.cc @@ -223,10 +223,7 @@ bool MultiSignalSpy::waitForSignalByIndex( Q_ASSERT(spy); while (spy->count() == 0 && !_timeout) { - QCoreApplication::sendPostedEvents(); - QCoreApplication::processEvents(); - QCoreApplication::flush(); - QTest::qSleep(100); + QTest::qWait(100); } // Clean up and return status diff --git a/src/uas/UASWaypointManager.cc b/src/uas/UASWaypointManager.cc index bc48bdae063c6d19493ddb987c1af71e85509456..5b21e9eb214b11bd3f55a508a25d7c54914a0e33 100644 --- a/src/uas/UASWaypointManager.cc +++ b/src/uas/UASWaypointManager.cc @@ -199,14 +199,14 @@ void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_ MissionItem *lwp_vo = new MissionItem(NULL, wp->seq, QGeoCoordinate(wp->x, wp->y, wp->z), + (MAV_CMD) wp->command, wp->param1, wp->param2, wp->param3, wp->param4, wp->autocontinue, wp->current, - (MAV_FRAME) wp->frame, - (MAV_CMD) wp->command); + (MAV_FRAME) wp->frame); addWaypointViewOnly(lwp_vo); @@ -214,14 +214,14 @@ void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_ MissionItem *lwp_ed = new MissionItem(NULL, wp->seq, QGeoCoordinate(wp->x, wp->y, wp->z), + (MAV_CMD) wp->command, wp->param1, wp->param2, wp->param3, wp->param4, wp->autocontinue, wp->current, - (MAV_FRAME) wp->frame, - (MAV_CMD) wp->command); + (MAV_FRAME) wp->frame); addWaypointEditable(lwp_ed, false); if (wp->current == 1) currentWaypointEditable = lwp_ed; } @@ -256,14 +256,14 @@ void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_ MissionItem *lwp_vo = new MissionItem(NULL, wp->seq, QGeoCoordinate(wp->x, wp->y, wp->z), + (MAV_CMD) wp->command, wp->param1, wp->param2, wp->param3, wp->param4, wp->autocontinue, wp->current, - (MAV_FRAME) wp->frame, - (MAV_CMD) wp->command); + (MAV_FRAME) wp->frame); waypointsViewOnly.replace(wp->seq, lwp_vo); emit waypointViewOnlyListChanged(); diff --git a/src/ui/SettingsDialog.cc b/src/ui/SettingsDialog.cc index 8200c9f69dd611a533238f3b31bbe6312455a505..ea9862b5f4a8b2b09c9d4a2cb5aa1da8a77f616e 100644 --- a/src/ui/SettingsDialog.cc +++ b/src/ui/SettingsDialog.cc @@ -37,6 +37,7 @@ #include "QGCFileDialog.h" #include "QGCMessageBox.h" #include "MainToolBar.h" +#include "FlightMapSettings.h" SettingsDialog::SettingsDialog(QWidget *parent, int showTab, Qt::WindowFlags flags) : QDialog(parent, flags), @@ -94,7 +95,18 @@ _ui(new Ui::SettingsDialog) connect(_ui->styleChooser, SIGNAL(currentIndexChanged(int)), this, SLOT(styleChanged(int))); connect(_ui->browseSavedFilesLocation, &QPushButton::clicked, this, &SettingsDialog::_selectSavedFilesDirectory); connect(_ui->buttonBox, &QDialogButtonBox::accepted, this, &SettingsDialog::_validateBeforeClose); - + + // Flight Map settings + + FlightMapSettings* fmSettings = FlightMapSettings::instance(); + _ui->bingMapRadio->setChecked(fmSettings->mapProvider() == "Bing"); + _ui->googleMapRadio->setChecked(fmSettings->mapProvider() == "Google"); + _ui->openMapRadio->setChecked(fmSettings->mapProvider() == "Open"); + + connect(_ui->bingMapRadio, &QRadioButton::clicked, this, &SettingsDialog::_bingMapRadioClicked); + connect(_ui->googleMapRadio, &QRadioButton::clicked, this, &SettingsDialog::_googleMapRadioClicked); + connect(_ui->openMapRadio, &QRadioButton::clicked, this, &SettingsDialog::_openMapRadioClicked); + switch (showTab) { case ShowCommLinks: _ui->tabWidget->setCurrentWidget(pLinkConf); @@ -193,3 +205,24 @@ void SettingsDialog::on_showRSSI_clicked(bool checked) { _mainWindow->getMainToolBar()->viewStateChanged(TOOL_BAR_SHOW_RSSI, checked); } + +void SettingsDialog::_bingMapRadioClicked(bool checked) +{ + if (checked) { + FlightMapSettings::instance()->setMapProvider("Bing"); + } +} + +void SettingsDialog::_googleMapRadioClicked(bool checked) +{ + if (checked) { + FlightMapSettings::instance()->setMapProvider("Google"); + } +} + +void SettingsDialog::_openMapRadioClicked(bool checked) +{ + if (checked) { + FlightMapSettings::instance()->setMapProvider("Open"); + } +} diff --git a/src/ui/SettingsDialog.h b/src/ui/SettingsDialog.h index ac1a31108ea065452367adf75ff2e0d74048538a..f991e9b79ab402d920f39275ccd0489da065292a 100644 --- a/src/ui/SettingsDialog.h +++ b/src/ui/SettingsDialog.h @@ -62,6 +62,10 @@ private slots: void on_showMav_clicked(bool checked); void on_showRSSI_clicked(bool checked); + + void _bingMapRadioClicked(bool checked); + void _googleMapRadioClicked(bool checked); + void _openMapRadioClicked(bool checked); private: MainWindow* _mainWindow; diff --git a/src/ui/SettingsDialog.ui b/src/ui/SettingsDialog.ui index 036595c8181c5e781afccf83a1b511923c1bf39a..0db0fa3fffed2086d347cccfc6379857d337a94c 100644 --- a/src/ui/SettingsDialog.ui +++ b/src/ui/SettingsDialog.ui @@ -7,7 +7,7 @@ 0 0 500 - 596 + 689 @@ -115,6 +115,36 @@ + + + + Map Provider + + + + + + Bing + + + + + + + Google + + + + + + + Open Streets + + + + + + diff --git a/src/ui/WaypointList.cc b/src/ui/WaypointList.cc index d70f94564d8237feb91a919a4f84c85c7f46dab3..f47cf7dcf610c475d529b4c1edaa16ff8cb75b1a 100644 --- a/src/ui/WaypointList.cc +++ b/src/ui/WaypointList.cc @@ -304,9 +304,17 @@ void WaypointList::addEditable(bool onCurrentPosition) } else { updateStatusLabel(tr("Added default GLOBAL (Relative alt.) waypoint.")); } - wp = new MissionItem(NULL, 0, + wp = new MissionItem(NULL, + 0, QGeoCoordinate(uas->getLatitude(), uas->getLongitude(), uas->getAltitudeAMSL()), - 0, WPM->getAcceptanceRadiusRecommendation(), 0, 0,true, true, (MAV_FRAME)WPM->getFrameRecommendation(), MAV_CMD_NAV_WAYPOINT); + MAV_CMD_NAV_WAYPOINT, + 0, + WPM->getAcceptanceRadiusRecommendation(), + 0, + 0, + true, + true, + (MAV_FRAME)WPM->getFrameRecommendation()); WPM->addWaypointEditable(wp); } else { @@ -316,9 +324,17 @@ void WaypointList::addEditable(bool onCurrentPosition) } else { updateStatusLabel(tr("Added default GLOBAL (Relative alt.) waypoint.")); } - wp = new MissionItem(NULL, 0, + wp = new MissionItem(NULL, + 0, QGeoCoordinate(HomePositionManager::instance()->getHomeLatitude(), HomePositionManager::instance()->getHomeLongitude(), WPM->getAltitudeRecommendation()), - 0, WPM->getAcceptanceRadiusRecommendation(), 0, 0,true, true, (MAV_FRAME)WPM->getFrameRecommendation(), MAV_CMD_NAV_WAYPOINT); + MAV_CMD_NAV_WAYPOINT, + 0, + WPM->getAcceptanceRadiusRecommendation(), + 0, + 0, + true, + true, + (MAV_FRAME)WPM->getFrameRecommendation()); WPM->addWaypointEditable(wp); } } @@ -327,15 +343,31 @@ void WaypointList::addEditable(bool onCurrentPosition) if (onCurrentPosition) { updateStatusLabel(tr("Added default LOCAL (NED) waypoint.")); - wp = new MissionItem(NULL, 0, + wp = new MissionItem(NULL, + 0, QGeoCoordinate(uas->getLocalX(), uas->getLocalY(), uas->getLocalZ()), - 0, WPM->getAcceptanceRadiusRecommendation(), 0, 0,true, true, MAV_FRAME_LOCAL_NED, MAV_CMD_NAV_WAYPOINT); + MAV_CMD_NAV_WAYPOINT, + 0, + WPM->getAcceptanceRadiusRecommendation(), + 0, + 0, + true, + true, + MAV_FRAME_LOCAL_NED); WPM->addWaypointEditable(wp); } else { updateStatusLabel(tr("Added default LOCAL (NED) waypoint.")); - wp = new MissionItem(0, 0, + wp = new MissionItem(0, + 0, QGeoCoordinate(0, 0, -0.50), - 0, WPM->getAcceptanceRadiusRecommendation(), 0, 0,true, true, MAV_FRAME_LOCAL_NED, MAV_CMD_NAV_WAYPOINT); + MAV_CMD_NAV_WAYPOINT, + 0, + WPM->getAcceptanceRadiusRecommendation(), + 0, + 0, + true, + true, + MAV_FRAME_LOCAL_NED); WPM->addWaypointEditable(wp); } } @@ -343,9 +375,17 @@ void WaypointList::addEditable(bool onCurrentPosition) { // MAV connected, but position unknown, add default waypoint updateStatusLabel(tr("WARNING: No position known. Adding default LOCAL (NED) waypoint")); - wp = new MissionItem(NULL, 0, + wp = new MissionItem(NULL, + 0, QGeoCoordinate(HomePositionManager::instance()->getHomeLatitude(), HomePositionManager::instance()->getHomeLongitude(), WPM->getAltitudeRecommendation()), - 0, WPM->getAcceptanceRadiusRecommendation(), 0, 0,true, true, (MAV_FRAME)WPM->getFrameRecommendation(), MAV_CMD_NAV_WAYPOINT); + MAV_CMD_NAV_WAYPOINT, + 0, + WPM->getAcceptanceRadiusRecommendation(), + 0, + 0, + true, + true, + (MAV_FRAME)WPM->getFrameRecommendation()); WPM->addWaypointEditable(wp); } } @@ -353,9 +393,17 @@ void WaypointList::addEditable(bool onCurrentPosition) { //Since no UAV available, create first default waypoint. updateStatusLabel(tr("No UAV connected. Adding default GLOBAL (NED) waypoint")); - wp = new MissionItem(NULL, 0, + wp = new MissionItem(NULL, + 0, QGeoCoordinate(HomePositionManager::instance()->getHomeLatitude(), HomePositionManager::instance()->getHomeLongitude(), WPM->getAltitudeRecommendation()), - 0, WPM->getAcceptanceRadiusRecommendation(), 0, 0,true, true, (MAV_FRAME)WPM->getFrameRecommendation(), MAV_CMD_NAV_WAYPOINT); + MAV_CMD_NAV_WAYPOINT, + 0, + WPM->getAcceptanceRadiusRecommendation(), + 0, + 0, + true, + true, + (MAV_FRAME)WPM->getFrameRecommendation()); WPM->addWaypointEditable(wp); } }