diff --git a/src/HomePositionManager.cc b/src/HomePositionManager.cc index a53209394855bf175117900aab1eb538b3b79ea4..d8ca76e011d501c7a218243e6b79cf04acccde63 100644 --- a/src/HomePositionManager.cc +++ b/src/HomePositionManager.cc @@ -40,24 +40,47 @@ IMPLEMENT_QGC_SINGLETON(HomePositionManager, HomePositionManager) -HomePositionManager::HomePositionManager(QObject* parent) : - QObject(parent), - homeLat(47.3769), - homeLon(8.549444), - homeAlt(470.0), - homeFrame(MAV_FRAME_GLOBAL) +const char* HomePositionManager::_settingsGroup = "HomePositionManager"; +const char* HomePositionManager::_latitudeKey = "Latitude"; +const char* HomePositionManager::_longitudeKey = "Longitude"; +const char* HomePositionManager::_altitudeKey = "Altitude"; + +HomePositionManager::HomePositionManager(QObject* parent) + : QObject(parent) + , homeLat(47.3769) + , homeLon(8.549444) + , homeAlt(470.0) { - loadSettings(); + _loadSettings(); } HomePositionManager::~HomePositionManager() { - storeSettings(); + } -void HomePositionManager::storeSettings() +void HomePositionManager::_storeSettings(void) { QSettings settings; + + settings.remove(_settingsGroup); + settings.beginGroup(_settingsGroup); + + for (int i=0; i<_homePositions.count(); i++) { + HomePosition* homePos = qobject_cast(_homePositions[i]); + + qDebug() << "Saving" << homePos->name(); + + settings.beginGroup(homePos->name()); + settings.setValue(_latitudeKey, homePos->coordinate().latitude()); + settings.setValue(_longitudeKey, homePos->coordinate().longitude()); + settings.setValue(_altitudeKey, homePos->coordinate().altitude()); + settings.endGroup(); + } + + settings.endGroup(); + + // Deprecated settings for old editor settings.beginGroup("QGC_UASMANAGER"); settings.setValue("HOMELAT", homeLat); settings.setValue("HOMELON", homeLon); @@ -65,9 +88,36 @@ void HomePositionManager::storeSettings() settings.endGroup(); } -void HomePositionManager::loadSettings() +void HomePositionManager::_loadSettings(void) { QSettings settings; + + _homePositions.clear(); + + settings.beginGroup(_settingsGroup); + + foreach(QString name, settings.childGroups()) { + QGeoCoordinate coordinate; + + qDebug() << "Load setting" << name; + + settings.beginGroup(name); + coordinate.setLatitude(settings.value(_latitudeKey).toDouble()); + coordinate.setLongitude(settings.value(_longitudeKey).toDouble()); + coordinate.setAltitude(settings.value(_altitudeKey).toDouble()); + settings.endGroup(); + + _homePositions.append(new HomePosition(name, coordinate, this)); + } + + settings.endGroup(); + + if (_homePositions.count() == 0) { + _homePositions.append(new HomePosition("ETH Campus", QGeoCoordinate(47.3769, 8.549444, 470.0))); + } + + // Deprecated settings for old editor + settings.beginGroup("QGC_UASMANAGER"); bool changed = setHomePosition(settings.value("HOMELAT", homeLat).toDouble(), settings.value("HOMELON", homeLon).toDouble(), @@ -97,9 +147,6 @@ bool HomePositionManager::setHomePosition(double lat, double lon, double alt) if (fabs(homeLon - lon) > 1e-7) changed = true; if (fabs(homeAlt - alt) > 0.5f) changed = true; - // Initialize conversion reference in any case - initReference(lat, lon, alt); - if (changed) { homeLat = lat; @@ -125,75 +172,81 @@ bool HomePositionManager::setHomePositionAndNotify(double lat, double lon, doubl return changed; } -void HomePositionManager::initReference(const double & latitude, const double & longitude, const double & altitude) +void HomePositionManager::updateHomePosition(const QString& name, const QGeoCoordinate& coordinate) { - Eigen::Matrix3d R; - double s_long, s_lat, c_long, c_lat; - sincos(latitude * DEG2RAD, &s_lat, &c_lat); - sincos(longitude * DEG2RAD, &s_long, &c_long); - - R(0, 0) = -s_long; - R(0, 1) = c_long; - R(0, 2) = 0; - - R(1, 0) = -s_lat * c_long; - R(1, 1) = -s_lat * s_long; - R(1, 2) = c_lat; - - R(2, 0) = c_lat * c_long; - R(2, 1) = c_lat * s_long; - R(2, 2) = s_lat; - - ecef_ref_orientation_ = Eigen::Quaterniond(R); - - ecef_ref_point_ = wgs84ToEcef(latitude, longitude, altitude); + HomePosition * homePos = NULL; + + for (int i=0; i<_homePositions.count(); i++) { + homePos = qobject_cast(_homePositions[i]); + if (homePos->name() == name) { + break; + } + homePos = NULL; + } + + if (homePos == NULL) { + HomePosition* homePos = new HomePosition(name, coordinate, this); + _homePositions.append(homePos); + } else { + homePos->setName(name); + homePos->setCoordinate(coordinate); + } + + _storeSettings(); } -Eigen::Vector3d HomePositionManager::wgs84ToEcef(const double & latitude, const double & longitude, const double & altitude) +void HomePositionManager::deleteHomePosition(const QString& name) { - const double a = 6378137.0; // semi-major axis - const double e_sq = 6.69437999014e-3; // first eccentricity squared - - double s_long, s_lat, c_long, c_lat; - sincos(latitude * DEG2RAD, &s_lat, &c_lat); - sincos(longitude * DEG2RAD, &s_long, &c_long); - - const double N = a / sqrt(1 - e_sq * s_lat * s_lat); - - Eigen::Vector3d ecef; - - ecef[0] = (N + altitude) * c_lat * c_long; - ecef[1] = (N + altitude) * c_lat * s_long; - ecef[2] = (N * (1 - e_sq) + altitude) * s_lat; + // Don't allow delete of last position + if (_homePositions.count() == 1) { + return; + } + + qDebug() << "Attempting delete" << name; + + for (int i=0; i<_homePositions.count(); i++) { + if (qobject_cast(_homePositions[i])->name() == name) { + qDebug() << "Deleting" << name; + _homePositions.removeAt(i); + break; + } + } + + _storeSettings(); +} - return ecef; +HomePosition::HomePosition(const QString& name, const QGeoCoordinate& coordinate, QObject* parent) + : QObject(parent) + , _coordinate(coordinate) +{ + setObjectName(name); } -Eigen::Vector3d HomePositionManager::ecefToEnu(const Eigen::Vector3d & ecef) +HomePosition::~HomePosition() { - return ecef_ref_orientation_ * (ecef - ecef_ref_point_); + } -void HomePositionManager::wgs84ToEnu(const double& lat, const double& lon, const double& alt, double* east, double* north, double* up) +QString HomePosition::name(void) { - Eigen::Vector3d ecef = wgs84ToEcef(lat, lon, alt); - Eigen::Vector3d enu = ecefToEnu(ecef); - *east = enu.x(); - *north = enu.y(); - *up = enu.z(); + return objectName(); } -void HomePositionManager::enuToWgs84(const double& x, const double& y, const double& z, double* lat, double* lon, double* alt) +void HomePosition::setName(const QString& name) { - *lat=homeLat+y/MEAN_EARTH_DIAMETER*360./PI; - *lon=homeLon+x/MEAN_EARTH_DIAMETER*360./PI/cos(homeLat*UMR); - *alt=homeAlt+z; + setObjectName(name); + HomePositionManager::instance()->_storeSettings(); + emit nameChanged(name); } -void HomePositionManager::nedToWgs84(const double& x, const double& y, const double& z, double* lat, double* lon, double* alt) +QGeoCoordinate HomePosition::coordinate(void) { - *lat=homeLat+x/MEAN_EARTH_DIAMETER*360./PI; - *lon=homeLon+y/MEAN_EARTH_DIAMETER*360./PI/cos(homeLat*UMR); - *alt=homeAlt-z; + return _coordinate; } +void HomePosition::setCoordinate(const QGeoCoordinate& coordinate) +{ + _coordinate = coordinate; + HomePositionManager::instance()->_storeSettings(); + emit coordinateChanged(coordinate); +} diff --git a/src/HomePositionManager.h b/src/HomePositionManager.h index e45500f34a46137883452d64b2183d7bb2ea8f06..0d34024d01b8ae49a03a676466497a97f02e3cc7 100644 --- a/src/HomePositionManager.h +++ b/src/HomePositionManager.h @@ -21,21 +21,41 @@ This file is part of the QGROUNDCONTROL project ======================================================================*/ -#ifndef _UASMANAGER_H_ -#define _UASMANAGER_H_ +#ifndef HomePositionManager_H +#define HomePositionManager_H -#include "UASInterface.h" - -#include -#include +#include "QGCSingleton.h" +#include "QmlObjectListModel.h" -#include +#include -#include "QGCGeo.h" -#include "QGCSingleton.h" +class HomePosition : public QObject +{ + Q_OBJECT + +public: + HomePosition(const QString& name, const QGeoCoordinate& coordinate, QObject* parent = NULL); + ~HomePosition(); + + Q_PROPERTY(QString name READ name WRITE setName NOTIFY nameChanged) + Q_PROPERTY(QGeoCoordinate coordinate READ coordinate WRITE setCoordinate NOTIFY coordinateChanged) + + // Property accessors + + QString name(void); + void setName(const QString& name); + + QGeoCoordinate coordinate(void); + void setCoordinate(const QGeoCoordinate& coordinate); + +signals: + void nameChanged(const QString& name); + void coordinateChanged(const QGeoCoordinate& coordinate); + +private: + QGeoCoordinate _coordinate; +}; -/// Manages an offline home position as well as performance coordinate transformations -/// around a home position. class HomePositionManager : public QObject { Q_OBJECT @@ -43,6 +63,40 @@ class HomePositionManager : public QObject DECLARE_QGC_SINGLETON(HomePositionManager, HomePositionManager) public: + Q_PROPERTY(QmlObjectListModel* homePositions READ homePositions CONSTANT) + + /// If name is not already a home position a new one will be added, otherwise the existing + /// home position will be updated + Q_INVOKABLE void updateHomePosition(const QString& name, const QGeoCoordinate& coordinate); + + Q_INVOKABLE void deleteHomePosition(const QString& name); + + // Property accesors + + QmlObjectListModel* homePositions(void) { return &_homePositions; } + + // Should only be called by HomePosition + void _storeSettings(void); + +private: + /// @brief All access to HomePositionManager singleton is through HomePositionManager::instance + HomePositionManager(QObject* parent = NULL); + ~HomePositionManager(); + + void _loadSettings(void); + + QmlObjectListModel _homePositions; + + static const char* _settingsGroup; + static const char* _latitudeKey; + static const char* _longitudeKey; + static const char* _altitudeKey; + +// Everything below is deprecated and will be removed once old Map code is removed +public: + + // Deprecated methods + /** @brief Get home position latitude */ double getHomeLatitude() const { return homeLat; @@ -56,24 +110,10 @@ public: return homeAlt; } - /** @brief Get the home position coordinate frame */ - int getHomeFrame() const - { - return homeFrame; - } - - /** @brief Convert WGS84 coordinates to earth centric frame */ - Eigen::Vector3d wgs84ToEcef(const double & latitude, const double & longitude, const double & altitude); - /** @brief Convert earth centric frame to EAST-NORTH-UP frame (x-y-z directions */ - Eigen::Vector3d ecefToEnu(const Eigen::Vector3d & ecef); - /** @brief Convert WGS84 lat/lon coordinates to carthesian coordinates with home position as origin */ - void wgs84ToEnu(const double& lat, const double& lon, const double& alt, double* east, double* north, double* up); - /** @brief Convert x,y,z coordinates to lat / lon / alt coordinates in east-north-up frame */ - void enuToWgs84(const double& x, const double& y, const double& z, double* lat, double* lon, double* alt); - /** @brief Convert x,y,z coordinates to lat / lon / alt coordinates in north-east-down frame */ - void nedToWgs84(const double& x, const double& y, const double& z, double* lat, double* lon, double* alt); - public slots: + + // Deprecated methods + /** @brief Set the current home position, but do not change it on the UAVs */ bool setHomePosition(double lat, double lon, double alt); @@ -81,11 +121,6 @@ public slots: bool setHomePositionAndNotify(double lat, double lon, double alt); - /** @brief Load settings */ - void loadSettings(); - /** @brief Store settings */ - void storeSettings(); - signals: /** @brief Current home position changed */ void homePositionChanged(double lat, double lon, double alt); @@ -94,23 +129,6 @@ protected: double homeLat; double homeLon; double homeAlt; - int homeFrame; - Eigen::Quaterniond ecef_ref_orientation_; - Eigen::Vector3d ecef_ref_point_; - - void initReference(const double & latitude, const double & longitude, const double & altitude); - -private: - /// @brief All access to HomePositionManager singleton is through HomePositionManager::instance - HomePositionManager(QObject* parent = NULL); - ~HomePositionManager(); - -public: - /* Need to align struct pointer to prevent a memory assertion: - * See http://eigen.tuxfamily.org/dox-devel/TopicUnalignedArrayAssert.html - * for details - */ - EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -#endif // _UASMANAGER_H_ +#endif diff --git a/src/MissionEditor/MissionEditor.qml b/src/MissionEditor/MissionEditor.qml index eb99f3a6ededc391d9bdc4d3b051f6f298f07e99..d52ecdbfc3a23ade54bb984abc550a1ec65aefee 100644 --- a/src/MissionEditor/MissionEditor.qml +++ b/src/MissionEditor/MissionEditor.qml @@ -27,6 +27,7 @@ import QtQuick.Dialogs 1.2 import QtLocation 5.3 import QtPositioning 5.3 +import QGroundControl 1.0 import QGroundControl.FlightMap 1.0 import QGroundControl.ScreenTools 1.0 import QGroundControl.Controls 1.0 @@ -37,14 +38,18 @@ import QGroundControl.Palette 1.0 QGCView { viewPanel: panel - readonly property real _defaultLatitude: 37.803784 - readonly property real _defaultLongitude: -122.462276 readonly property int _decimalPlaces: 7 readonly property real _horizontalMargin: ScreenTools.defaultFontPixelWidth / 2 readonly property real _verticalMargin: ScreenTools.defaultFontPixelHeight / 2 readonly property var _activeVehicle: multiVehicleManager.activeVehicle + readonly property real _editFieldWidth: ScreenTools.defaultFontPixelWidth * 16 - property var _missionItems: controller.missionItems + property var _missionItems: controller.missionItems + property bool _showHomePositionManager: false + + property var _homePositionManager: QGroundControl.homePositionManager + property string _homePositionName: _homePositionManager.homePositions.get(0).name + property var _homePositionCoordinate: _homePositionManager.homePositions.get(0).coordinate QGCPalette { id: _qgcPal; colorGroupEnabled: enabled } @@ -68,8 +73,8 @@ QGCView { anchors.top: parent.top anchors.bottom: parent.bottom mapName: "MissionEditor" - latitude: _defaultLatitude - longitude: _defaultLongitude + latitude: _homePositionCoordinate.latitude + longitude: _homePositionCoordinate.longitude QGCLabel { anchors.right: parent.right @@ -83,12 +88,21 @@ QGCView { var coordinate = editorMap.toCoordinate(Qt.point(mouse.x, mouse.y)) coordinate.latitude = coordinate.latitude.toFixed(_decimalPlaces) coordinate.longitude = coordinate.longitude.toFixed(_decimalPlaces) - coordinate.altitude = 0 - var index = controller.addMissionItem(coordinate) - setCurrentItem(index) + coordinate.altitude = coordinate.altitude.toFixed(_decimalPlaces) + if (_showHomePositionManager) { + _homePositionCoordinate = coordinate + } else { + var index = controller.addMissionItem(coordinate) + setCurrentItem(index) + } } } + MissionItemIndicator { + label: "H" + coordinate: _homePositionCoordinate + } + // Add the mission items to the map MapItemView { model: controller.missionItems @@ -140,6 +154,16 @@ QGCView { Menu { id: toolMenu + MenuItem { + text: "Manage Home Position" + checkable: true + checked: _showHomePositionManager + + onTriggered: _showHomePositionManager = checked + } + + MenuSeparator { } + MenuItem { text: "Get mission items from vehicle" enabled: _activeVehicle && !_activeVehicle.missionManager.inProgress @@ -184,63 +208,234 @@ QGCView { } } - // Mission item list - ListView { - id: missionItemSummaryList + // Mission Item Editor + Item { anchors.topMargin: _verticalMargin anchors.left: parent.left anchors.right: parent.right anchors.top: toolsButton.bottom anchors.bottom: parent.bottom - spacing: _verticalMargin - orientation: ListView.Vertical - model: controller.canEdit ? controller.missionItems : 0 - - property real _maxItemHeight: 0 - - delegate: - MissionItemEditor { - missionItem: object - width: parent.width - - onClicked: setCurrentItem(object.sequenceNumber) - - onRemove: { - var newCurrentItem = object.sequenceNumber - 1 - controller.removeMissionItem(object.sequenceNumber) - if (_missionItems.count) { - newCurrentItem = Math.min(_missionItems.count - 1, newCurrentItem) - setCurrentItem(newCurrentItem) + visible: !_showHomePositionManager + + ListView { + id: missionItemSummaryList + anchors.fill: parent + spacing: _verticalMargin + orientation: ListView.Vertical + model: controller.canEdit ? controller.missionItems : 0 + + property real _maxItemHeight: 0 + + delegate: + MissionItemEditor { + missionItem: object + width: parent.width + + onClicked: setCurrentItem(object.sequenceNumber) + + onRemove: { + var newCurrentItem = object.sequenceNumber - 1 + controller.removeMissionItem(object.sequenceNumber) + if (_missionItems.count) { + newCurrentItem = Math.min(_missionItems.count - 1, newCurrentItem) + setCurrentItem(newCurrentItem) + } } + + onMoveUp: controller.moveUp(object.sequenceNumber) + onMoveDown: controller.moveDown(object.sequenceNumber) } + } // ListView - onMoveUp: controller.moveUp(object.sequenceNumber) - onMoveDown: controller.moveDown(object.sequenceNumber) - } - } // ListView + QGCLabel { + anchors.fill: parent + visible: controller.missionItems.count == 0 + wrapMode: Text.WordWrap + text: "Click in the map to add Mission Items" + } - QGCLabel { - anchors.topMargin: _verticalMargin - anchors.left: parent.left - anchors.right: parent.right - anchors.top: toolsButton.bottom - anchors.bottom: parent.bottom - visible: controller.missionItems.count == 0 - wrapMode: Text.WordWrap - text: "Click in the map to add Mission Items" - } + QGCLabel { + anchors.fill: parent + visible: !controller.canEdit + wrapMode: Text.WordWrap + text: "The set of mission items you have loaded cannot be edited by QGroundControl. " + + "You will only be able to save these to a file, or send them to a vehicle." + } + } // Item - Mission Item editor - QGCLabel { + // Home Position Manager + Item { anchors.topMargin: _verticalMargin anchors.left: parent.left anchors.right: parent.right anchors.top: toolsButton.bottom anchors.bottom: parent.bottom - visible: !controller.canEdit - wrapMode: Text.WordWrap - text: "The set of mission items you have loaded cannot be edited by QGroundControl. " + - "You will only be able to save these to a file, or send them to a vehicle." - } + visible: _showHomePositionManager + + Column { + anchors.fill: parent + + QGCLabel { + font.pixelSize: ScreenTools.mediumFontPixelSize + text: "Home Position Manager" + } + + Item { + width: 10 + height: ScreenTools.defaultFontPixelHeight + } + + QGCLabel { + text: "Select home position to use:" + } + + QGCComboBox { + id: homePosCombo + width: parent.width + textRole: "text" + model: _homePositionManager.homePositions + + onCurrentIndexChanged: { + if (currentIndex != -1) { + var homePos = _homePositionManager.homePositions.get(currentIndex) + _homePositionName = homePos.name + _homePositionCoordinate = homePos.coordinate + } + } + } + + Item { + width: 10 + height: ScreenTools.defaultFontPixelHeight + } + + QGCLabel { + width: parent.width + wrapMode: Text.WordWrap + text: "To add a new home position, click in the Map to set the position. Then give it a name and click Add." + } + + Item { + width: 10 + height: ScreenTools.defaultFontPixelHeight / 3 + } + + Item { + width: parent.width + height: nameField.height + + QGCLabel { + anchors.baseline: nameField.baseline + text: "Name:" + } + + QGCTextField { + id: nameField + anchors.right: parent.right + width: _editFieldWidth + text: _homePositionName + } + } + + Item { + width: 10 + height: ScreenTools.defaultFontPixelHeight / 3 + } + + Item { + width: parent.width + height: latitudeField.height + + QGCLabel { + anchors.baseline: latitudeField.baseline + text: "Lat:" + } + + QGCTextField { + id: latitudeField + anchors.right: parent.right + width: _editFieldWidth + text: _homePositionCoordinate.latitude + } + } + + Item { + width: 10 + height: ScreenTools.defaultFontPixelHeight / 3 + } + + Item { + width: parent.width + height: longitudeField.height + + QGCLabel { + anchors.baseline: longitudeField.baseline + text: "Lon:" + } + + QGCTextField { + id: longitudeField + anchors.right: parent.right + width: _editFieldWidth + text: _homePositionCoordinate.longitude + } + } + + Item { + width: 10 + height: ScreenTools.defaultFontPixelHeight / 3 + } + + Item { + width: parent.width + height: altitudeField.height + + QGCLabel { + anchors.baseline: altitudeField.baseline + text: "Alt:" + } + + QGCTextField { + id: altitudeField + anchors.right: parent.right + width: _editFieldWidth + text: _homePositionCoordinate.altitude + } + } + + Item { + width: 10 + height: ScreenTools.defaultFontPixelHeight + } + + Row { + spacing: ScreenTools.defaultFontPixelWidth + + QGCButton { + text: "Add/Update" + + onClicked: { + _homePositionManager.updateHomePosition(nameField.text, QtPositioning.coordinate(latitudeField.text, longitudeField.text, altitudeField.text)) + homePosCombo.currentIndex = homePosCombo.find(nameField.text) + } + } + + QGCButton { + text: "Delete" + + onClicked: { + homePosCombo.currentIndex = -1 + _homePositionManager.deleteHomePosition(nameField.text) + homePosCombo.currentIndex = 0 + var homePos = _homePositionManager.homePositions.get(0) + _homePositionName = homePos.name + _homePositionCoordinate = homePos.coordinate + } + } + } + } // Column + } // Item - Home Position Manager + } // Item } // Rectangle - mission item list } // Item - split view container diff --git a/src/QmlControls/QmlObjectListModel.cc b/src/QmlControls/QmlObjectListModel.cc index b420629bbd133fcbb647ccb85a93b180b8981321..dde0f688fcaba434cc1c47104f9658f265dadab4 100644 --- a/src/QmlControls/QmlObjectListModel.cc +++ b/src/QmlControls/QmlObjectListModel.cc @@ -29,6 +29,7 @@ #include const int QmlObjectListModel::ObjectRole = Qt::UserRole; +const int QmlObjectListModel::TextRole = Qt::UserRole + 1; QmlObjectListModel::QmlObjectListModel(QObject* parent) : QAbstractListModel(parent) @@ -60,6 +61,8 @@ QVariant QmlObjectListModel::data(const QModelIndex &index, int role) const if (role == ObjectRole) { return QVariant::fromValue(_objectList[index.row()]); + } else if (role == TextRole) { + return QVariant::fromValue(_objectList[index.row()]->objectName()); } else { return QVariant(); } @@ -70,6 +73,7 @@ QHash QmlObjectListModel::roleNames(void) const QHash hash; hash[ObjectRole] = "object"; + hash[TextRole] = "text"; return hash; } diff --git a/src/QmlControls/QmlObjectListModel.h b/src/QmlControls/QmlObjectListModel.h index 16b119397ece638a8bc6ed56fc2a923c2ecaa39b..9c9b6c2a84ab039bc5b6cb0788db35e799c7eb14 100644 --- a/src/QmlControls/QmlObjectListModel.h +++ b/src/QmlControls/QmlObjectListModel.h @@ -65,6 +65,7 @@ private: QList _objectList; static const int ObjectRole; + static const int TextRole; }; #endif diff --git a/src/comm/QGCFlightGearLink.cc b/src/comm/QGCFlightGearLink.cc index 2945c1d0eb4c4c546732952aa81dca38db99f9f9..4c7f84b1ad19855b372af08ece7c69e723f8ec8f 100644 --- a/src/comm/QGCFlightGearLink.cc +++ b/src/comm/QGCFlightGearLink.cc @@ -38,6 +38,7 @@ This file is part of the QGROUNDCONTROL project #include #include +#include #include "QGCFlightGearLink.h" #include "QGC.h" diff --git a/src/comm/QGCXPlaneLink.cc b/src/comm/QGCXPlaneLink.cc index 21718d63b8dc7d0d67d0fd633497c0357b337531..ef85146bf49bca85b17acaa7c94042036eee3624 100644 --- a/src/comm/QGCXPlaneLink.cc +++ b/src/comm/QGCXPlaneLink.cc @@ -33,10 +33,13 @@ This file is part of the QGROUNDCONTROL project #include #include #include +#include + #include +#include + #include "QGCXPlaneLink.h" #include "QGC.h" -#include #include "UAS.h" #include "UASInterface.h" #include "QGCMessageBox.h" diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 76d3e5b20715dc8593d88b5546794dfda8b96dff..1930d4a97afa0eb64b071fb99deb4da5a7ac77e1 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -2784,9 +2784,8 @@ void UAS::home() double latitude = HomePositionManager::instance()->getHomeLatitude(); double longitude = HomePositionManager::instance()->getHomeLongitude(); double altitude = HomePositionManager::instance()->getHomeAltitude(); - int frame = HomePositionManager::instance()->getHomeFrame(); - mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_ALL, MAV_CMD_OVERRIDE_GOTO, 1, MAV_GOTO_DO_CONTINUE, MAV_GOTO_HOLD_AT_CURRENT_POSITION, frame, 0, latitude, longitude, altitude); + mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_ALL, MAV_CMD_OVERRIDE_GOTO, 1, MAV_GOTO_DO_CONTINUE, MAV_GOTO_HOLD_AT_CURRENT_POSITION, MAV_FRAME_GLOBAL, 0, latitude, longitude, altitude); _vehicle->sendMessage(msg); } diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index dc01ef034d74a77a4c0f71066036eae6ea7316f9..257dc8fe96c85c2aef99289eeb887ead7f98419e 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -609,7 +609,6 @@ void MainWindow::closeEvent(QCloseEvent *event) _storeCurrentViewState(); storeSettings(); - HomePositionManager::instance()->storeSettings(); event->accept(); }