From b43ccf87a3941e1998d7b943df60d8216243d304 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Tue, 13 Oct 2015 15:04:36 -0700 Subject: [PATCH] Support live home position in Mission Editor --- src/MissionEditor/MissionEditor.cc | 40 ++++++- src/MissionEditor/MissionEditor.h | 23 +++- src/MissionEditor/MissionEditor.qml | 165 ++++++++++++++++++++++------ src/Vehicle/Vehicle.cc | 6 +- src/comm/MockLink.cc | 23 ++++ src/comm/MockLink.h | 1 + 6 files changed, 213 insertions(+), 45 deletions(-) diff --git a/src/MissionEditor/MissionEditor.cc b/src/MissionEditor/MissionEditor.cc index 44087a528..0a9919900 100644 --- a/src/MissionEditor/MissionEditor.cc +++ b/src/MissionEditor/MissionEditor.cc @@ -38,6 +38,8 @@ MissionEditor::MissionEditor(QWidget *parent) : QGCQmlWidgetHolder(parent) , _missionItems(NULL) , _canEdit(true) + , _activeVehicle(NULL) + , _liveHomePositionAvailable(false) { // Get rid of layout default margins QLayout* pl = layout(); @@ -45,11 +47,16 @@ MissionEditor::MissionEditor(QWidget *parent) pl->setContentsMargins(0,0,0,0); } - Vehicle* activeVehicle = MultiVehicleManager::instance()->activeVehicle(); + MultiVehicleManager* multiVehicleMgr = MultiVehicleManager::instance(); + + connect(multiVehicleMgr, &MultiVehicleManager::activeVehicleChanged, this, &MissionEditor::_activeVehicleChanged); + + Vehicle* activeVehicle = multiVehicleMgr->activeVehicle(); if (activeVehicle) { MissionManager* missionManager = activeVehicle->missionManager(); connect(missionManager, &MissionManager::newMissionItemsAvailable, this, &MissionEditor::_newMissionItemsAvailable); _newMissionItemsAvailable(); + _activeVehicleChanged(activeVehicle); } else { _missionItems = new QmlObjectListModel(this); _initAllMissionItems(); @@ -389,3 +396,34 @@ void MissionEditor::_itemCommandChanged(MavlinkQmlSingleton::Qml_MAV_CMD command _recalcChildItems(); _recalcWaypointLines(); } + +void MissionEditor::_activeVehicleChanged(Vehicle* activeVehicle) +{ + if (_activeVehicle) { + disconnect(_activeVehicle, &Vehicle::homePositionAvailableChanged, this, &MissionEditor::_activeVehicleHomePositionAvailableChanged); + disconnect(_activeVehicle, &Vehicle::homePositionChanged, this, &MissionEditor::_activeVehicleHomePositionChanged); + _activeVehicle = NULL; + _activeVehicleHomePositionAvailableChanged(false); + } + + _activeVehicle = activeVehicle; + + if (_activeVehicle) { + connect(_activeVehicle, &Vehicle::homePositionAvailableChanged, this, &MissionEditor::_activeVehicleHomePositionAvailableChanged); + connect(_activeVehicle, &Vehicle::homePositionChanged, this, &MissionEditor::_activeVehicleHomePositionChanged); + _activeVehicleHomePositionChanged(_activeVehicle->homePosition()); + _activeVehicleHomePositionAvailableChanged(_activeVehicle->homePositionAvailable()); + } +} + +void MissionEditor::_activeVehicleHomePositionAvailableChanged(bool homePositionAvailable) +{ + _liveHomePositionAvailable = homePositionAvailable; + emit liveHomePositionAvailableChanged(_liveHomePositionAvailable); +} + +void MissionEditor::_activeVehicleHomePositionChanged(const QGeoCoordinate& homePosition) +{ + _liveHomePosition = homePosition; + emit liveHomePositionChanged(_liveHomePosition); +} diff --git a/src/MissionEditor/MissionEditor.h b/src/MissionEditor/MissionEditor.h index 491423ba8..3f6e7564e 100644 --- a/src/MissionEditor/MissionEditor.h +++ b/src/MissionEditor/MissionEditor.h @@ -26,6 +26,7 @@ This file is part of the QGROUNDCONTROL project #include "QGCQmlWidgetHolder.h" #include "QmlObjectListModel.h" +#include "Vehicle.h" class MissionEditor : public QGCQmlWidgetHolder { @@ -35,9 +36,11 @@ public: MissionEditor(QWidget* parent = NULL); ~MissionEditor(); - Q_PROPERTY(QmlObjectListModel* missionItems READ missionItems NOTIFY missionItemsChanged) - Q_PROPERTY(QmlObjectListModel* waypointLines READ waypointLines NOTIFY waypointLinesChanged) - Q_PROPERTY(bool canEdit READ canEdit NOTIFY canEditChanged) + Q_PROPERTY(QmlObjectListModel* missionItems READ missionItems NOTIFY missionItemsChanged) + Q_PROPERTY(QmlObjectListModel* waypointLines READ waypointLines NOTIFY waypointLinesChanged) + Q_PROPERTY(bool canEdit READ canEdit NOTIFY canEditChanged) + Q_PROPERTY(bool liveHomePositionAvailable READ liveHomePositionAvailable NOTIFY liveHomePositionAvailableChanged) + Q_PROPERTY(QGeoCoordinate liveHomePosition READ liveHomePosition NOTIFY liveHomePositionChanged) Q_INVOKABLE int addMissionItem(QGeoCoordinate coordinate); Q_INVOKABLE void getMissionItems(void); @@ -53,16 +56,23 @@ public: QmlObjectListModel* missionItems(void) { return _missionItems; } QmlObjectListModel* waypointLines(void) { return &_waypointLines; } bool canEdit(void) { return _canEdit; } - + bool liveHomePositionAvailable(void) { return _liveHomePositionAvailable; } + QGeoCoordinate liveHomePosition(void) { return _liveHomePosition; } + signals: void missionItemsChanged(void); void canEditChanged(bool canEdit); void waypointLinesChanged(void); + void liveHomePositionAvailableChanged(bool homePositionAvailable); + void liveHomePositionChanged(const QGeoCoordinate& homePosition); private slots: void _newMissionItemsAvailable(); void _itemCoordinateChanged(const QGeoCoordinate& coordinate); void _itemCommandChanged(MavlinkQmlSingleton::Qml_MAV_CMD command); + void _activeVehicleChanged(Vehicle* activeVehicle); + void _activeVehicleHomePositionAvailableChanged(bool homePositionAvailable); + void _activeVehicleHomePositionChanged(const QGeoCoordinate& homePosition); private: void _recalcSequence(void); @@ -78,7 +88,10 @@ private: QmlObjectListModel* _missionItems; QmlObjectListModel _waypointLines; bool _canEdit; ///< true: UI can edit these items, false: can't edit, can only send to vehicle or save - + Vehicle* _activeVehicle; + bool _liveHomePositionAvailable; + QGeoCoordinate _liveHomePosition; + static const char* _settingsGroup; }; diff --git a/src/MissionEditor/MissionEditor.qml b/src/MissionEditor/MissionEditor.qml index 1e95c81f2..a072e77f7 100644 --- a/src/MissionEditor/MissionEditor.qml +++ b/src/MissionEditor/MissionEditor.qml @@ -52,7 +52,11 @@ QGCView { property var _homePositionManager: QGroundControl.homePositionManager property string _homePositionName: _homePositionManager.homePositions.get(0).name - property var homePositionCoordinate: _homePositionManager.homePositions.get(0).coordinate + + property var offlineHomePosition: _homePositionManager.homePositions.get(0).coordinate + property var liveHomePosition: controller.liveHomePosition + property var liveHomePositionAvailable: controller.liveHomePositionAvailable + property var homePosition: offlineHomePosition // live or offline depending on state QGCPalette { id: _qgcPal; colorGroupEnabled: enabled } @@ -76,20 +80,24 @@ QGCView { } } - // Home position is mission item 0, so keep them in sync - onHomePositionCoordinateChanged: { + function updateHomePosition() { + homePosition = liveHomePositionAvailable ? liveHomePosition : offlineHomePosition // Changing the coordinate will set the dirty bit, so we save and reset it var dirtyBit = _missionItems.dirty - _missionItems.get(0).coordinate = homePositionCoordinate + _missionItems.get(0).coordinate = homePosition _missionItems.dirty = dirtyBit } - Component.onCompleted: onHomePositionCoordinateChanged + Component.onCompleted: updateHomePosition() + onOfflineHomePositionChanged: updateHomePosition() + onLiveHomePositionAvailableChanged: updateHomePosition() + onLiveHomePositionChanged: updateHomePosition() Connections { target: controller - onMissionItemsChanged: _missionItems.get(0).coordinate = homePositionCoordinate + // When the mission items change _missionsItems[0] changes as well so we need to reset it to home + onMissionItemsChanged: updateHomePosition } QGCViewPanel { @@ -108,8 +116,8 @@ QGCView { mapName: "MissionEditor" Component.onCompleted: { - latitude = homePositionCoordinate.latitude - longitude = homePositionCoordinate.longitude + latitude = homePosition.latitude + longitude = homePosition.longitude } MouseArea { @@ -121,7 +129,7 @@ QGCView { coordinate.longitude = coordinate.longitude.toFixed(_decimalPlaces) coordinate.altitude = coordinate.altitude.toFixed(_decimalPlaces) if (_showHomePositionManager) { - homePositionCoordinate = coordinate + offlineHomePosition = coordinate } else if (_addMissionItems) { var index = controller.addMissionItem(coordinate) setCurrentItem(index) @@ -200,7 +208,7 @@ QGCView { onClicked: { centerMapButton.hideDropDown() - editorMap.center = QtPositioning.coordinate(homePositionCoordinate.latitude, homePositionCoordinate.longitude) + editorMap.center = QtPositioning.coordinate(homePosition.latitude, homePosition.longitude) _showHomePositionManager = true } } @@ -229,8 +237,8 @@ QGCView { 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)) + var region = QtPositioning.rectangle(QtPositioning.coordinate(homePosition.latitude, homePosition.longitude), + QtPositioning.coordinate(homePosition.latitude, homePosition.longitude)) // Now expand the region to include all mission items for (var i=0; i<_missionItems.count; i++) { @@ -509,11 +517,12 @@ QGCView { visible: _showHomePositionManager && !_showHelpPanel Column { - anchors.fill: parent + anchors.fill: parent + visible: !liveHomePositionAvailable QGCLabel { font.pixelSize: ScreenTools.mediumFontPixelSize - text: "Home Position Manager" + text: "Offline Home Position Manager" } Item { @@ -522,7 +531,18 @@ QGCView { } QGCLabel { - text: "Select home position to use:" + width: parent.width + wrapMode: Text.WordWrap + text: "This is used to specify a simulated home position while you are not connected to a Vehicle." + } + + Item { + width: 10 + height: ScreenTools.defaultFontPixelHeight + } + + QGCLabel { + text: "Select home position to use:" } QGCComboBox { @@ -535,9 +555,9 @@ QGCView { if (currentIndex != -1) { var homePos = _homePositionManager.homePositions.get(currentIndex) _homePositionName = homePos.name - homePositionCoordinate = homePos.coordinate - editorMap.latitude = homePositionCoordinate.latitude - editorMap.longitude = homePositionCoordinate.longitude + offlineHomePosition = homePos.coordinate + editorMap.latitude = offlineHomePosition.latitude + editorMap.longitude = offlineHomePosition.longitude } } } @@ -585,18 +605,18 @@ QGCView { Item { width: parent.width - height: latitudeField.height + height: offlineLatitudeField.height QGCLabel { - anchors.baseline: latitudeField.baseline + anchors.baseline: offlineLatitudeField.baseline text: "Lat:" } QGCTextField { - id: latitudeField + id: offlineLatitudeField anchors.right: parent.right width: _editFieldWidth - text: homePositionCoordinate.latitude + text: offlineHomePosition.latitude } } @@ -607,18 +627,18 @@ QGCView { Item { width: parent.width - height: longitudeField.height + height: offlineLongitudeField.height QGCLabel { - anchors.baseline: longitudeField.baseline + anchors.baseline: offlineLongitudeField.baseline text: "Lon:" } QGCTextField { - id: longitudeField + id: offlineLongitudeField anchors.right: parent.right width: _editFieldWidth - text: homePositionCoordinate.longitude + text: offlineHomePosition.longitude } } @@ -629,18 +649,18 @@ QGCView { Item { width: parent.width - height: altitudeField.height + height: offlineAltitudeField.height QGCLabel { - anchors.baseline: altitudeField.baseline + anchors.baseline: offlineAltitudeField.baseline text: "Alt:" } QGCTextField { - id: altitudeField + id: offlineAltitudeField anchors.right: parent.right width: _editFieldWidth - text: homePositionCoordinate.altitude + text: offlineHomePosition.altitude } } @@ -656,8 +676,8 @@ QGCView { text: "Add/Update" onClicked: { - homePositionCoordinate = QtPositioning.coordinate(latitudeField.text, longitudeField.text, altitudeField.text) - _homePositionManager.updateHomePosition(nameField.text, homePositionCoordinate) + offlineHomePosition = QtPositioning.coordinate(latitudeField.text, longitudeField.text, altitudeField.text) + _homePositionManager.updateHomePosition(nameField.text, offlineHomePosition) homePosCombo.currentIndex = homePosCombo.find(nameField.text) } } @@ -671,11 +691,88 @@ QGCView { homePosCombo.currentIndex = 0 var homePos = _homePositionManager.homePositions.get(0) _homePositionName = homePos.name - homePositionCoordinate = homePos.coordinate + offlineHomePosition = homePos.coordinate } } } - } // Column + } // Column - Offline view + + Column { + anchors.fill: parent + visible: liveHomePositionAvailable + + QGCLabel { + font.pixelSize: ScreenTools.mediumFontPixelSize + text: "Vehicle Home Position" + } + + Item { + width: 10 + height: ScreenTools.defaultFontPixelHeight + } + + Item { + width: parent.width + height: liveLatitudeField.height + + QGCLabel { + anchors.baseline: liveLatitudeField.baseline + text: "Lat:" + } + + QGCLabel { + id: liveLatitudeField + anchors.right: parent.right + width: _editFieldWidth + text: liveHomePosition.latitude + } + } + + Item { + width: 10 + height: ScreenTools.defaultFontPixelHeight / 3 + } + + Item { + width: parent.width + height: liveLongitudeField.height + + QGCLabel { + anchors.baseline: liveLongitudeField.baseline + text: "Lon:" + } + + QGCLabel { + id: liveLongitudeField + anchors.right: parent.right + width: _editFieldWidth + text: liveHomePosition.longitude + } + } + + Item { + width: 10 + height: ScreenTools.defaultFontPixelHeight / 3 + } + + Item { + width: parent.width + height: liveAltitudeField.height + + QGCLabel { + anchors.baseline: liveAltitudeField.baseline + text: "Alt:" + } + + QGCLabel { + id: liveAltitudeField + anchors.right: parent.right + width: _editFieldWidth + text: liveHomePosition.altitude + } + } + } // Column - Online view + } // Item - Home Position Manager // Help Panel diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 130e63013..7ea27f37e 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -227,8 +227,8 @@ void Vehicle::_handleHomePosition(mavlink_message_t& message) _homePosition.setAltitude(homePos.altitude / 1000.0); _homePositionAvailable = true; - emit homePositionAvailableChanged(true); emit homePositionChanged(_homePosition); + emit homePositionAvailableChanged(true); } void Vehicle::_handleHeartbeat(mavlink_message_t& message) @@ -963,10 +963,6 @@ bool Vehicle::homePositionAvailable(void) QGeoCoordinate Vehicle::homePosition(void) { - if (!_homePositionAvailable) { - qWarning() << "Call to homePosition while _homePositionAvailable == false"; - } - return _homePosition; } diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc index 53d251882..6779caedf 100644 --- a/src/comm/MockLink.cc +++ b/src/comm/MockLink.cc @@ -157,6 +157,7 @@ void MockLink::_run1HzTasks(void) { if (_mavlinkStarted && _connected) { _sendHeartBeat(); + _sendHomePosition(); } } @@ -673,3 +674,25 @@ void MockLink::setMissionItemFailureMode(MockLinkMissionItemHandler::FailureMode { _missionItemHandler.setMissionItemFailureMode(failureMode, firstTimeOnly); } + +void MockLink::_sendHomePosition(void) +{ + mavlink_message_t msg; + + float bogus[4]; + bogus[0] = 0.0f; + bogus[1] = 0.0f; + bogus[2] = 0.0f; + bogus[3] = 0.0f; + + mavlink_msg_home_position_pack(_vehicleSystemId, + _vehicleComponentId, + &msg, + (int32_t)(47.633033f * 1E7), + (int32_t)(-122.08794f * 1E7), + (int32_t)(2.0f * 1000), + 0.0f, 0.0f, 0.0f, + &bogus[0], + 0.0f, 0.0f, 0.0f); + respondWithMavlinkMessage(msg); +} diff --git a/src/comm/MockLink.h b/src/comm/MockLink.h index ceb4269db..d8692e065 100644 --- a/src/comm/MockLink.h +++ b/src/comm/MockLink.h @@ -143,6 +143,7 @@ private: void _handleCommandLong(const mavlink_message_t& msg); float _floatUnionForParam(int componentId, const QString& paramName); void _setParamFloatUnionIntoMap(int componentId, const QString& paramName, float paramFloat); + void _sendHomePosition(void); MockLinkMissionItemHandler _missionItemHandler; -- 2.22.0