diff --git a/QGCApplication.pro b/QGCApplication.pro index cd24b64e451b97c48af3fbf3c3c4d4899f94b8c4..9bf7968ec55d37332c98b74c728111917f6e7c89 100644 --- a/QGCApplication.pro +++ b/QGCApplication.pro @@ -228,7 +228,6 @@ HEADERS += \ src/Joystick/JoystickManager.h \ src/LogCompressor.h \ src/MG.h \ - src/MissionEditor/MissionEditorController.h \ src/MissionManager/MissionManager.h \ src/MissionManager/MissionController.h \ src/QGC.h \ @@ -347,7 +346,6 @@ SOURCES += \ src/Joystick/JoystickManager.cc \ src/LogCompressor.cc \ src/main.cc \ - src/MissionEditor/MissionEditorController.cc \ src/MissionManager/MissionManager.cc \ src/MissionManager/MissionController.cc \ src/QGC.cc \ diff --git a/src/FactSystem/FactSystemTestBase.cc b/src/FactSystem/FactSystemTestBase.cc index 01168f0a9594554d57c44b6a39c8d2b8535b2ebf..cc1b1b2ab57afa660e57825279db38d1e1c3bc0a 100644 --- a/src/FactSystem/FactSystemTestBase.cc +++ b/src/FactSystem/FactSystemTestBase.cc @@ -45,7 +45,7 @@ void FactSystemTestBase::_init(MAV_AUTOPILOT autopilot) UnitTest::init(); MockLink* link = new MockLink(); - link->setAutopilotType(autopilot); + link->setFirmwareType(autopilot); LinkManager::instance()->_addLink(link); LinkManager::instance()->connectLink(link); diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc index 92aec3bf98f3ee4103dbb3d67cff3db8bc389b89..f2cfa3ed80ff7bf10221b26e1291cbb1ad1e5b46 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc @@ -378,3 +378,9 @@ void APMFirmwarePlugin::setSupportedModes(QList supportedModes) { _supportedModes = supportedModes; } + +bool APMFirmwarePlugin::sendHomePositionToVehicle(void) +{ + // APM stack wants the home position sent in the first position + return true; +} diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h index fa639db1651641d6a72519d7b38719dff82da48f..6d92ab778e7d27f570845f9fd27a34a8f7556deb 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h @@ -87,7 +87,8 @@ public: virtual int manualControlReservedButtonCount(void); virtual void adjustMavlinkMessage(mavlink_message_t* message); virtual void initializeVehicle(Vehicle* vehicle); - + virtual bool sendHomePositionToVehicle(void); + protected: /// All access to singleton is through stack specific implementation APMFirmwarePlugin(QObject* parent = NULL); diff --git a/src/FirmwarePlugin/FirmwarePlugin.h b/src/FirmwarePlugin/FirmwarePlugin.h index 3783c5a8db6b2576b7630836e2195a99273e7f5b..eacd29ebbacf66a7755ae1513356da1c73c8f3e2 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.h +++ b/src/FirmwarePlugin/FirmwarePlugin.h @@ -96,6 +96,14 @@ public: /// Called when Vehicle is first created to send any necessary mavlink messages to the firmware. virtual void initializeVehicle(Vehicle* vehicle) = 0; + + /// Determines how to handle the first item of the mission item list. Internally to QGC the first item + /// is always the home position. + /// @return + /// true: Send first mission item as home position to vehicle. When vehicle has no mission items on + /// it, it may or may not return a home position back in position 0. + /// false: Do not send first item to vehicle, sequence numbers must be adjusted + virtual bool sendHomePositionToVehicle(void) = 0; protected: FirmwarePlugin(QObject* parent = NULL) : QGCSingleton(parent) { } diff --git a/src/FirmwarePlugin/Generic/GenericFirmwarePlugin.cc b/src/FirmwarePlugin/Generic/GenericFirmwarePlugin.cc index 910eba2f8ee99abbfb1794098be820df84b91e88..0971990949e46093c0acd390a6c5137ec7405d3e 100644 --- a/src/FirmwarePlugin/Generic/GenericFirmwarePlugin.cc +++ b/src/FirmwarePlugin/Generic/GenericFirmwarePlugin.cc @@ -110,3 +110,11 @@ void GenericFirmwarePlugin::initializeVehicle(Vehicle* vehicle) // Generic Flight Stack is by definition "generic", so no extra work } + +bool GenericFirmwarePlugin::sendHomePositionToVehicle(void) +{ + // Generic stack does not want home position sent in the first position. + // Subsequent sequence numbers must be adjusted. + // This is the mavlink spec default. + return false; +} diff --git a/src/FirmwarePlugin/Generic/GenericFirmwarePlugin.h b/src/FirmwarePlugin/Generic/GenericFirmwarePlugin.h index 7b2634e4f6a313ba5e722fcf30f4ff03e39e6df5..abeb96e773863ec62385326edd706236f6df4972 100644 --- a/src/FirmwarePlugin/Generic/GenericFirmwarePlugin.h +++ b/src/FirmwarePlugin/Generic/GenericFirmwarePlugin.h @@ -46,6 +46,7 @@ public: virtual int manualControlReservedButtonCount(void); virtual void adjustMavlinkMessage(mavlink_message_t* message); virtual void initializeVehicle(Vehicle* vehicle); + virtual bool sendHomePositionToVehicle(void); private: /// All access to singleton is through AutoPilotPluginManager::instance diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc index 2a2de268b7f826e0f28e58c43886271c0be2be6f..d78c1914c7b1daf12d62cc1b9bab1e58a14f22c1 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc @@ -200,3 +200,10 @@ void PX4FirmwarePlugin::initializeVehicle(Vehicle* vehicle) // PX4 Flight Stack doesn't need to do any extra work } + +bool PX4FirmwarePlugin::sendHomePositionToVehicle(void) +{ + // PX4 stack does not want home position sent in the first position. + // Subsequent sequence numbers must be adjusted. + return false; +} diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h index f1400622b12812b1d9f197d9a58657433c8d2447..ebbd49ebb1780ba67744dc6e3d2f5730c5ea199a 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h @@ -46,6 +46,7 @@ public: virtual int manualControlReservedButtonCount(void); virtual void adjustMavlinkMessage(mavlink_message_t* message); virtual void initializeVehicle(Vehicle* vehicle); + virtual bool sendHomePositionToVehicle(void); private: /// All access to singleton is through AutoPilotPluginManager::instance diff --git a/src/FlightDisplay/FlightDisplayView.qml b/src/FlightDisplay/FlightDisplayView.qml index 37bceaf65c241e79e926b9595dfa0cb9fdb2bc45..ec7d26443605813fd53b54bcfb797b4bc00c7a01 100644 --- a/src/FlightDisplay/FlightDisplayView.qml +++ b/src/FlightDisplay/FlightDisplayView.qml @@ -69,21 +69,26 @@ Item { readonly property var _flightMap: flightMap - property real _roll: _activeVehicle ? (isNaN(_activeVehicle.roll) ? _defaultRoll : _activeVehicle.roll) : _defaultRoll - property real _pitch: _activeVehicle ? (isNaN(_activeVehicle.pitch) ? _defaultPitch : _activeVehicle.pitch) : _defaultPitch - property real _heading: _activeVehicle ? (isNaN(_activeVehicle.heading) ? _defaultHeading : _activeVehicle.heading) : _defaultHeading + property real _roll: _activeVehicle ? (isNaN(_activeVehicle.roll) ? _defaultRoll : _activeVehicle.roll) : _defaultRoll + property real _pitch: _activeVehicle ? (isNaN(_activeVehicle.pitch) ? _defaultPitch : _activeVehicle.pitch) : _defaultPitch + property real _heading: _activeVehicle ? (isNaN(_activeVehicle.heading) ? _defaultHeading : _activeVehicle.heading) : _defaultHeading - property var _vehicleCoordinate: _activeVehicle ? _activeVehicle.coordinate : _defaultVehicleCoordinate + property var _vehicleCoordinate: _activeVehicle ? (_activeVehicle.satelliteLock >= 2 ? _activeVehicle.coordinate : _defaultVehicleCoordinate) : _defaultVehicleCoordinate - property real _altitudeWGS84: _activeVehicle ? _activeVehicle.altitudeWGS84 : _defaultAltitudeWGS84 - property real _groundSpeed: _activeVehicle ? _activeVehicle.groundSpeed : _defaultGroundSpeed - property real _airSpeed: _activeVehicle ? _activeVehicle.airSpeed : _defaultAirSpeed - property real _climbRate: _activeVehicle ? _activeVehicle.climbRate : _defaultClimbRate + property real _altitudeWGS84: _activeVehicle ? _activeVehicle.altitudeWGS84 : _defaultAltitudeWGS84 + property real _groundSpeed: _activeVehicle ? _activeVehicle.groundSpeed : _defaultGroundSpeed + property real _airSpeed: _activeVehicle ? _activeVehicle.airSpeed : _defaultAirSpeed + property real _climbRate: _activeVehicle ? _activeVehicle.climbRate : _defaultClimbRate property bool _showMap: getBool(QGroundControl.flightMapSettings.loadMapSetting(flightMap.mapName, _showMapBackgroundKey, "1")) FlightDisplayViewController { id: _controller } - MissionController { id: _missionController } + + MissionController { + id: _missionController + + Component.onCompleted: start(false /* editMode */) + } ExclusiveGroup { id: _dropButtonsExclusiveGroup diff --git a/src/MissionEditor/MissionEditor.qml b/src/MissionEditor/MissionEditor.qml index 5a72bccf42ab902af5a20dc31eeb24a55748d9c9..fc1c6709b661a0f6a6e738c6973eef2a7936f18c 100644 --- a/src/MissionEditor/MissionEditor.qml +++ b/src/MissionEditor/MissionEditor.qml @@ -54,24 +54,27 @@ QGCView { readonly property string _autoSyncKey: "AutoSync" readonly property string _showHelpKey: "ShowHelp" readonly property int _addMissionItemsButtonAutoOffTimeout: 10000 + readonly property var _defaultVehicleCoordinate: QtPositioning.coordinate(37.803784, -122.462276) property var _missionItems: controller.missionItems - property var _homePositionManager: QGroundControl.homePositionManager - property string _homePositionName: _homePositionManager.homePositions.get(0).name - + //property var _homePositionManager: QGroundControl.homePositionManager + //property string _homePositionName: _homePositionManager.homePositions.get(0).name //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 + property var homePosition: _defaultVehicleCoordinate property bool _syncNeeded: controller.missionItems.dirty property bool _syncInProgress: _activeVehicle ? _activeVehicle.missionManager.inProgress : false property bool _showHelp: QGroundControl.flightMapSettings.loadBoolMapSetting(editorMap.mapName, _showHelpKey, true) - MissionEditorController { + MissionController { id: controller + + Component.onCompleted: start(true /* editMode */) /* FIXME: autoSync is temporarily disconnected since it's still buggy @@ -104,6 +107,7 @@ QGCView { function updateHomePosition() { if (liveHomePositionAvailable) { + homePosition = liveHomePosition _missionItems.get(0).coordinate = liveHomePosition _missionItems.get(0).homePositionValid = true } @@ -280,8 +284,8 @@ QGCView { } } // Item - Mission Item editor -/* -Home Position Manager is commented out for now until a better implementation is completed + /* + Home Position Manager temporarily disbled till more work is done on it // Home Position Manager Rectangle { @@ -554,7 +558,7 @@ Home Position Manager is commented out for now until a better implementation is } } // Column - Online view } // Item - Home Position Manager -*/ + */ // Help Panel Rectangle { @@ -654,7 +658,7 @@ Home Position Manager is commented out for now until a better implementation is } /* - Home Position Manager commented until more complete implementation is done + Home Position Manager disabled Image { id: homePositionManagerHelpIcon @@ -678,7 +682,7 @@ Home Position Manager is commented out for now until a better implementation is "When enabled, allows you to select/add/update flying field locations. " + "You can save multiple flying field locations for use while creating missions while you are not connected to your vehicle." } -*/ + */ Image { id: mapCenterHelpIcon @@ -801,8 +805,7 @@ Home Position Manager is commented out for now until a better implementation is } /* - Home Position Manager commented until more complete implementation is done - + Home Position manager temporarily disable RoundButton { id: homePositionManagerButton anchors.margins: _margin diff --git a/src/MissionEditor/MissionEditorController.cc b/src/MissionEditor/MissionEditorController.cc deleted file mode 100644 index e1e7522b996967832de38d674fcbd3d31041483d..0000000000000000000000000000000000000000 --- a/src/MissionEditor/MissionEditorController.cc +++ /dev/null @@ -1,493 +0,0 @@ -/*===================================================================== - -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 "MissionEditorController.h" -#include "ScreenToolsController.h" -#include "MultiVehicleManager.h" -#include "MissionManager.h" -#include "QGCFileDialog.h" -#include "CoordinateVector.h" -#include "QGCMessageBox.h" - -#include -#include -#include - -const char* MissionEditorController::_settingsGroup = "MissionEditorController"; - -MissionEditorController::MissionEditorController(QObject *parent) - : QObject(parent) - , _missionItems(NULL) - , _canEdit(true) - , _activeVehicle(NULL) - , _liveHomePositionAvailable(false) - , _autoSync(false) - , _firstMissionItemSync(false) - , _missionItemsRequested(false) -{ - MultiVehicleManager* multiVehicleMgr = MultiVehicleManager::instance(); - - connect(multiVehicleMgr, &MultiVehicleManager::activeVehicleChanged, this, &MissionEditorController::_activeVehicleChanged); - - Vehicle* activeVehicle = multiVehicleMgr->activeVehicle(); - if (activeVehicle) { - _activeVehicleChanged(activeVehicle); - } else { - _missionItemsRequested = true; - _newMissionItemsAvailable(); - } -} - -MissionEditorController::~MissionEditorController() -{ -} - -void MissionEditorController::_newMissionItemsAvailable(void) -{ - if (_firstMissionItemSync) { - // This is the first time the vehicle is seeing items. We have to be careful of transitioning from offline - // to online. - - _firstMissionItemSync = false; - if (_missionItems && _missionItems->count() > 1) { - QGCMessageBox::StandardButton button = QGCMessageBox::warning("Mission Editing", - "The vehicle has sent a new set of Mission Items. " - "Do you want to discard your current set of unsaved items and use the ones from the vehicle instead?", - QGCMessageBox::Yes | QGCMessageBox::No, - QGCMessageBox::No); - if (button == QGCMessageBox::No) { - return; - } - } - } else if (!_missionItemsRequested) { - // We didn't specifically ask for new mission items. Disregard the new set since it is - // the most likely the set we just sent to the vehicle. - return; - } - - _missionItemsRequested = false; - - if (_missionItems) { - _deinitAllMissionItems(); - _missionItems->deleteLater(); - } - - MissionManager* missionManager = NULL; - if (_activeVehicle) { - missionManager = _activeVehicle->missionManager(); - } - - if (!missionManager || missionManager->inProgress()) { - _canEdit = true; - _missionItems = new QmlObjectListModel(this); - } else { - _canEdit = missionManager->canEdit(); - _missionItems = missionManager->copyMissionItems(); - } - - _initAllMissionItems(); -} - -void MissionEditorController::getMissionItems(void) -{ - Vehicle* activeVehicle = MultiVehicleManager::instance()->activeVehicle(); - - if (activeVehicle) { - _missionItemsRequested = true; - MissionManager* missionManager = activeVehicle->missionManager(); - connect(missionManager, &MissionManager::newMissionItemsAvailable, this, &MissionEditorController::_newMissionItemsAvailable); - activeVehicle->missionManager()->requestMissionItems(); - } -} - -void MissionEditorController::sendMissionItems(void) -{ - Vehicle* activeVehicle = MultiVehicleManager::instance()->activeVehicle(); - - if (activeVehicle) { - activeVehicle->missionManager()->writeMissionItems(*_missionItems, true /* skipFirstItem */); - _missionItems->setDirty(false); - } -} - -int MissionEditorController::addMissionItem(QGeoCoordinate coordinate) -{ - if (!_canEdit) { - qWarning() << "addMissionItem called with _canEdit == false"; - } - - // Coordinate will come through without altitude - coordinate.setAltitude(MissionItem::defaultAltitude); - - MissionItem * newItem = new MissionItem(this, _missionItems->count(), coordinate, MAV_CMD_NAV_WAYPOINT); - _initMissionItem(newItem); - if (_missionItems->count() == 1) { - newItem->setCommand(MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF); - } - _missionItems->append(newItem); - - _recalcAll(); - - return _missionItems->count() - 1; -} - -void MissionEditorController::removeMissionItem(int index) -{ - if (!_canEdit) { - qWarning() << "addMissionItem called with _canEdit == false"; - return; - } - - MissionItem* item = qobject_cast(_missionItems->removeAt(index)); - - _deinitMissionItem(item); - - _recalcAll(); - - // Set the new current item - - if (index >= _missionItems->count()) { - index--; - } - for (int i=0; i<_missionItems->count(); i++) { - MissionItem* item = qobject_cast(_missionItems->get(i)); - item->setIsCurrentItem(i == index); - } -} - -void MissionEditorController::loadMissionFromFile(void) -{ - QString errorString; - QString filename = QGCFileDialog::getOpenFileName(NULL, "Select Mission File to load"); - - if (filename.isEmpty()) { - return; - } - - if (_missionItems) { - _deinitAllMissionItems(); - _missionItems->deleteLater(); - } - _missionItems = new QmlObjectListModel(this); - - _canEdit = true; - - QFile file(filename); - - if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) { - errorString = file.errorString(); - } else { - QTextStream in(&file); - - const QStringList& version = in.readLine().split(" "); - - if (!(version.size() == 3 && version[0] == "QGC" && version[1] == "WPL" && version[2] == "120")) { - errorString = "The mission file is not compatible with the current version of QGroundControl."; - } else { - while (!in.atEnd()) { - MissionItem* item = new MissionItem(); - - if (item->load(in)) { - _missionItems->append(item); - - if (!item->canEdit()) { - _canEdit = false; - } - } else { - errorString = "The mission file is corrupted."; - break; - } - } - } - - } - - if (!errorString.isEmpty()) { - _missionItems->clear(); - } - - _initAllMissionItems(); -} - -void MissionEditorController::saveMissionToFile(void) -{ - QString errorString; - QString filename = QGCFileDialog::getSaveFileName(NULL, "Select file to save mission to"); - - if (filename.isEmpty()) { - return; - } - - QFile file(filename); - - if (!file.open(QIODevice::WriteOnly | QIODevice::Text)) { - errorString = file.errorString(); - } else { - QTextStream out(&file); - - out << "QGC WPL 120\r\n"; // Version string - - for (int i=0; i<_missionItems->count(); i++) { - qobject_cast(_missionItems->get(i))->save(out); - } - } - - _missionItems->setDirty(false); -} - -void MissionEditorController::_recalcWaypointLines(void) -{ - bool firstCoordinateItem = true; - MissionItem* lastCoordinateItem = qobject_cast(_missionItems->get(0)); - - _waypointLines.clear(); - - for (int i=1; i<_missionItems->count(); i++) { - MissionItem* item = qobject_cast(_missionItems->get(i)); - - if (item->specifiesCoordinate()) { - if (firstCoordinateItem) { - if (item->command() == MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF) { - // The first coordinate we hit is a takeoff command so link back to home position - _waypointLines.append(new CoordinateVector(qobject_cast(_missionItems->get(0))->coordinate(), item->coordinate())); - } else { - // First coordiante is not a takeoff command, it does not link backwards to anything - } - firstCoordinateItem = false; - } else { - // Subsequent coordinate items link to last coordinate item - _waypointLines.append(new CoordinateVector(lastCoordinateItem->coordinate(), item->coordinate())); - } - lastCoordinateItem = item; - } - } - - emit waypointLinesChanged(); -} - -// This will update the sequence numbers to be sequential starting from 0 -void MissionEditorController::_recalcSequence(void) -{ - for (int i=0; i<_missionItems->count(); i++) { - MissionItem* item = qobject_cast(_missionItems->get(i)); - - // Setup ascending sequence numbers - item->setSequenceNumber(i); - } -} - -// This will update the child item hierarchy -void MissionEditorController::_recalcChildItems(void) -{ - MissionItem* currentParentItem = qobject_cast(_missionItems->get(0)); - - currentParentItem->childItems()->clear(); - - for (int i=1; i<_missionItems->count(); i++) { - MissionItem* item = qobject_cast(_missionItems->get(i)); - - // Set up non-coordinate item child hierarchy - if (item->specifiesCoordinate()) { - item->childItems()->clear(); - currentParentItem = item; - } else { - currentParentItem->childItems()->append(item); - } - } -} - -void MissionEditorController::_recalcAll(void) -{ - _recalcSequence(); - _recalcChildItems(); - _recalcWaypointLines(); -} - -/// Initializes a new set of mission items which may have come from the vehicle or have been loaded from a file -void MissionEditorController::_initAllMissionItems(void) -{ - // Add the home position item to the front - MissionItem* homeItem = new MissionItem(this); - homeItem->setHomePositionSpecialCase(true); - homeItem->setCommand(MavlinkQmlSingleton::MAV_CMD_NAV_WAYPOINT); - _missionItems->insert(0, homeItem); - - for (int i=0; i<_missionItems->count(); i++) { - _initMissionItem(qobject_cast(_missionItems->get(i))); - } - - _recalcAll(); - - emit missionItemsChanged(); - emit canEditChanged(_canEdit); - - _missionItems->setDirty(false); - - connect(_missionItems, &QmlObjectListModel::dirtyChanged, this, &MissionEditorController::_dirtyChanged); -} - -void MissionEditorController::_deinitAllMissionItems(void) -{ - for (int i=0; i<_missionItems->count(); i++) { - _deinitMissionItem(qobject_cast(_missionItems->get(i))); - } - - connect(_missionItems, &QmlObjectListModel::dirtyChanged, this, &MissionEditorController::_dirtyChanged); -} - -void MissionEditorController::_initMissionItem(MissionItem* item) -{ - _missionItems->setDirty(false); - - connect(item, &MissionItem::commandChanged, this, &MissionEditorController::_itemCommandChanged); - connect(item, &MissionItem::coordinateChanged, this, &MissionEditorController::_itemCoordinateChanged); -} - -void MissionEditorController::_deinitMissionItem(MissionItem* item) -{ - disconnect(item, &MissionItem::commandChanged, this, &MissionEditorController::_itemCommandChanged); - disconnect(item, &MissionItem::coordinateChanged, this, &MissionEditorController::_itemCoordinateChanged); -} - -void MissionEditorController::_itemCoordinateChanged(const QGeoCoordinate& coordinate) -{ - Q_UNUSED(coordinate); - _recalcWaypointLines(); -} - -void MissionEditorController::_itemCommandChanged(MavlinkQmlSingleton::Qml_MAV_CMD command) -{ - Q_UNUSED(command);; - _recalcChildItems(); - _recalcWaypointLines(); -} - -void MissionEditorController::_activeVehicleChanged(Vehicle* activeVehicle) -{ - if (_activeVehicle) { - MissionManager* missionManager = _activeVehicle->missionManager(); - - disconnect(missionManager, &MissionManager::newMissionItemsAvailable, this, &MissionEditorController::_newMissionItemsAvailable); - disconnect(missionManager, &MissionManager::inProgressChanged, this, &MissionEditorController::_inProgressChanged); - disconnect(_activeVehicle, &Vehicle::homePositionAvailableChanged, this, &MissionEditorController::_activeVehicleHomePositionAvailableChanged); - disconnect(_activeVehicle, &Vehicle::homePositionChanged, this, &MissionEditorController::_activeVehicleHomePositionChanged); - _activeVehicle = NULL; - _newMissionItemsAvailable(); - _activeVehicleHomePositionAvailableChanged(false); - } - - _activeVehicle = activeVehicle; - - if (_activeVehicle) { - MissionManager* missionManager = activeVehicle->missionManager(); - - connect(missionManager, &MissionManager::newMissionItemsAvailable, this, &MissionEditorController::_newMissionItemsAvailable); - connect(missionManager, &MissionManager::inProgressChanged, this, &MissionEditorController::_inProgressChanged); - connect(_activeVehicle, &Vehicle::homePositionAvailableChanged, this, &MissionEditorController::_activeVehicleHomePositionAvailableChanged); - connect(_activeVehicle, &Vehicle::homePositionChanged, this, &MissionEditorController::_activeVehicleHomePositionChanged); - - if (missionManager->inProgress()) { - // Vehicle is still in process of requesting mission items - _firstMissionItemSync = true; - } else { - // Vehicle already has mission items - _firstMissionItemSync = false; - } - - _missionItemsRequested = true; - _newMissionItemsAvailable(); - - _activeVehicleHomePositionChanged(_activeVehicle->homePosition()); - _activeVehicleHomePositionAvailableChanged(_activeVehicle->homePositionAvailable()); - } -} - -void MissionEditorController::_activeVehicleHomePositionAvailableChanged(bool homePositionAvailable) -{ - _liveHomePositionAvailable = homePositionAvailable; - emit liveHomePositionAvailableChanged(_liveHomePositionAvailable); -} - -void MissionEditorController::_activeVehicleHomePositionChanged(const QGeoCoordinate& homePosition) -{ - _liveHomePosition = homePosition; - emit liveHomePositionChanged(_liveHomePosition); -} - -void MissionEditorController::deleteCurrentMissionItem(void) -{ - for (int i=0; i<_missionItems->count(); i++) { - MissionItem* item = qobject_cast(_missionItems->get(i)); - if (item->isCurrentItem() && i != 0) { - removeMissionItem(i); - return; - } - } -} - -void MissionEditorController::setAutoSync(bool autoSync) -{ - _autoSync = autoSync; - emit autoSyncChanged(_autoSync); - - if (_autoSync) { - _dirtyChanged(true); - } -} - -void MissionEditorController::_dirtyChanged(bool dirty) -{ - if (dirty && _autoSync) { - Vehicle* activeVehicle = MultiVehicleManager::instance()->activeVehicle(); - - if (activeVehicle && !activeVehicle->armed()) { - if (_activeVehicle->missionManager()->inProgress()) { - _queuedSend = true; - } else { - _autoSyncSend(); - } - } - } -} - -void MissionEditorController::_autoSyncSend(void) -{ - qDebug() << "Auto-syncing with vehicle"; - _queuedSend = false; - if (_missionItems) { - sendMissionItems(); - _missionItems->setDirty(false); - } -} - -void MissionEditorController::_inProgressChanged(bool inProgress) -{ - if (!inProgress && _queuedSend) { - _autoSyncSend(); - } -} - -QmlObjectListModel* MissionEditorController::missionItems(void) -{ - return _missionItems; -} diff --git a/src/MissionEditor/MissionEditorController.h b/src/MissionEditor/MissionEditorController.h deleted file mode 100644 index e74e6daea17e8855fd99245b59b6c69879aa9561..0000000000000000000000000000000000000000 --- a/src/MissionEditor/MissionEditorController.h +++ /dev/null @@ -1,109 +0,0 @@ -/*===================================================================== - -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 MissionEditorController_H -#define MissionEditorController_H - -#include - -#include "QmlObjectListModel.h" -#include "Vehicle.h" - -class MissionEditorController : public QObject -{ - Q_OBJECT - -public: - MissionEditorController(QObject* parent = NULL); - ~MissionEditorController(); - - 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_PROPERTY(bool autoSync READ autoSync WRITE setAutoSync NOTIFY autoSyncChanged) - - Q_INVOKABLE int addMissionItem(QGeoCoordinate coordinate); - Q_INVOKABLE void getMissionItems(void); - Q_INVOKABLE void sendMissionItems(void); - Q_INVOKABLE void loadMissionFromFile(void); - Q_INVOKABLE void saveMissionToFile(void); - Q_INVOKABLE void removeMissionItem(int index); - Q_INVOKABLE void deleteCurrentMissionItem(void); - - // Property accessors - - QmlObjectListModel* missionItems(void); - QmlObjectListModel* waypointLines(void) { return &_waypointLines; } - bool canEdit(void) { return _canEdit; } - bool liveHomePositionAvailable(void) { return _liveHomePositionAvailable; } - QGeoCoordinate liveHomePosition(void) { return _liveHomePosition; } - bool autoSync(void) { return _autoSync; } - void setAutoSync(bool autoSync); - -signals: - void missionItemsChanged(void); - void canEditChanged(bool canEdit); - void waypointLinesChanged(void); - void liveHomePositionAvailableChanged(bool homePositionAvailable); - void liveHomePositionChanged(const QGeoCoordinate& homePosition); - void autoSyncChanged(bool autoSync); - -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); - void _dirtyChanged(bool dirty); - void _inProgressChanged(bool inProgress); - -private: - void _recalcSequence(void); - void _recalcWaypointLines(void); - void _recalcChildItems(void); - void _recalcAll(void); - void _initAllMissionItems(void); - void _deinitAllMissionItems(void); - void _initMissionItem(MissionItem* item); - void _deinitMissionItem(MissionItem* item); - void _autoSyncSend(void); - -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; - bool _autoSync; - bool _firstMissionItemSync; - bool _missionItemsRequested; - bool _queuedSend; - - static const char* _settingsGroup; -}; - -#endif diff --git a/src/MissionItem.h b/src/MissionItem.h index 91ef639affb7f0c7516cdf28bb4bd65e54427205..11005d26ea6e48d927828427deb7be25c918acd1 100644 --- a/src/MissionItem.h +++ b/src/MissionItem.h @@ -86,7 +86,7 @@ public: Q_PROPERTY(QmlObjectListModel* childItems READ childItems CONSTANT) /// true: this item is being used as a home position indicator - Q_PROPERTY(bool homePosition MEMBER _homePositionSpecialCase CONSTANT) + Q_PROPERTY(bool homePosition READ homePosition CONSTANT) /// true: home position should be shown Q_PROPERTY(bool homePositionValid READ homePositionValid WRITE setHomePositionValid NOTIFY homePositionValidChanged) @@ -131,6 +131,7 @@ public: QmlObjectListModel* childItems(void) { return &_childItems; } + bool homePosition(void) { return _homePositionSpecialCase; } bool homePositionValid(void) { return _homePositionValid; } void setHomePositionValid(bool homePositionValid); diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index 41da7139d062a5c2768dad7ff802137766556230..0e05c17993666c4c451a24f6d2658f7f4cdde94e 100644 --- a/src/MissionManager/MissionController.cc +++ b/src/MissionManager/MissionController.cc @@ -22,109 +22,281 @@ This file is part of the QGROUNDCONTROL project ======================================================================*/ #include "MissionController.h" -#include "ScreenToolsController.h" #include "MultiVehicleManager.h" #include "MissionManager.h" +#include "QGCFileDialog.h" #include "CoordinateVector.h" +#include "QGCMessageBox.h" +#include "FirmwarePlugin.h" + +const char* MissionController::_settingsGroup = "MissionController"; MissionController::MissionController(QObject *parent) : QObject(parent) + , _editMode(false) , _missionItems(NULL) + , _canEdit(true) , _activeVehicle(NULL) + , _liveHomePositionAvailable(false) + , _autoSync(false) + , _firstMissionItemSync(false) + , _missionItemsRequested(false) + , _queuedSend(false) { + +} + +MissionController::~MissionController() +{ +} + +void MissionController::start(bool editMode) +{ + _editMode = editMode; + MultiVehicleManager* multiVehicleMgr = MultiVehicleManager::instance(); - + connect(multiVehicleMgr, &MultiVehicleManager::activeVehicleChanged, this, &MissionController::_activeVehicleChanged); - + Vehicle* activeVehicle = multiVehicleMgr->activeVehicle(); if (activeVehicle) { _activeVehicleChanged(activeVehicle); } else { + _missionItemsRequested = true; _newMissionItemsAvailable(); } } -MissionController::~MissionController() -{ -} - void MissionController::_newMissionItemsAvailable(void) { + if (_editMode) { + if (_firstMissionItemSync) { + // This is the first time the vehicle is seeing items. We have to be careful of transitioning from offline + // to online. + + _firstMissionItemSync = false; + if (_missionItems && _missionItems->count() > 1) { + QGCMessageBox::StandardButton button = QGCMessageBox::warning("Mission Editing", + "The vehicle has sent a new set of Mission Items. " + "Do you want to discard your current set of unsaved items and use the ones from the vehicle instead?", + QGCMessageBox::Yes | QGCMessageBox::No, + QGCMessageBox::No); + if (button == QGCMessageBox::No) { + return; + } + } + } else if (!_missionItemsRequested) { + // We didn't specifically ask for new mission items. Disregard the new set since it is + // the most likely the set we just sent to the vehicle. + return; + } + } + + _missionItemsRequested = false; + if (_missionItems) { + _deinitAllMissionItems(); _missionItems->deleteLater(); } - + MissionManager* missionManager = NULL; - Vehicle* activeVehicle = MultiVehicleManager::instance()->activeVehicle(); - if (activeVehicle) { - missionManager = activeVehicle->missionManager(); + if (_activeVehicle) { + missionManager = _activeVehicle->missionManager(); } if (!missionManager || missionManager->inProgress()) { + _canEdit = true; _missionItems = new QmlObjectListModel(this); } else { + _canEdit = missionManager->canEdit(); _missionItems = missionManager->copyMissionItems(); } _initAllMissionItems(); } -void MissionController::_recalcWaypointLines(void) +void MissionController::getMissionItems(void) { - int firstIndex = _homePositionValid ? 0 : 1; + Vehicle* activeVehicle = MultiVehicleManager::instance()->activeVehicle(); - _waypointLines.clear(); + if (activeVehicle) { + _missionItemsRequested = true; + MissionManager* missionManager = activeVehicle->missionManager(); + connect(missionManager, &MissionManager::newMissionItemsAvailable, this, &MissionController::_newMissionItemsAvailable); + activeVehicle->missionManager()->requestMissionItems(); + } +} + +void MissionController::sendMissionItems(void) +{ + Vehicle* activeVehicle = MultiVehicleManager::instance()->activeVehicle(); - if (firstIndex < _missionItems->count()) { - bool firstCoordinateItem = true; - MissionItem* lastCoordinateItem = qobject_cast(_missionItems->get(firstIndex)); + if (activeVehicle) { + activeVehicle->missionManager()->writeMissionItems(*_missionItems); + _missionItems->setDirty(false); + } +} - for (int i=firstIndex; i<_missionItems->count(); i++) { - MissionItem* item = qobject_cast(_missionItems->get(i)); +int MissionController::addMissionItem(QGeoCoordinate coordinate) +{ + if (!_canEdit) { + qWarning() << "addMissionItem called with _canEdit == false"; + } - if (item->specifiesCoordinate()) { - if (firstCoordinateItem) { - if (item->command() == MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF && _homePositionValid) { - // The first coordinate we hit is a takeoff command so link back to home position if we have one - _waypointLines.append(new CoordinateVector(qobject_cast(_missionItems->get(0))->coordinate(), item->coordinate())); - } else { - // First coordiante is not a takeoff command, it does not link backwards to anything + // Coordinate will come through without altitude + coordinate.setAltitude(MissionItem::defaultAltitude); + + MissionItem * newItem = new MissionItem(this, _missionItems->count(), coordinate, MAV_CMD_NAV_WAYPOINT); + _initMissionItem(newItem); + if (_missionItems->count() == 1) { + newItem->setCommand(MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF); + } + _missionItems->append(newItem); + + _recalcAll(); + + return _missionItems->count() - 1; +} + +void MissionController::removeMissionItem(int index) +{ + if (!_canEdit) { + qWarning() << "addMissionItem called with _canEdit == false"; + return; + } + + MissionItem* item = qobject_cast(_missionItems->removeAt(index)); + + _deinitMissionItem(item); + + _recalcAll(); + + // Set the new current item + + if (index >= _missionItems->count()) { + index--; + } + for (int i=0; i<_missionItems->count(); i++) { + MissionItem* item = qobject_cast(_missionItems->get(i)); + item->setIsCurrentItem(i == index); + } +} + +void MissionController::loadMissionFromFile(void) +{ + QString errorString; + QString filename = QGCFileDialog::getOpenFileName(NULL, "Select Mission File to load"); + + if (filename.isEmpty()) { + return; + } + + if (_missionItems) { + _deinitAllMissionItems(); + _missionItems->deleteLater(); + } + _missionItems = new QmlObjectListModel(this); + + _canEdit = true; + + // FIXME: This needs to handle APM files which have WP 0 in them + + QFile file(filename); + + if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) { + errorString = file.errorString(); + } else { + QTextStream in(&file); + + const QStringList& version = in.readLine().split(" "); + + if (!(version.size() == 3 && version[0] == "QGC" && version[1] == "WPL" && version[2] == "120")) { + errorString = "The mission file is not compatible with the current version of QGroundControl."; + } else { + while (!in.atEnd()) { + MissionItem* item = new MissionItem(); + + if (item->load(in)) { + _missionItems->append(item); + + if (!item->canEdit()) { + _canEdit = false; } - firstCoordinateItem = false; } else { - // Subsequent coordinate items link to last coordinate item - _waypointLines.append(new CoordinateVector(lastCoordinateItem->coordinate(), item->coordinate())); + errorString = "The mission file is corrupted."; + break; } - lastCoordinateItem = item; } } + } - - emit waypointLinesChanged(); + + if (!errorString.isEmpty()) { + _missionItems->clear(); + } + + _initAllMissionItems(); } -// This will update the child item hierarchy -void MissionController::_recalcChildItems(void) +void MissionController::saveMissionToFile(void) { - int firstIndex = _homePositionValid ? 0 : 1; + QString errorString; + QString filename = QGCFileDialog::getSaveFileName(NULL, "Select file to save mission to"); + + if (filename.isEmpty()) { + return; + } - if (_missionItems->count() > firstIndex) { - MissionItem* currentParentItem = qobject_cast(_missionItems->get(firstIndex)); + QFile file(filename); - currentParentItem->childItems()->clear(); + if (!file.open(QIODevice::WriteOnly | QIODevice::Text)) { + errorString = file.errorString(); + } else { + QTextStream out(&file); - for (int i=firstIndex+1; i<_missionItems->count(); i++) { - MissionItem* item = qobject_cast(_missionItems->get(i)); + out << "QGC WPL 120\r\n"; // Version string - // Set up non-coordinate item child hierarchy - if (item->specifiesCoordinate()) { - item->childItems()->clear(); - currentParentItem = item; - } else { - currentParentItem->childItems()->append(item); + for (int i=1; i<_missionItems->count(); i++) { + qobject_cast(_missionItems->get(i))->save(out); + } + } + + _missionItems->setDirty(false); +} + +void MissionController::_recalcWaypointLines(void) +{ + MissionItem* lastCoordinateItem = qobject_cast(_missionItems->get(0)); + bool firstCoordinateItem = true; + + _waypointLines.clear(); + + for (int i=1; i<_missionItems->count(); i++) { + MissionItem* item = qobject_cast(_missionItems->get(i)); + + if (item->specifiesCoordinate()) { + if (firstCoordinateItem) { + if (item->command() == MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF) { + MissionItem* homeItem = qobject_cast(_missionItems->get(0)); + + // The first coordinate we hit is a takeoff command so link back to home position if valid + if (homeItem->homePositionValid()) { + _waypointLines.append(new CoordinateVector(qobject_cast(_missionItems->get(0))->coordinate(), item->coordinate())); + } + } else { + // First coordiante is not a takeoff command, it does not link backwards to anything + } + firstCoordinateItem = false; + } else if (!lastCoordinateItem->homePosition() || lastCoordinateItem->homePositionValid()) { + // Subsequent coordinate items link to last coordinate item. If the last coordinate item + // is an invalid home position we skip the line + _waypointLines.append(new CoordinateVector(lastCoordinateItem->coordinate(), item->coordinate())); } + lastCoordinateItem = item; } } + + emit waypointLinesChanged(); } // This will update the sequence numbers to be sequential starting from 0 @@ -138,6 +310,26 @@ void MissionController::_recalcSequence(void) } } +// This will update the child item hierarchy +void MissionController::_recalcChildItems(void) +{ + MissionItem* currentParentItem = qobject_cast(_missionItems->get(0)); + + currentParentItem->childItems()->clear(); + + for (int i=1; i<_missionItems->count(); i++) { + MissionItem* item = qobject_cast(_missionItems->get(i)); + + // Set up non-coordinate item child hierarchy + if (item->specifiesCoordinate()) { + item->childItems()->clear(); + currentParentItem = item; + } else { + currentParentItem->childItems()->append(item); + } + } +} + void MissionController::_recalcAll(void) { _recalcSequence(); @@ -148,41 +340,181 @@ void MissionController::_recalcAll(void) /// Initializes a new set of mission items which may have come from the vehicle or have been loaded from a file void MissionController::_initAllMissionItems(void) { - // Add the home position item to the front - MissionItem* homeItem = new MissionItem(this); - homeItem->setHomePositionSpecialCase(true); + MissionItem* homeItem = NULL; + + if (_activeVehicle && _activeVehicle->firmwarePlugin()->sendHomePositionToVehicle() && _missionItems->count() != 0) { + homeItem = qobject_cast(_missionItems->get(0)); + homeItem->setHomePositionSpecialCase(true); + } else { + // Add the home position item to the front + homeItem = new MissionItem(this); + homeItem->setHomePositionSpecialCase(true); + homeItem->setCommand(MavlinkQmlSingleton::MAV_CMD_NAV_WAYPOINT); + _missionItems->insert(0, homeItem); + } homeItem->setHomePositionValid(false); - homeItem->setCommand(MavlinkQmlSingleton::MAV_CMD_NAV_WAYPOINT); - homeItem->setLatitude(47.3769); - homeItem->setLongitude(8.549444); - _missionItems->insert(0, homeItem); - + + for (int i=0; i<_missionItems->count(); i++) { + _initMissionItem(qobject_cast(_missionItems->get(i))); + } + _recalcAll(); - + emit missionItemsChanged(); + emit canEditChanged(_canEdit); + + _missionItems->setDirty(false); + + connect(_missionItems, &QmlObjectListModel::dirtyChanged, this, &MissionController::_dirtyChanged); +} + +void MissionController::_deinitAllMissionItems(void) +{ + for (int i=0; i<_missionItems->count(); i++) { + _deinitMissionItem(qobject_cast(_missionItems->get(i))); + } + + connect(_missionItems, &QmlObjectListModel::dirtyChanged, this, &MissionController::_dirtyChanged); +} + +void MissionController::_initMissionItem(MissionItem* item) +{ + _missionItems->setDirty(false); + + connect(item, &MissionItem::commandChanged, this, &MissionController::_itemCommandChanged); + connect(item, &MissionItem::coordinateChanged, this, &MissionController::_itemCoordinateChanged); +} + +void MissionController::_deinitMissionItem(MissionItem* item) +{ + disconnect(item, &MissionItem::commandChanged, this, &MissionController::_itemCommandChanged); + disconnect(item, &MissionItem::coordinateChanged, this, &MissionController::_itemCoordinateChanged); +} + +void MissionController::_itemCoordinateChanged(const QGeoCoordinate& coordinate) +{ + Q_UNUSED(coordinate); + _recalcWaypointLines(); +} + +void MissionController::_itemCommandChanged(MavlinkQmlSingleton::Qml_MAV_CMD command) +{ + Q_UNUSED(command);; + _recalcChildItems(); + _recalcWaypointLines(); } void MissionController::_activeVehicleChanged(Vehicle* activeVehicle) { if (_activeVehicle) { MissionManager* missionManager = _activeVehicle->missionManager(); + disconnect(missionManager, &MissionManager::newMissionItemsAvailable, this, &MissionController::_newMissionItemsAvailable); + disconnect(missionManager, &MissionManager::inProgressChanged, this, &MissionController::_inProgressChanged); + disconnect(_activeVehicle, &Vehicle::homePositionAvailableChanged, this, &MissionController::_activeVehicleHomePositionAvailableChanged); + disconnect(_activeVehicle, &Vehicle::homePositionChanged, this, &MissionController::_activeVehicleHomePositionChanged); _activeVehicle = NULL; + _newMissionItemsAvailable(); + _activeVehicleHomePositionAvailableChanged(false); } - + _activeVehicle = activeVehicle; - + if (_activeVehicle) { MissionManager* missionManager = activeVehicle->missionManager(); + connect(missionManager, &MissionManager::newMissionItemsAvailable, this, &MissionController::_newMissionItemsAvailable); + connect(missionManager, &MissionManager::inProgressChanged, this, &MissionController::_inProgressChanged); + connect(_activeVehicle, &Vehicle::homePositionAvailableChanged, this, &MissionController::_activeVehicleHomePositionAvailableChanged); + connect(_activeVehicle, &Vehicle::homePositionChanged, this, &MissionController::_activeVehicleHomePositionChanged); + + if (missionManager->inProgress()) { + // Vehicle is still in process of requesting mission items + _firstMissionItemSync = true; + } else { + // Vehicle already has mission items + _firstMissionItemSync = false; + } + + _missionItemsRequested = true; _newMissionItemsAvailable(); + + _activeVehicleHomePositionChanged(_activeVehicle->homePosition()); + _activeVehicleHomePositionAvailableChanged(_activeVehicle->homePositionAvailable()); + } +} + +void MissionController::_activeVehicleHomePositionAvailableChanged(bool homePositionAvailable) +{ + _liveHomePositionAvailable = homePositionAvailable; + emit liveHomePositionAvailableChanged(_liveHomePositionAvailable); +} + +void MissionController::_activeVehicleHomePositionChanged(const QGeoCoordinate& homePosition) +{ + _liveHomePosition = homePosition; + emit liveHomePositionChanged(_liveHomePosition); +} + +void MissionController::deleteCurrentMissionItem(void) +{ + for (int i=0; i<_missionItems->count(); i++) { + MissionItem* item = qobject_cast(_missionItems->get(i)); + if (item->isCurrentItem() && i != 0) { + removeMissionItem(i); + return; + } + } +} + +void MissionController::setAutoSync(bool autoSync) +{ + // FIXME: AutoSync temporarily turned off +#if 0 + _autoSync = autoSync; + emit autoSyncChanged(_autoSync); + + if (_autoSync) { + _dirtyChanged(true); + } +#else + Q_UNUSED(autoSync) +#endif +} + +void MissionController::_dirtyChanged(bool dirty) +{ + if (dirty && _autoSync) { + Vehicle* activeVehicle = MultiVehicleManager::instance()->activeVehicle(); + + if (activeVehicle && !activeVehicle->armed()) { + if (_activeVehicle->missionManager()->inProgress()) { + _queuedSend = true; + } else { + _autoSyncSend(); + } + } + } +} + +void MissionController::_autoSyncSend(void) +{ + qDebug() << "Auto-syncing with vehicle"; + _queuedSend = false; + if (_missionItems) { + sendMissionItems(); + _missionItems->setDirty(false); } } -void MissionController::setHomePositionValid(bool homePositionValid) +void MissionController::_inProgressChanged(bool inProgress) { - _homePositionValid = homePositionValid; - qobject_cast(_missionItems->get(0))->setHomePositionValid(homePositionValid); + if (!inProgress && _queuedSend) { + _autoSyncSend(); + } +} - emit homePositionValidChanged(_homePositionValid); +QmlObjectListModel* MissionController::missionItems(void) +{ + return _missionItems; } diff --git a/src/MissionManager/MissionController.h b/src/MissionManager/MissionController.h index 43a96ae5db5f4bbc450b6159e15a68607860d349..fba66c9ae4aa3cd97f1bf0ef7f03d6df5528c8c8 100644 --- a/src/MissionManager/MissionController.h +++ b/src/MissionManager/MissionController.h @@ -29,37 +29,57 @@ This file is part of the QGROUNDCONTROL project #include "QmlObjectListModel.h" #include "Vehicle.h" -/// MissionController is a read only controller for Mission Items class MissionController : public QObject { Q_OBJECT - + public: MissionController(QObject* parent = NULL); ~MissionController(); - Q_PROPERTY(QmlObjectListModel* missionItems READ missionItems NOTIFY missionItemsChanged) - Q_PROPERTY(QmlObjectListModel* waypointLines READ waypointLines NOTIFY waypointLinesChanged) + 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_PROPERTY(bool autoSync READ autoSync WRITE setAutoSync NOTIFY autoSyncChanged) + + Q_INVOKABLE void start(bool editMode) ; + Q_INVOKABLE int addMissionItem(QGeoCoordinate coordinate); + Q_INVOKABLE void getMissionItems(void); + Q_INVOKABLE void sendMissionItems(void); + Q_INVOKABLE void loadMissionFromFile(void); + Q_INVOKABLE void saveMissionToFile(void); + Q_INVOKABLE void removeMissionItem(int index); + Q_INVOKABLE void deleteCurrentMissionItem(void); - /// true: home position should be shown on map, false: home position not shown on map - Q_PROPERTY(bool homePositionValid READ homePositionValid WRITE setHomePositionValid NOTIFY homePositionValidChanged) - // Property accessors - - QmlObjectListModel* missionItems(void) { return _missionItems; } - QmlObjectListModel* waypointLines(void) { return &_waypointLines; } - bool homePositionValid(void) { return _homePositionValid; } - void setHomePositionValid(bool homPositionValid); + QmlObjectListModel* missionItems(void); + QmlObjectListModel* waypointLines(void) { return &_waypointLines; } + bool canEdit(void) { return _canEdit; } + bool liveHomePositionAvailable(void) { return _liveHomePositionAvailable; } + QGeoCoordinate liveHomePosition(void) { return _liveHomePosition; } + bool autoSync(void) { return _autoSync; } + void setAutoSync(bool autoSync); signals: void missionItemsChanged(void); + void canEditChanged(bool canEdit); void waypointLinesChanged(void); - void homePositionValidChanged(bool homePositionValid); - + void liveHomePositionAvailableChanged(bool homePositionAvailable); + void liveHomePositionChanged(const QGeoCoordinate& homePosition); + void autoSyncChanged(bool autoSync); + 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); + void _dirtyChanged(bool dirty); + void _inProgressChanged(bool inProgress); private: void _recalcSequence(void); @@ -67,12 +87,25 @@ private: void _recalcChildItems(void); void _recalcAll(void); void _initAllMissionItems(void); + void _deinitAllMissionItems(void); + void _initMissionItem(MissionItem* item); + void _deinitMissionItem(MissionItem* item); + void _autoSyncSend(void); private: + bool _editMode; 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 _homePositionValid; + bool _liveHomePositionAvailable; + QGeoCoordinate _liveHomePosition; + bool _autoSync; + bool _firstMissionItemSync; + bool _missionItemsRequested; + bool _queuedSend; + + static const char* _settingsGroup; }; #endif diff --git a/src/MissionManager/MissionManager.cc b/src/MissionManager/MissionManager.cc index 364774422757add39bf111ba89fd94105596b5a2..f17b699c018f389b0a8428416beae4cb8dd1cb96 100644 --- a/src/MissionManager/MissionManager.cc +++ b/src/MissionManager/MissionManager.cc @@ -26,6 +26,7 @@ #include "MissionManager.h" #include "Vehicle.h" +#include "FirmwarePlugin.h" #include "MAVLinkProtocol.h" QGC_LOGGING_CATEGORY(MissionManagerLog, "MissionManagerLog") @@ -53,8 +54,10 @@ MissionManager::~MissionManager() } -void MissionManager::writeMissionItems(const QmlObjectListModel& missionItems, bool skipFirstItem) +void MissionManager::writeMissionItems(const QmlObjectListModel& missionItems) { + bool skipFirstItem = !_vehicle->firmwarePlugin()->sendHomePositionToVehicle(); + _retryCount = 0; _missionItems.clear(); @@ -65,10 +68,12 @@ void MissionManager::writeMissionItems(const QmlObjectListModel& missionItems, b MissionItem* item = qobject_cast(_missionItems.get(_missionItems.count() - 1)); - // Editor uses 1-based sequence numbers, adjust them before going out - item->setSequenceNumber(item->sequenceNumber() - 1); - if (item->command() == MavlinkQmlSingleton::MAV_CMD_DO_JUMP) { - item->setParam1((int)item->param1() - 1); + if (skipFirstItem) { + // Home is in sequence 1, remainder of items start at sequence 1 + item->setSequenceNumber(item->sequenceNumber() - 1); + if (item->command() == MavlinkQmlSingleton::MAV_CMD_DO_JUMP) { + item->setParam1((int)item->param1() - 1); + } } } emit newMissionItemsAvailable(); @@ -200,9 +205,9 @@ bool MissionManager::_stopAckTimeout(AckType_t expectedAck) return success; } -void MissionManager::_sendTransactionComplete(void) +void MissionManager::_readTransactionComplete(void) { - qCDebug(MissionManagerLog) << "_sendTransactionComplete read sequence complete"; + qCDebug(MissionManagerLog) << "_readTransactionComplete read sequence complete"; mavlink_message_t message; mavlink_mission_ack_t missionAck; @@ -233,8 +238,7 @@ void MissionManager::_handleMissionCount(const mavlink_message_t& message) qCDebug(MissionManagerLog) << "_handleMissionCount count:" << _cMissionItems; if (_cMissionItems == 0) { - emit newMissionItemsAvailable(); - emit inProgressChanged(false); + _readTransactionComplete(); } else { _requestNextMissionItem(0); } @@ -304,7 +308,7 @@ void MissionManager::_handleMissionItem(const mavlink_message_t& message) int nextSequenceNumber = missionItem.seq + 1; if (nextSequenceNumber == _cMissionItems) { - _sendTransactionComplete(); + _readTransactionComplete(); } else { _requestNextMissionItem(nextSequenceNumber); } diff --git a/src/MissionManager/MissionManager.h b/src/MissionManager/MissionManager.h index 964d72ad8f5fed0d1ed375866fbdba93a56d48ce..ab9c83ae5d0404a1ed0ecd2091e2704287195b27 100644 --- a/src/MissionManager/MissionManager.h +++ b/src/MissionManager/MissionManager.h @@ -63,8 +63,7 @@ public: /// Writes the specified set of mission items to the vehicle /// @oaram missionItems Items to send to vehicle - /// @param skipFirstItem true: Don't send first item - void writeMissionItems(const QmlObjectListModel& missionItems, bool skipFirstItem); + void writeMissionItems(const QmlObjectListModel& missionItems); /// Returns a copy of the current set of mission items. Caller is responsible for /// freeing returned object. @@ -106,7 +105,7 @@ private: void _startAckTimeout(AckType_t ack); bool _stopAckTimeout(AckType_t expectedAck); - void _sendTransactionComplete(void); + void _readTransactionComplete(void); void _handleMissionCount(const mavlink_message_t& message); void _handleMissionItem(const mavlink_message_t& message); void _handleMissionRequest(const mavlink_message_t& message); @@ -137,4 +136,4 @@ private: QmlObjectListModel _missionItems; }; -#endif \ No newline at end of file +#endif diff --git a/src/MissionManager/MissionManagerTest.cc b/src/MissionManager/MissionManagerTest.cc index 017cce4d18725e179a9b9d2b9b4d36bd7045b20e..5605dfb04b94a7d91f3b03292d9c748873b0d0ca 100644 --- a/src/MissionManager/MissionManagerTest.cc +++ b/src/MissionManager/MissionManagerTest.cc @@ -35,8 +35,9 @@ const MissionManagerTest::TestCase_t MissionManagerTest::_rgTestCases[] = { { "4\t0\t3\t21\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", { 4, 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 } }, { "5\t0\t3\t22\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", { 5, 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 } }, { "6\t0\t2\t112\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", { 6, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_CONDITION_DELAY, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_MISSION } }, - { "7\t0\t2\t177\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", { 7, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_DO_JUMP, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_MISSION } }, + { "7\t0\t2\t177\t3\t20\t30\t40\t-10\t-20\t-30\t1\r\n", { 7, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_DO_JUMP, 3, 20.0, 30.0, 40.0, true, false, MAV_FRAME_MISSION } }, }; +const size_t MissionManagerTest::_cTestCases = sizeof(_rgTestCases)/sizeof(_rgTestCases[0]); MissionManagerTest::MissionManagerTest(void) : _mockLink(NULL) @@ -44,7 +45,7 @@ MissionManagerTest::MissionManagerTest(void) } -void MissionManagerTest::init(void) +void MissionManagerTest::_initForFirmwareType(MAV_AUTOPILOT firmwareType) { UnitTest::init(); @@ -53,6 +54,7 @@ void MissionManagerTest::init(void) _mockLink = new MockLink(); Q_CHECK_PTR(_mockLink); + _mockLink->setFirmwareType(firmwareType); LinkManager::instance()->_addLink(_mockLink); linkMgr->connectLink(_mockLink); @@ -110,7 +112,7 @@ void MissionManagerTest::_checkInProgressValues(bool inProgress) QCOMPARE(signalArgs[0].toBool(), inProgress); } -void MissionManagerTest::_readEmptyVehicle(void) +void MissionManagerTest::_readEmptyVehicleWorker(void) { _missionManager->requestMissionItems(); @@ -125,8 +127,7 @@ void MissionManagerTest::_readEmptyVehicle(void) // inProgressChanged signal to signal completion. _multiSpy->waitForSignalByIndex(newMissionItemsAvailableSignalIndex, _signalWaitTime); _multiSpy->waitForSignalByIndex(inProgressChangedSignalIndex, _signalWaitTime); - QCOMPARE(_multiSpy->checkSignalByMask(newMissionItemsAvailableSignalMask | inProgressChangedSignalMask), true); - QCOMPARE(_multiSpy->checkNoSignalByMask(canEditChangedSignalMask), true); + QCOMPARE(_multiSpy->checkOnlySignalByMask(newMissionItemsAvailableSignalMask | inProgressChangedSignalMask), true); _checkInProgressValues(false); // Vehicle should have no items at this point @@ -143,7 +144,6 @@ void MissionManagerTest::_writeItems(MockLinkMissionItemHandler::FailureMode_t f } // Setup our test case data - const size_t cTestCases = sizeof(_rgTestCases)/sizeof(_rgTestCases[0]); QmlObjectListModel* list = new QmlObjectListModel(); // Editor has a home position item on the front, so we do the same @@ -156,7 +156,7 @@ void MissionManagerTest::_writeItems(MockLinkMissionItemHandler::FailureMode_t f homeItem->setSequenceNumber(0); list->insert(0, homeItem); - for (size_t i=0; iwriteMissionItems(*list, true /* skipFirstItem */); + _missionManager->writeMissionItems(*list); // writeMissionItems should emit these signals before returning: // inProgressChanged @@ -197,8 +197,15 @@ void MissionManagerTest::_writeItems(MockLinkMissionItemHandler::FailureMode_t f // Validate inProgressChanged signal value _checkInProgressValues(false); - // We should have gotten back all mission items - QCOMPARE(_missionManager->missionItems()->count(), (int)cTestCases); + // Validate item count in mission manager + + int expectedCount = (int)_cTestCases; + if (_mockLink->getFirmwareType() == MAV_AUTOPILOT_ARDUPILOTMEGA) { + // Home position at position 0 comes from vehicle + expectedCount++; + } + + QCOMPARE(_missionManager->missionItems()->count(), expectedCount); } else { // This should be a failed run @@ -260,6 +267,7 @@ void MissionManagerTest::_roundTripItems(MockLinkMissionItemHandler::FailureMode QCOMPARE(_multiSpy->checkSignalByMask(newMissionItemsAvailableSignalMask | inProgressChangedSignalMask), true); QCOMPARE(_multiSpy->checkNoSignalByMask(canEditChangedSignalMask), true); _checkInProgressValues(false); + } else { // This should be a failed run @@ -292,7 +300,11 @@ void MissionManagerTest::_roundTripItems(MockLinkMissionItemHandler::FailureMode size_t cMissionItemsExpected; if (failureMode == MockLinkMissionItemHandler::FailNone || failFirstTimeOnly == true) { - cMissionItemsExpected = sizeof(_rgTestCases)/sizeof(_rgTestCases[0]); + cMissionItemsExpected = (int)_cTestCases; + if (_mockLink->getFirmwareType() == MAV_AUTOPILOT_ARDUPILOTMEGA) { + // Home position at position 0 comes from vehicle + cMissionItemsExpected++; + } } else { switch (failureMode) { case MockLinkMissionItemHandler::FailReadRequestListNoResponse: @@ -315,28 +327,49 @@ void MissionManagerTest::_roundTripItems(MockLinkMissionItemHandler::FailureMode QCOMPARE(_missionManager->missionItems()->count(), (int)cMissionItemsExpected); QCOMPARE(_missionManager->canEdit(), true); - - for (size_t i=0; i(_missionManager->missionItems()->get(i)); + + size_t firstActualItem = 0; + if (_mockLink->getFirmwareType() == MAV_AUTOPILOT_ARDUPILOTMEGA) { + // First item is home position, don't validate it + firstActualItem++; + } + + int testCaseIndex = 0; + for (size_t actualItemIndex=firstActualItem; actualItemIndexexpectedItem.sequenceNumber; + int expectedParam1 = (int)testCase->expectedItem.param1; + if (_mockLink->getFirmwareType() == MAV_AUTOPILOT_ARDUPILOTMEGA) { + // Account for home position in first item + expectedSequenceNumber++; + if (testCase->expectedItem.command == MAV_CMD_DO_JUMP) { + // Expected data in test case is for PX4 + expectedParam1++; + } + } + + MissionItem* actual = qobject_cast(_missionManager->missionItems()->get(actualItemIndex)); - qDebug() << "Test case" << i; - QCOMPARE(actual->sequenceNumber(), testCase->expectedItem.sequenceNumber); + qDebug() << "Test case" << testCaseIndex; + QCOMPARE(actual->sequenceNumber(), expectedSequenceNumber); 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((int)actual->param1(), (int)expectedParam1); 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); - QCOMPARE(actual->param1(), testCase->expectedItem.param1); + + testCaseIndex++; } } -void MissionManagerTest::_testWriteFailureHandling(void) +void MissionManagerTest::_testWriteFailureHandlingWorker(void) { /* /// Called to send a MISSION_ACK message while the MissionManager is in idle state @@ -378,7 +411,7 @@ void MissionManagerTest::_testWriteFailureHandling(void) } } -void MissionManagerTest::_testReadFailureHandling(void) +void MissionManagerTest::_testReadFailureHandlingWorker(void) { /* /// Called to send a MISSION_ACK message while the MissionManager is in idle state @@ -417,3 +450,40 @@ void MissionManagerTest::_testReadFailureHandling(void) _mockLink->resetMissionItemHandler(); } } + +void MissionManagerTest::_testEmptyVehicleAPM(void) +{ + _initForFirmwareType(MAV_AUTOPILOT_ARDUPILOTMEGA); + _readEmptyVehicleWorker(); +} + +void MissionManagerTest::_testEmptyVehiclePX4(void) +{ + _initForFirmwareType(MAV_AUTOPILOT_ARDUPILOTMEGA); + _readEmptyVehicleWorker(); +} + +void MissionManagerTest::_testWriteFailureHandlingAPM(void) +{ + _initForFirmwareType(MAV_AUTOPILOT_ARDUPILOTMEGA); + _testWriteFailureHandlingWorker(); +} + +void MissionManagerTest::_testReadFailureHandlingAPM(void) +{ + _initForFirmwareType(MAV_AUTOPILOT_ARDUPILOTMEGA); + _testReadFailureHandlingWorker(); +} + + +void MissionManagerTest::_testWriteFailureHandlingPX4(void) +{ + _initForFirmwareType(MAV_AUTOPILOT_PX4); + _testWriteFailureHandlingWorker(); +} + +void MissionManagerTest::_testReadFailureHandlingPX4(void) +{ + _initForFirmwareType(MAV_AUTOPILOT_PX4); + _testReadFailureHandlingWorker(); +} diff --git a/src/MissionManager/MissionManagerTest.h b/src/MissionManager/MissionManagerTest.h index 432afd92c09d07c021efcf806f5aaee18e8de4e4..3fb7c3d818edbcba342850656c9a51821e422896 100644 --- a/src/MissionManager/MissionManagerTest.h +++ b/src/MissionManager/MissionManagerTest.h @@ -39,18 +39,23 @@ public: MissionManagerTest(void); private slots: - void init(void); void cleanup(void); - void _testWriteFailureHandling(void); - void _testReadFailureHandling(void); - + void _testEmptyVehicleAPM(void); + void _testEmptyVehiclePX4(void); + void _testWriteFailureHandlingAPM(void); + void _testReadFailureHandlingAPM(void); + void _testWriteFailureHandlingPX4(void); + void _testReadFailureHandlingPX4(void); + private: + void _initForFirmwareType(MAV_AUTOPILOT firmwareType); void _checkInProgressValues(bool inProgress); void _roundTripItems(MockLinkMissionItemHandler::FailureMode_t failureMode, MissionManager::ErrorCode_t errorCode, bool failFirstTimeOnly); void _writeItems(MockLinkMissionItemHandler::FailureMode_t failureMode, MissionManager::ErrorCode_t errorCode, bool failFirstTimeOnly); - - void _readEmptyVehicle(void); + void _testWriteFailureHandlingWorker(void); + void _testReadFailureHandlingWorker(void); + void _readEmptyVehicleWorker(void); MockLink* _mockLink; MissionManager* _missionManager; @@ -93,6 +98,7 @@ private: } TestCase_t; static const TestCase_t _rgTestCases[]; + static const size_t _cTestCases; static const int _signalWaitTime = MissionManager::_ackTimeoutMilliseconds * MissionManager::_maxRetryCount * 2; }; diff --git a/src/QGCApplication.cc b/src/QGCApplication.cc index 51baaad31955d615e8580f71419af238802f7abd..3f34623d6b2a43bd0381dc4412d87c4089bb02e3 100644 --- a/src/QGCApplication.cc +++ b/src/QGCApplication.cc @@ -89,7 +89,6 @@ #include "CoordinateVector.h" #include "MainToolBarController.h" #include "MissionController.h" -#include "MissionEditorController.h" #include "FlightDisplayViewController.h" #include "VideoSurface.h" #include "VideoReceiver.h" @@ -355,7 +354,6 @@ void QGCApplication::_initCommon(void) qmlRegisterType ("QGroundControl.Controllers", 1, 0, "ScreenToolsController"); qmlRegisterType ("QGroundControl.Controllers", 1, 0, "MainToolBarController"); qmlRegisterType ("QGroundControl.Controllers", 1, 0, "MissionController"); - qmlRegisterType ("QGroundControl.Controllers", 1, 0, "MissionEditorController"); qmlRegisterType ("QGroundControl.Controllers", 1, 0, "FlightDisplayViewController"); #ifndef __mobile__ diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 7ea27f37e8cbd509be78fe29effa5bf8e12fd9c8..b1c4e824e133c774e4fdeae93ab1e26316fe4f6b 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -166,27 +166,9 @@ Vehicle::~Vehicle() { delete _missionManager; _missionManager = NULL; - - // Stop listening for system messages - disconnect(UASMessageHandler::instance(), &UASMessageHandler::textMessageCountChanged, this, &Vehicle::_handleTextMessage); - // Disconnect any previously connected active MAV - disconnect(_mav, SIGNAL(attitudeChanged (UASInterface*, double,double,double,quint64)), this, SLOT(_updateAttitude(UASInterface*, double, double, double, quint64))); - disconnect(_mav, SIGNAL(attitudeChanged (UASInterface*, int,double,double,double,quint64)), this, SLOT(_updateAttitude(UASInterface*,int,double, double, double, quint64))); - disconnect(_mav, SIGNAL(speedChanged (UASInterface*, double, double, quint64)), this, SLOT(_updateSpeed(UASInterface*, double, double, quint64))); - disconnect(_mav, SIGNAL(altitudeChanged (UASInterface*, double, double, double, double, quint64)), this, SLOT(_updateAltitude(UASInterface*, double, double, double, double, quint64))); - disconnect(_mav, SIGNAL(navigationControllerErrorsChanged (UASInterface*, double, double, double)), this, SLOT(_updateNavigationControllerErrors(UASInterface*, double, double, double))); - disconnect(_mav, SIGNAL(statusChanged (UASInterface*,QString,QString)), this, SLOT(_updateState(UASInterface*,QString,QString))); - disconnect(_mav, &UASInterface::NavigationControllerDataChanged, this, &Vehicle::_updateNavigationControllerData); - disconnect(_mav, &UASInterface::heartbeatTimeout, this, &Vehicle::_heartbeatTimeout); - disconnect(_mav, &UASInterface::batteryChanged, this, &Vehicle::_updateBatteryRemaining); - disconnect(_mav, &UASInterface::batteryConsumedChanged, this, &Vehicle::_updateBatteryConsumedChanged); - disconnect(_mav, &UASInterface::nameChanged, this, &Vehicle::_updateName); - disconnect(_mav, &UASInterface::systemTypeSet, this, &Vehicle::_setSystemType); - disconnect(_mav, &UASInterface::localizationChanged, this, &Vehicle::_setSatLoc); - UAS* pUas = dynamic_cast(_mav); - if(pUas) { - disconnect(pUas, &UAS::satelliteCountChanged, this, &Vehicle::_setSatelliteCount); - } + + delete _mav; + _mav = NULL; } void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message) diff --git a/src/VehicleSetup/SetupViewTest.cc b/src/VehicleSetup/SetupViewTest.cc index e430316448f9476760bf0e89580a98e1def985cc..3891dc4e6013fd64456ffd963b2740faec711c13 100644 --- a/src/VehicleSetup/SetupViewTest.cc +++ b/src/VehicleSetup/SetupViewTest.cc @@ -60,7 +60,7 @@ void SetupViewTest::_clickThrough_test(void) MockLink* link = new MockLink(); Q_CHECK_PTR(link); - link->setAutopilotType(MAV_AUTOPILOT_PX4); + link->setFirmwareType(MAV_AUTOPILOT_PX4); LinkManager::instance()->_addLink(link); linkMgr->connectLink(link); diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc index 4d3a1259efac58805282ce2db21b6ed412519a0f..dd8e708b2d5c38e743c20225795c4a536510f771 100644 --- a/src/comm/MockLink.cc +++ b/src/comm/MockLink.cc @@ -84,13 +84,14 @@ MockLink::MockLink(MockConfiguration* config) , _mavlinkStarted(false) , _mavBaseMode(MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) , _mavState(MAV_STATE_STANDBY) - , _autopilotType(MAV_AUTOPILOT_PX4) + , _firmwareType(MAV_AUTOPILOT_PX4) , _fileServer(NULL) , _sendStatusText(false) + , _apmSendHomePositionOnEmptyList(false) { _config = config; if (_config) { - _autopilotType = config->firmwareType(); + _firmwareType = config->firmwareType(); _sendStatusText = config->sendStatusText(); } @@ -250,7 +251,7 @@ void MockLink::_sendHeartBeat(void) _vehicleComponentId, &msg, MAV_TYPE_QUADROTOR, // MAV_TYPE - _autopilotType, // MAV_AUTOPILOT + _firmwareType, // MAV_AUTOPILOT _mavBaseMode, // MAV_MODE _mavCustomMode, // custom mode _mavState); // MAV_STATE @@ -436,7 +437,7 @@ float MockLink::_floatUnionForParam(int componentId, const QString& paramName) switch (paramType) { case MAV_PARAM_TYPE_INT8: - if (_autopilotType == MAV_AUTOPILOT_ARDUPILOTMEGA) { + if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) { valueUnion.param_float = (unsigned char)paramVar.toChar().toLatin1(); } else { valueUnion.param_int8 = (unsigned char)paramVar.toChar().toLatin1(); @@ -444,7 +445,7 @@ float MockLink::_floatUnionForParam(int componentId, const QString& paramName) break; case MAV_PARAM_TYPE_INT32: - if (_autopilotType == MAV_AUTOPILOT_ARDUPILOTMEGA) { + if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) { valueUnion.param_float = paramVar.toInt(); } else { valueUnion.param_int32 = paramVar.toInt(); @@ -452,7 +453,7 @@ float MockLink::_floatUnionForParam(int componentId, const QString& paramName) break; case MAV_PARAM_TYPE_UINT32: - if (_autopilotType == MAV_AUTOPILOT_ARDUPILOTMEGA) { + if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) { valueUnion.param_float = paramVar.toUInt(); } else { valueUnion.param_uint32 = paramVar.toUInt(); diff --git a/src/comm/MockLink.h b/src/comm/MockLink.h index 1ee7b95887ecf27f7da1a5693c2415e7e232faa9..ae043f92eeac4483b9cbc3448fb9124a03ebbe66 100644 --- a/src/comm/MockLink.h +++ b/src/comm/MockLink.h @@ -74,9 +74,16 @@ public: // MockLink methods int vehicleId(void) { return _vehicleSystemId; } - MAV_AUTOPILOT getAutopilotType(void) { return _autopilotType; } - void setAutopilotType(MAV_AUTOPILOT autopilot) { _autopilotType = autopilot; } + MAV_AUTOPILOT getFirmwareType(void) { return _firmwareType; } + void setFirmwareType(MAV_AUTOPILOT autopilot) { _firmwareType = autopilot; } void setSendStatusText(bool sendStatusText) { _sendStatusText = sendStatusText; } + + /// APM stack has strange handling of the first item of the mission list. If it has no + /// onboard mission items, sometimes it sends back a home position in position 0 and + /// sometimes it doesn't. Don't ask. This option allows you to configure that behavior + /// for unit testing. + void setAPMMissionResponseMode(bool sendHomePositionOnEmptyList) { _apmSendHomePositionOnEmptyList = sendHomePositionOnEmptyList; } + void emitRemoteControlChannelRawChanged(int channel, uint16_t raw); /// Sends the specified mavlink message to QGC @@ -177,11 +184,12 @@ private: uint8_t _mavState; MockConfiguration* _config; - MAV_AUTOPILOT _autopilotType; + MAV_AUTOPILOT _firmwareType; MockLinkFileServer* _fileServer; bool _sendStatusText; + bool _apmSendHomePositionOnEmptyList; static float _vehicleLatitude; static float _vehicleLongitude; diff --git a/src/comm/MockLinkMissionItemHandler.cc b/src/comm/MockLinkMissionItemHandler.cc index e33cb6a8768fb80fd2465c8de628ab1c0d3d98b6..13d5ca0fd5dc32132ff097e4cbc55ce35130d1f0 100644 --- a/src/comm/MockLinkMissionItemHandler.cc +++ b/src/comm/MockLinkMissionItemHandler.cc @@ -32,6 +32,7 @@ MockLinkMissionItemHandler::MockLinkMissionItemHandler(MockLink* mockLink) : _mockLink(mockLink) , _missionItemResponseTimer(NULL) , _failureMode(FailNone) + , _sendHomePositionOnEmptyList(false) { Q_ASSERT(mockLink); } @@ -99,6 +100,11 @@ void MockLinkMissionItemHandler::_handleMissionRequestList(const mavlink_message mavlink_msg_mission_request_list_decode(&msg, &request); Q_ASSERT(request.target_system == _mockLink->vehicleId()); + + int itemCount = _missionItems.count(); + if (itemCount == 0 && _sendHomePositionOnEmptyList) { + itemCount = 1; + } mavlink_message_t responseMsg; @@ -107,7 +113,7 @@ void MockLinkMissionItemHandler::_handleMissionRequestList(const mavlink_message &responseMsg, // Outgoing message msg.sysid, // Target is original sender msg.compid, // Target is original sender - _missionItems.count()); // Number of mission items + itemCount); // Number of mission items _mockLink->respondWithMavlinkMessage(responseMsg); } else { qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequestList not responding due to failure mode"; @@ -148,7 +154,16 @@ void MockLinkMissionItemHandler::_handleMissionRequest(const mavlink_message_t& } else { mavlink_message_t responseMsg; - mavlink_mission_item_t item = _missionItems[request.seq]; + mavlink_mission_item_t item; + if (_missionItems.count() == 0 && _sendHomePositionOnEmptyList) { + item.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; + item.command = MAV_CMD_NAV_WAYPOINT; + item.current = false; + item.autocontinue = true; + item.param1 = item.param2 = item.param3 = item.param4 = item.x = item.y = item.z = 0; + } else { + item = _missionItems[request.seq]; + } mavlink_msg_mission_item_pack(_mockLink->vehicleId(), MAV_COMP_ID_MISSIONPLANNER, diff --git a/src/comm/MockLinkMissionItemHandler.h b/src/comm/MockLinkMissionItemHandler.h index 57e3d2948a11f4c50f740b3143fe310f9762a45f..1405e503776e4a032c752bd6a0e1a82529ef5846 100644 --- a/src/comm/MockLinkMissionItemHandler.h +++ b/src/comm/MockLinkMissionItemHandler.h @@ -88,6 +88,8 @@ public: /// Reset the state of the MissionItemHandler to no items, no transactions in progress. void reset(void) { _missionItems.clear(); } + void setSendHomePositionOnEmptyList(bool sendHomePositionOnEmptyList) { _sendHomePositionOnEmptyList = sendHomePositionOnEmptyList; } + private slots: void _missionItemResponseTimeout(void); @@ -112,6 +114,7 @@ private: QTimer* _missionItemResponseTimer; FailureMode_t _failureMode; bool _failureFirstTimeOnly; + bool _sendHomePositionOnEmptyList; }; #endif diff --git a/src/qgcunittest/MainWindowTest.cc b/src/qgcunittest/MainWindowTest.cc index 6288292bc75cf7fa39e27c5ab8b1780f2cfb100c..95d0b0da34329be31350f7230d9f5a7860b45f1a 100644 --- a/src/qgcunittest/MainWindowTest.cc +++ b/src/qgcunittest/MainWindowTest.cc @@ -63,7 +63,7 @@ void MainWindowTest::_connectWindowClose_test(MAV_AUTOPILOT autopilot) MockLink* link = new MockLink(); Q_CHECK_PTR(link); - link->setAutopilotType(autopilot); + link->setFirmwareType(autopilot); LinkManager::instance()->_addLink(link); linkMgr->connectLink(link);