From d499b513ff0e01e4af3fdcf410b424ac8ad28a6a Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 22 Nov 2015 11:14:59 -0800 Subject: [PATCH] Mission raw/friendly edit support --- qgcresources.qrc | 2 + qgroundcontrol.pro | 12 +- qgroundcontrol.qrc | 10 +- src/MissionEditor/MissionEditor.qml | 59 +- src/MissionItem.cc | 929 ------------------ src/MissionItem.h | 320 ------ src/MissionManager/CogWheel.svg | 84 ++ src/MissionManager/MavCmdInfo.json | 200 ++++ src/MissionManager/MissionController.cc | 33 +- src/MissionManager/MissionController.h | 4 - .../MissionControllerManagerTest.cc | 2 - .../MissionControllerManagerTest.h | 24 +- src/MissionManager/MissionItem.cc | 916 +++++++++++++++++ src/MissionManager/MissionItem.h | 294 ++++++ src/{ => MissionManager}/MissionItemTest.cc | 25 +- src/{ => MissionManager}/MissionItemTest.h | 4 +- src/MissionManager/MissionManager.cc | 18 +- src/MissionManager/MissionManager.h | 5 - src/MissionManager/MissionManagerTest.cc | 7 +- src/QmlControls/MissionItemEditor.qml | 59 +- src/QmlControls/MissionItemSummary.qml | 73 -- src/QmlControls/ParameterEditorDialog.qml | 4 +- src/QmlControls/QGCView.qml | 17 +- .../QGroundControl.Controls.qmldir | 1 - 24 files changed, 1629 insertions(+), 1473 deletions(-) delete mode 100644 src/MissionItem.cc delete mode 100644 src/MissionItem.h create mode 100644 src/MissionManager/CogWheel.svg create mode 100644 src/MissionManager/MavCmdInfo.json create mode 100644 src/MissionManager/MissionItem.cc create mode 100644 src/MissionManager/MissionItem.h rename src/{ => MissionManager}/MissionItemTest.cc (92%) rename src/{ => MissionManager}/MissionItemTest.h (97%) delete mode 100644 src/QmlControls/MissionItemSummary.qml diff --git a/qgcresources.qrc b/qgcresources.qrc index df0a8531c..2a45d0053 100644 --- a/qgcresources.qrc +++ b/qgcresources.qrc @@ -95,6 +95,8 @@ src/ui/toolbar/Images/Signal100.svg src/ui/toolbar/Images/Yield.svg + src/MissionManager/CogWheel.svg + diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index b3ba1acf4..99ed88366 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -247,8 +247,9 @@ HEADERS += \ src/Joystick/JoystickManager.h \ src/LogCompressor.h \ src/MG.h \ - src/MissionManager/MissionManager.h \ src/MissionManager/MissionController.h \ + src/MissionManager/MissionItem.h \ + src/MissionManager/MissionManager.h \ src/QGC.h \ src/QGCApplication.h \ src/QGCComboBox.h \ @@ -291,7 +292,6 @@ HEADERS += \ src/ui/toolbar/MainToolBarController.h \ src/ui/uas/QGCUnconnectedInfoWidget.h \ src/ui/uas/UASMessageView.h \ - src/MissionItem.h \ src/AutoPilotPlugins/PX4/PX4AirframeLoader.h \ src/QmlControls/QGCImageProvider.h \ @@ -363,8 +363,9 @@ SOURCES += \ src/Joystick/JoystickManager.cc \ src/LogCompressor.cc \ src/main.cc \ - src/MissionManager/MissionManager.cc \ src/MissionManager/MissionController.cc \ + src/MissionManager/MissionItem.cc \ + src/MissionManager/MissionManager.cc \ src/QGC.cc \ src/QGCApplication.cc \ src/QGCComboBox.cc \ @@ -402,7 +403,6 @@ SOURCES += \ src/ui/toolbar/MainToolBarController.cc \ src/ui/uas/QGCUnconnectedInfoWidget.cc \ src/ui/uas/UASMessageView.cc \ - src/MissionItem.cc \ src/AutoPilotPlugins/PX4/PX4AirframeLoader.cc \ src/QmlControls/QGCImageProvider.cc \ @@ -471,9 +471,9 @@ HEADERS += \ src/FactSystem/FactSystemTestBase.h \ src/FactSystem/FactSystemTestGeneric.h \ src/FactSystem/FactSystemTestPX4.h \ - src/MissionItemTest.h \ src/MissionManager/MissionControllerTest.h \ src/MissionManager/MissionControllerManagerTest.h \ + src/MissionManager/MissionItemTest.h \ src/MissionManager/MissionManagerTest.h \ src/qgcunittest/GeoTest.h \ src/qgcunittest/FileDialogTest.h \ @@ -494,9 +494,9 @@ SOURCES += \ src/FactSystem/FactSystemTestBase.cc \ src/FactSystem/FactSystemTestGeneric.cc \ src/FactSystem/FactSystemTestPX4.cc \ - src/MissionItemTest.cc \ src/MissionManager/MissionControllerTest.cc \ src/MissionManager/MissionControllerManagerTest.cc \ + src/MissionManager/MissionItemTest.cc \ src/MissionManager/MissionManagerTest.cc \ src/qgcunittest/GeoTest.cc \ src/qgcunittest/FileDialogTest.cc \ diff --git a/qgroundcontrol.qrc b/qgroundcontrol.qrc index 176c47811..124ad54ae 100644 --- a/qgroundcontrol.qrc +++ b/qgroundcontrol.qrc @@ -28,7 +28,6 @@ src/AutoPilotPlugins/PX4/PowerComponent.qml src/AutoPilotPlugins/PX4/PowerComponentSummary.qml src/VehicleSetup/PX4FlowSensor.qml - src/QmlControls/ClickableColor.qml src/QmlControls/DropButton.qml src/QmlControls/ExclusiveGroupItem.qml @@ -37,7 +36,6 @@ src/ui/toolbar/MainToolBarIndicators.qml src/QmlControls/MissionItemEditor.qml src/QmlControls/MissionItemIndexLabel.qml - src/QmlControls/MissionItemSummary.qml src/QmlControls/ModeSwitchDisplay.qml src/QmlControls/ParameterEditor.qml src/QmlControls/ParameterEditorDialog.qml @@ -62,20 +60,17 @@ src/QmlControls/VehicleRotationCal.qml src/QmlControls/VehicleSummaryRow.qml src/ViewWidgets/ViewWidget.qml - src/FactSystem/FactControls/FactCheckBox.qml src/FactSystem/FactControls/FactComboBox.qml src/FactSystem/FactControls/FactLabel.qml src/FactSystem/FactControls/FactPanel.qml src/FactSystem/FactControls/FactTextField.qml src/FactSystem/FactControls/qmldir - src/FlightDisplay/FlightDisplayView.qml src/FlightDisplay/FlightDisplayViewMap.qml src/FlightDisplay/FlightDisplayViewVideo.qml src/FlightDisplay/FlightDisplayViewWidgets.qml src/FlightDisplay/qmldir - src/FlightMap/FlightMap.qml src/FlightMap/MapItems/MissionItemIndicator.qml src/FlightMap/MapItems/MissionItemView.qml @@ -90,10 +85,8 @@ src/FlightMap/QGCVideoBackground.qml src/FlightMap/qmldir src/FlightMap/MapItems/VehicleMapItem.qml - src/QmlControls/QGroundControl.ScreenTools.qmldir src/QmlControls/ScreenTools.qml - src/QmlControls/QmlTest.qml src/AutoPilotPlugins/PX4/RadioComponent.qml src/AutoPilotPlugins/PX4/RadioComponentSummary.qml @@ -107,4 +100,7 @@ src/test.qml src/VehicleSetup/VehicleSummary.qml + + src/MissionManager/MavCmdInfo.json + diff --git a/src/MissionEditor/MissionEditor.qml b/src/MissionEditor/MissionEditor.qml index 533965d3a..d92ac62d0 100644 --- a/src/MissionEditor/MissionEditor.qml +++ b/src/MissionEditor/MissionEditor.qml @@ -40,7 +40,8 @@ import QGroundControl.Controllers 1.0 QGCView { id: _root - viewPanel: panel + viewPanel: panel + topDialogMargin: height - mainWindow.availableHeight // zOrder comes from the Loader in MainWindow.qml z: QGroundControl.zOrderTopMost @@ -211,7 +212,6 @@ QGCView { property var missionItem property var missionItemIndicator - property real heading: missionItem ? missionItem.heading : 0 readonly property real _radius: ScreenTools.defaultFontPixelHeight * 4 readonly property real _arrowHeight: ScreenTools.defaultFontPixelHeight @@ -328,30 +328,34 @@ QGCView { onClicked: setCurrentItem(object.sequenceNumber) - Connections { - target: object - - onIsCurrentItemChanged: { - if (object.isCurrentItem) { - _root.showDistance(object) - if (object.specifiesCoordinate) { - // Setup our drag item - if (object.sequenceNumber != 0) { - itemDragger.visible = true - itemDragger.missionItem = Qt.binding(function() { return object }) - itemDragger.missionItemIndicator = Qt.binding(function() { return itemIndicator }) - } else { - itemDragger.clearItem() - } - - // Move to the new position - editorMap.latitude = object.coordinate.latitude - editorMap.longitude = object.coordinate.longitude + function updateItemIndicator() + { + if (object.isCurrentItem) { + _root.showDistance(object) + if (object.specifiesCoordinate) { + // Setup our drag item + if (object.sequenceNumber != 0) { + itemDragger.visible = true + itemDragger.missionItem = Qt.binding(function() { return object }) + itemDragger.missionItemIndicator = Qt.binding(function() { return itemIndicator }) + } else { + itemDragger.clearItem() } + + // Move to the new position + editorMap.latitude = object.coordinate.latitude + editorMap.longitude = object.coordinate.longitude } } } + Connections { + target: object + + onIsCurrentItemChanged: updateItemIndicator() + onCommandChanged: updateItemIndicator() + } + // These are the non-coordinate child mission items attached to this item Row { anchors.top: parent.top @@ -380,7 +384,7 @@ QGCView { // Mission Item Editor Item { id: missionItemEditor - height: mainWindow.avaiableHeight + height: mainWindow.availableHeight anchors.bottom: parent.bottom anchors.right: parent.right width: _rightPanelWidth @@ -389,11 +393,10 @@ QGCView { z: QGroundControl.zOrderTopMost ListView { - id: missionItemSummaryList anchors.fill: parent spacing: _margin / 2 orientation: ListView.Vertical - model: controller.canEdit ? controller.missionItems : 0 + model: controller.missionItems property real _maxItemHeight: 0 @@ -416,14 +419,6 @@ QGCView { } } } // ListView - - QGCLabel { - anchors.fill: parent - visible: !controller.canEdit - wrapMode: Text.WordWrap - text: "The set of mission items you have loaded cannot be edited by QGroundControl. " + - "You will only be able to save these to a file, or send them to a vehicle." - } } // Item - Mission Item editor /* diff --git a/src/MissionItem.cc b/src/MissionItem.cc deleted file mode 100644 index a7c053080..000000000 --- a/src/MissionItem.cc +++ /dev/null @@ -1,929 +0,0 @@ -/*=================================================================== -QGroundControl Open Source Ground Control Station - -(c) 2009, 2010 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 -#include - -#include "MissionItem.h" - -QGC_LOGGING_CATEGORY(MissionItemLog, "MissionItemLog") - -const double MissionItem::defaultPitch = 15.0; -const double MissionItem::defaultHeading = 0.0; -const double MissionItem::defaultAltitude = 25.0; -const double MissionItem::defaultAcceptanceRadius = 3.0; -const double MissionItem::defaultLoiterOrbitRadius = 10.0; -const double MissionItem::defaultLoiterTurns = 1.0; - -QDebug operator<<(QDebug dbg, const MissionItem& missionItem) -{ - QDebugStateSaver saver(dbg); - dbg.nospace() << "MissionItem(" << missionItem.coordinate() << ")"; - - return dbg; -} - -QDebug operator<<(QDebug dbg, const MissionItem* missionItem) -{ - QDebugStateSaver saver(dbg); - dbg.nospace() << "MissionItem(" << missionItem->coordinate() << ")"; - - return dbg; -} - -const MissionItem::MavCmd2Name_t MissionItem::_rgMavCmd2Name[_cMavCmd2Name] = { - { MAV_CMD_NAV_WAYPOINT, "Waypoint" }, - { MAV_CMD_NAV_LOITER_UNLIM, "Loiter" }, - { MAV_CMD_NAV_LOITER_TURNS, "Loiter (turns)" }, - { MAV_CMD_NAV_LOITER_TIME, "Loiter (seconds)" }, - { MAV_CMD_NAV_RETURN_TO_LAUNCH, "Return Home" }, - { MAV_CMD_NAV_LAND, "Land" }, - { MAV_CMD_NAV_TAKEOFF, "Takeoff" }, - { MAV_CMD_CONDITION_DELAY, "Delay" }, - { MAV_CMD_DO_JUMP, "Jump To Command" }, -}; - -MissionItem::MissionItem(QObject* parent, - int sequenceNumber, - QGeoCoordinate coordinate, - int command, - double param1, - double param2, - double param3, - double param4, - bool autocontinue, - bool isCurrentItem, - int frame) - : QObject(parent) - , _sequenceNumber(sequenceNumber) - , _frame(-1) // Forces set of _altitudeRelativeToHomeFact - , _command((MavlinkQmlSingleton::Qml_MAV_CMD)command) - , _autocontinue(autocontinue) - , _isCurrentItem(isCurrentItem) - , _reachedTime(0) - , _distance(0.0) - , _headingDegreesFact(NULL) - , _dirty(false) - , _homePositionSpecialCase(false) - , _homePositionValid(false) -{ - _latitudeFact = new Fact(0, "Latitude:", FactMetaData::valueTypeDouble, this); - _longitudeFact = new Fact(0, "Longitude:", FactMetaData::valueTypeDouble, this); - _altitudeFact = new Fact(0, "Altitude:", FactMetaData::valueTypeDouble, this); - _headingDegreesFact = new Fact(0, "Heading:", FactMetaData::valueTypeDouble, this); - _loiterOrbitRadiusFact = new Fact(0, "Radius:", FactMetaData::valueTypeDouble, this); - _param1Fact = new Fact(0, QString(), FactMetaData::valueTypeDouble, this); - _param2Fact = new Fact(0, QString(), FactMetaData::valueTypeDouble, this); - _altitudeRelativeToHomeFact = new Fact(0, "Altitude is relative to home", FactMetaData::valueTypeDouble, this); - - setFrame(frame); - - setCoordinate(coordinate); - setParam1(param1); - setParam2(param2); - _setYawRadians(param4); - setLoiterOrbitRadius(param3); - - // FIXME: Need to fill out more meta data - - FactMetaData* latitudeMetaData = new FactMetaData(FactMetaData::valueTypeDouble, _latitudeFact); - latitudeMetaData->setUnits("deg"); - latitudeMetaData->setDecimalPlaces(7); - - FactMetaData* longitudeMetaData = new FactMetaData(FactMetaData::valueTypeDouble, _longitudeFact); - longitudeMetaData->setUnits("deg"); - longitudeMetaData->setDecimalPlaces(7); - - FactMetaData* altitudeMetaData = new FactMetaData(FactMetaData::valueTypeDouble, _altitudeFact); - altitudeMetaData->setUnits("meters"); - - FactMetaData* headingMetaData = new FactMetaData(FactMetaData::valueTypeDouble, _headingDegreesFact); - headingMetaData->setUnits("deg"); - - _pitchMetaData = new FactMetaData(FactMetaData::valueTypeDouble, this); - _pitchMetaData->setUnits("deg"); - - _acceptanceRadiusMetaData = new FactMetaData(FactMetaData::valueTypeDouble, this); - _acceptanceRadiusMetaData->setUnits("meters"); - - _holdTimeMetaData = new FactMetaData(FactMetaData::valueTypeDouble, this); - _holdTimeMetaData->setUnits("seconds"); - - FactMetaData* loiterOrbitRadiusMetaData = new FactMetaData(FactMetaData::valueTypeDouble, this); - loiterOrbitRadiusMetaData->setUnits("meters"); - - _loiterTurnsMetaData = new FactMetaData(FactMetaData::valueTypeInt32, this); - _loiterTurnsMetaData->setUnits("count"); - - _loiterSecondsMetaData = new FactMetaData(FactMetaData::valueTypeDouble, this); - _loiterSecondsMetaData->setUnits("seconds"); - - _delaySecondsMetaData = new FactMetaData(FactMetaData::valueTypeDouble, this); - _delaySecondsMetaData->setUnits("seconds"); - - _jumpSequenceMetaData = new FactMetaData(FactMetaData::valueTypeInt32, this); - _jumpSequenceMetaData->setUnits("#"); - - _jumpRepeatMetaData = new FactMetaData(FactMetaData::valueTypeInt32, this); - _jumpRepeatMetaData->setUnits("count"); - - _latitudeFact->setMetaData(latitudeMetaData); - _longitudeFact->setMetaData(longitudeMetaData); - _altitudeFact->setMetaData(altitudeMetaData); - _headingDegreesFact->setMetaData(headingMetaData); - _loiterOrbitRadiusFact->setMetaData(loiterOrbitRadiusMetaData); - - _connectSignals(); -} - -MissionItem::MissionItem(const MissionItem& other, QObject* parent) - : QObject(parent) -{ - _latitudeFact = new Fact(this); - _longitudeFact = new Fact(this); - _altitudeFact = new Fact(this); - _headingDegreesFact = new Fact(this); - _loiterOrbitRadiusFact = new Fact(this); - _param1Fact = new Fact(this); - _param2Fact = new Fact(this); - _altitudeRelativeToHomeFact = new Fact(this); - - _pitchMetaData = new FactMetaData(this); - - _acceptanceRadiusMetaData = new FactMetaData(this); - _holdTimeMetaData = new FactMetaData(this); - _loiterTurnsMetaData = new FactMetaData(this); - _loiterSecondsMetaData = new FactMetaData(this); - _delaySecondsMetaData = new FactMetaData(this); - _jumpSequenceMetaData = new FactMetaData(this); - _jumpRepeatMetaData = new FactMetaData(this); - - _connectSignals(); - - *this = other; -} - -const MissionItem& MissionItem::operator=(const MissionItem& other) -{ - _sequenceNumber = other._sequenceNumber; - _isCurrentItem = other._isCurrentItem; - _frame = other._frame; - _command = other._command; - _autocontinue = other._autocontinue; - _reachedTime = other._reachedTime; - _distance = other._distance; - _altitudeRelativeToHomeFact = other._altitudeRelativeToHomeFact; - _dirty = other._dirty; - _homePositionSpecialCase = other._homePositionSpecialCase; - _homePositionValid = other._homePositionValid; - - *_latitudeFact = *other._latitudeFact; - *_longitudeFact = *other._longitudeFact; - *_altitudeFact = *other._altitudeFact; - *_headingDegreesFact = *other._headingDegreesFact; - *_loiterOrbitRadiusFact = *other._loiterOrbitRadiusFact; - *_param1Fact = *other._param1Fact; - *_param2Fact = *other._param2Fact; - - *_pitchMetaData = *other._pitchMetaData; - *_acceptanceRadiusMetaData = *other._acceptanceRadiusMetaData; - *_holdTimeMetaData = *other._holdTimeMetaData; - *_loiterTurnsMetaData = *other._loiterTurnsMetaData; - *_loiterSecondsMetaData = *other._loiterSecondsMetaData; - *_delaySecondsMetaData = *other._delaySecondsMetaData; - *_jumpSequenceMetaData = *other._jumpSequenceMetaData; - *_jumpRepeatMetaData = *other._jumpRepeatMetaData; - - return *this; -} - -void MissionItem::_connectSignals(void) -{ - // Connect to valueChanged to track dirty state - connect(_latitudeFact, &Fact::valueChanged, this, &MissionItem::_factValueChanged); - connect(_longitudeFact, &Fact::valueChanged, this, &MissionItem::_factValueChanged); - connect(_altitudeFact, &Fact::valueChanged, this, &MissionItem::_factValueChanged); - connect(_headingDegreesFact, &Fact::valueChanged, this, &MissionItem::_factValueChanged); - connect(_loiterOrbitRadiusFact, &Fact::valueChanged, this, &MissionItem::_factValueChanged); - connect(_param1Fact, &Fact::valueChanged, this, &MissionItem::_factValueChanged); - connect(_param2Fact, &Fact::valueChanged, this, &MissionItem::_factValueChanged); - connect(_altitudeRelativeToHomeFact, &Fact::valueChanged, this, &MissionItem::_factValueChanged); - - // Connect valueChanged signals so we can output coordinateChanged signal - connect(_latitudeFact, &Fact::valueChanged, this, &MissionItem::_coordinateFactChanged); - connect(_longitudeFact, &Fact::valueChanged, this, &MissionItem::_coordinateFactChanged); - connect(_altitudeFact, &Fact::valueChanged, this, &MissionItem::_coordinateFactChanged); - - connect(_headingDegreesFact, &Fact::valueChanged, this, &MissionItem::_headingDegreesFactChanged); - connect(_altitudeRelativeToHomeFact, &Fact::valueChanged, this, &MissionItem::_altitudeRelativeToHomeFactChanged); -} - -MissionItem::~MissionItem() -{ -} - -bool MissionItem::isNavigationType() -{ - return (_command < MavlinkQmlSingleton::MAV_CMD_NAV_LAST); -} - -void MissionItem::save(QTextStream &saveStream) -{ - QString position("%1\t%2\t%3"); - position = position.arg(x(), 0, 'g', 18); - position = position.arg(y(), 0, 'g', 18); - position = position.arg(z(), 0, 'g', 18); - QString parameters("%1\t%2\t%3\t%4"); - parameters = parameters.arg(param1(), 0, 'g', 18).arg(param2(), 0, 'g', 18).arg(loiterOrbitRadius(), 0, 'g', 18).arg(_yawRadians(), 0, 'g', 18); - // FORMAT: - // as documented here: http://qgroundcontrol.org/waypoint_protocol - saveStream << this->sequenceNumber() << "\t" << this->isCurrentItem() << "\t" << this->frame() << "\t" << this->command() << "\t" << parameters << "\t" << position << "\t" << this->autoContinue() << "\r\n"; //"\t" << this->getDescription() << "\r\n"; -} - -bool MissionItem::load(QTextStream &loadStream) -{ - const QStringList &wpParams = loadStream.readLine().split("\t"); - if (wpParams.size() == 12) { - setSequenceNumber(wpParams[0].toInt()); - setIsCurrentItem(wpParams[1].toInt() == 1 ? true : false); - setFrame(wpParams[2].toInt()); - setAction(wpParams[3].toInt()); - setParam1(wpParams[4].toDouble()); - setParam2(wpParams[5].toDouble()); - setLoiterOrbitRadius(wpParams[6].toDouble()); - _setYawRadians(wpParams[7].toDouble()); - setLatitude(wpParams[8].toDouble()); - setLongitude(wpParams[9].toDouble()); - setAltitude(wpParams[10].toDouble()); - _autocontinue = (wpParams[11].toInt() == 1 ? true : false); - return true; - } - return false; -} - - -void MissionItem::setSequenceNumber(int sequenceNumber) -{ - _sequenceNumber = sequenceNumber; - emit sequenceNumberChanged(_sequenceNumber); -} - -void MissionItem::setX(double x) -{ - if (!isinf(x) && !isnan(x) && ((_frame == MAV_FRAME_LOCAL_NED) || (_frame == MAV_FRAME_LOCAL_ENU))) - { - setLatitude(x); - } -} - -void MissionItem::setY(double y) -{ - if (!isinf(y) && !isnan(y) && ((_frame == MAV_FRAME_LOCAL_NED) || (_frame == MAV_FRAME_LOCAL_ENU))) - { - setLongitude(y); - } -} - -void MissionItem::setZ(double z) -{ - if (!isinf(z) && !isnan(z) && ((_frame == MAV_FRAME_LOCAL_NED) || (_frame == MAV_FRAME_LOCAL_ENU))) - { - setAltitude(z); - } -} - -void MissionItem::setLatitude(double lat) -{ - if (_latitudeFact->value().toDouble() != lat) - { - _latitudeFact->setValue(lat); - emit coordinateChanged(coordinate()); - } -} - -void MissionItem::setLongitude(double lon) -{ - if (_longitudeFact->value().toDouble() != lon) - { - _longitudeFact->setValue(lon); - emit coordinateChanged(coordinate()); - } -} - -void MissionItem::setAltitude(double altitude) -{ - if (_altitudeFact->value().toDouble() != altitude) - { - _altitudeFact->setValue(altitude); - emit valueStringsChanged(valueStrings()); - emit coordinateChanged(coordinate()); - } -} - -void MissionItem::setAction(int /*MAV_CMD*/ action) -{ - if (_command != action) { - _command = (MavlinkQmlSingleton::Qml_MAV_CMD)action; - - // Fix defaults according to WP type - - switch (_command) { - case MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF: - setParam1(defaultPitch); - break; - case MavlinkQmlSingleton::MAV_CMD_NAV_WAYPOINT: - setAcceptanceRadius(defaultAcceptanceRadius); - break; - case MavlinkQmlSingleton::MAV_CMD_NAV_LOITER_UNLIM: - case MavlinkQmlSingleton::MAV_CMD_NAV_LOITER_TIME: - setLoiterOrbitRadius(defaultLoiterOrbitRadius); - break; - case MavlinkQmlSingleton::MAV_CMD_NAV_LOITER_TURNS: - setLoiterOrbitRadius(defaultLoiterOrbitRadius); - setParam1(defaultLoiterTurns); - break; - default: - break; - } - setHeadingDegrees(defaultHeading); - setAltitude(defaultAltitude); - - if (specifiesCoordinate()) { - if (_frame != MAV_FRAME_GLOBAL && _frame != MAV_FRAME_GLOBAL_RELATIVE_ALT) { - setFrame(MAV_FRAME_GLOBAL_RELATIVE_ALT); - } - } else { - setFrame(MAV_FRAME_MISSION); - } - - emit commandNameChanged(commandName()); - emit commandChanged((MavlinkQmlSingleton::Qml_MAV_CMD)_command); - emit valueLabelsChanged(valueLabels()); - emit valueStringsChanged(valueStrings()); - } -} - -int MissionItem::frame(void) const -{ - if (_altitudeRelativeToHomeFact->value().toBool()) { - return MAV_FRAME_GLOBAL_RELATIVE_ALT; - } else { - return _frame; - } -} - -void MissionItem::setFrame(int /*MAV_FRAME*/ frame) -{ - if (_frame != frame) { - _altitudeRelativeToHomeFact->setValue(frame == MAV_FRAME_GLOBAL_RELATIVE_ALT); - _frame = frame; - emit frameChanged(_frame); - } -} - -void MissionItem::setAutocontinue(bool autoContinue) -{ - if (_autocontinue != autoContinue) { - _autocontinue = autoContinue; - emit autoContinueChanged(_autocontinue); - } -} - -void MissionItem::setIsCurrentItem(bool isCurrentItem) -{ - if (_isCurrentItem != isCurrentItem) { - _isCurrentItem = isCurrentItem; - emit isCurrentItemChanged(isCurrentItem); - } -} - -void MissionItem::setAcceptanceRadius(double radius) -{ - setParam2(radius); -} - -void MissionItem::setParam1(double param) -{ - if (param1() != param) - { - _param1Fact->setValue(param); - emit valueStringsChanged(valueStrings()); - } -} - -void MissionItem::setParam2(double param) -{ - if (param2() != param) - { - _param2Fact->setValue(param); - emit valueStringsChanged(valueStrings()); - } -} - -void MissionItem::setParam3(double param3) -{ - setLoiterOrbitRadius(param3); -} - -void MissionItem::setParam4(double param4) -{ - _setYawRadians(param4); -} - -void MissionItem::setParam5(double param5) -{ - setLatitude(param5); -} - -void MissionItem::setParam6(double param6) -{ - setLongitude(param6); -} - -void MissionItem::setParam7(double param7) -{ - setAltitude(param7); -} - -void MissionItem::setLoiterOrbitRadius(double radius) -{ - if (loiterOrbitRadius() != radius) { - _loiterOrbitRadiusFact->setValue(radius); - emit valueStringsChanged(valueStrings()); - } -} - -void MissionItem::setHoldTime(int holdTime) -{ - setParam1(holdTime); -} - -void MissionItem::setHoldTime(double holdTime) -{ - setParam1(holdTime); -} - -bool MissionItem::specifiesCoordinate(void) const -{ - switch (_command) { - case MAV_CMD_NAV_WAYPOINT: - case MAV_CMD_NAV_LOITER_UNLIM: - case MAV_CMD_NAV_LOITER_TURNS: - case MAV_CMD_NAV_LOITER_TIME: - case MAV_CMD_NAV_LAND: - case MAV_CMD_NAV_TAKEOFF: - return true; - default: - return false; - } -} - -QString MissionItem::commandName(void) -{ - QString type; - - switch (_command) { - case MAV_CMD_NAV_WAYPOINT: - type = "Waypoint"; - break; - case MAV_CMD_NAV_LOITER_UNLIM: - case MAV_CMD_NAV_LOITER_TURNS: - case MAV_CMD_NAV_LOITER_TIME: - type = "Loiter"; - break; - case MAV_CMD_NAV_RETURN_TO_LAUNCH: - type = "Return Home"; - break; - case MAV_CMD_NAV_LAND: - type = "Land"; - break; - case MAV_CMD_NAV_TAKEOFF: - type = "Takeoff"; - break; - case MAV_CMD_CONDITION_DELAY: - type = "Delay"; - break; - case MAV_CMD_DO_JUMP: - type = "Jump To Command"; - break; - default: - type = QString("Unknown (%1)").arg(_command); - break; - } - - return type; -} - -QString MissionItem::commandDescription(void) -{ - QString description; - - switch (_command) { - case MAV_CMD_NAV_WAYPOINT: - description = "Travel to a position in 3D space."; - break; - case MAV_CMD_NAV_LOITER_UNLIM: - description = "Travel to a position and Loiter around the specified radius indefinitely."; - break; - case MAV_CMD_NAV_LOITER_TURNS: - description = "Travel to a position and Loiter around the specified radius for a number of turns."; - break; - case MAV_CMD_NAV_LOITER_TIME: - description = "Travel to a position and Loiter around the specified radius for an amount of time."; - break; - case MAV_CMD_NAV_RETURN_TO_LAUNCH: - description = "Send the vehicle back to the home position."; - break; - case MAV_CMD_NAV_LAND: - description = "Land vehicle at the specified location."; - break; - case MAV_CMD_NAV_TAKEOFF: - description = "Take off from the ground and travel towards the specified position."; - break; - case MAV_CMD_CONDITION_DELAY: - description = "Delay"; - break; - case MAV_CMD_DO_JUMP: - description = "Jump To Command"; - break; - default: - description = QString("Unknown (%1)").arg(_command); - break; - } - - return description; -} - -QStringList MissionItem::valueLabels(void) -{ - QStringList labels; - - switch (_command) { - case MAV_CMD_NAV_WAYPOINT: - if (frame() == MAV_FRAME_GLOBAL_RELATIVE_ALT) { - labels << "Alt (rel):"; - } else { - labels << "Alt:"; - } - labels << "Heading:" << "Radius:" << "Hold:"; - break; - case MAV_CMD_NAV_LOITER_UNLIM: - labels << "Heading:" << "Radius:"; - break; - case MAV_CMD_NAV_LOITER_TURNS: - labels << "Heading:" << "Radius:"<< "Turns:"; - break; - case MAV_CMD_NAV_LOITER_TIME: - labels << "Heading:" << "Radius:" << "Seconds:"; - break; - case MAV_CMD_NAV_RETURN_TO_LAUNCH: - break; - case MAV_CMD_NAV_LAND: - if (frame() == MAV_FRAME_GLOBAL_RELATIVE_ALT) { - labels << "Alt (rel):"; - } else { - labels << "Alt:"; - } - labels << "Heading:"; - break; - case MAV_CMD_NAV_TAKEOFF: - if (frame() == MAV_FRAME_GLOBAL_RELATIVE_ALT) { - labels << "Alt (rel):"; - } else { - labels << "Alt:"; - } - labels << "Heading:" << "Pitch:"; - break; - case MAV_CMD_CONDITION_DELAY: - labels << "Seconds:"; - break; - case MAV_CMD_DO_JUMP: - labels << "Jump to:" << "Repeat:"; - break; - default: - break; - } - - return labels; -} - -QString MissionItem::_oneDecimalString(double value) -{ - return QString("%1").arg(value, 0 /* min field width */, 'f' /* format */, 1 /* precision */); -} - -QStringList MissionItem::valueStrings(void) -{ - QStringList list; - - switch (_command) { - case MAV_CMD_NAV_WAYPOINT: - list << _oneDecimalString(_altitudeFact->value().toDouble()) << _oneDecimalString(headingDegrees()) << _oneDecimalString(param2()) << _oneDecimalString(param1()); - break; - case MAV_CMD_NAV_LOITER_UNLIM: - list << _oneDecimalString(headingDegrees()) << _oneDecimalString(loiterOrbitRadius()); - break; - case MAV_CMD_NAV_LOITER_TURNS: - list << _oneDecimalString(headingDegrees()) << _oneDecimalString(loiterOrbitRadius()) << _oneDecimalString(param1()); - break; - case MAV_CMD_NAV_LOITER_TIME: - list << _oneDecimalString(headingDegrees()) << _oneDecimalString(loiterOrbitRadius()) << _oneDecimalString(param1()); - break; - case MAV_CMD_NAV_RETURN_TO_LAUNCH: - break; - case MAV_CMD_NAV_LAND: - list << _oneDecimalString(_altitudeFact->value().toDouble()) << _oneDecimalString(headingDegrees()); - break; - case MAV_CMD_NAV_TAKEOFF: - list << _oneDecimalString(_altitudeFact->value().toDouble()) << _oneDecimalString(headingDegrees()) << _oneDecimalString(param1()); - break; - case MAV_CMD_CONDITION_DELAY: - list << _oneDecimalString(param1()); - break; - case MAV_CMD_DO_JUMP: - list << _oneDecimalString(param1()) << _oneDecimalString(param2()); - break; - default: - break; - } - - return list; -} - -QStringList MissionItem::commandNames(void) { - QStringList list; - - for (int i=0; i<_cMavCmd2Name; i++) { - list += _rgMavCmd2Name[i].name; - } - - return list; -} - -int MissionItem::commandByIndex(void) -{ - for (int i=0; i<_cMavCmd2Name; i++) { - if (_rgMavCmd2Name[i].command == (MAV_CMD)_command) { - return i; - } - } - - return -1; -} - -void MissionItem::setCommandByIndex(int index) -{ - if (index < 0 || index >= _cMavCmd2Name) { - qWarning() << "Invalid index" << index; - return; - } - - setCommand((MavlinkQmlSingleton::Qml_MAV_CMD)_rgMavCmd2Name[index].command); -} - -QmlObjectListModel* MissionItem::textFieldFacts(void) -{ - QmlObjectListModel* model = new QmlObjectListModel(this); - - switch ((MAV_CMD)_command) { - case MAV_CMD_NAV_WAYPOINT: - _param1Fact->_setName("Hold:"); - _param1Fact->setMetaData(_holdTimeMetaData); - model->append(_altitudeFact); - if (!_homePositionSpecialCase) { - model->append(_param1Fact); - } - break; - case MAV_CMD_NAV_LOITER_UNLIM: - model->append(_altitudeFact); - model->append(_loiterOrbitRadiusFact); - break; - case MAV_CMD_NAV_LOITER_TURNS: - _param1Fact->_setName("Turns:"); - _param1Fact->setMetaData(_loiterTurnsMetaData); - model->append(_altitudeFact); - model->append(_loiterOrbitRadiusFact); - model->append(_param1Fact); - break; - case MAV_CMD_NAV_LOITER_TIME: - _param1Fact->_setName("Seconds:"); - _param1Fact->setMetaData(_loiterSecondsMetaData); - model->append(_altitudeFact); - model->append(_loiterOrbitRadiusFact); - model->append(_param1Fact); - break; - case MAV_CMD_NAV_LAND: - model->append(_altitudeFact); - break; - case MAV_CMD_NAV_TAKEOFF: - _param1Fact->_setName("Pitch:"); - _param1Fact->setMetaData(_pitchMetaData); - model->append(_altitudeFact); - model->append(_param1Fact); - break; - case MAV_CMD_CONDITION_DELAY: - _param1Fact->_setName("Seconds:"); - _param1Fact->setMetaData(_delaySecondsMetaData); - model->append(_param1Fact); - break; - case MAV_CMD_DO_JUMP: - _param1Fact->_setName("Seq #:"); - _param1Fact->setMetaData(_jumpSequenceMetaData); - _param2Fact->_setName("Repeat:"); - _param2Fact->setMetaData(_jumpRepeatMetaData); - model->append(_param1Fact); - model->append(_param2Fact); - break; - default: - break; - } - - if (specifiesHeading()) { - model->append(_headingDegreesFact); - } - - - return model; -} - -QmlObjectListModel* MissionItem::checkboxFacts(void) -{ - QmlObjectListModel* model = new QmlObjectListModel(this); - - switch ((MAV_CMD)_command) { - case MAV_CMD_NAV_WAYPOINT: - if (!_homePositionSpecialCase) { - model->append(_altitudeRelativeToHomeFact); - } - break; - case MAV_CMD_NAV_LOITER_UNLIM: - model->append(_altitudeRelativeToHomeFact); - break; - case MAV_CMD_NAV_LOITER_TURNS: - model->append(_altitudeRelativeToHomeFact); - break; - case MAV_CMD_NAV_LOITER_TIME: - model->append(_altitudeRelativeToHomeFact); - break; - case MAV_CMD_NAV_RETURN_TO_LAUNCH: - break; - case MAV_CMD_NAV_LAND: - model->append(_altitudeRelativeToHomeFact); - break; - case MAV_CMD_NAV_TAKEOFF: - model->append(_altitudeRelativeToHomeFact); - break; - default: - break; - } - - return model; -} - -double MissionItem::headingDegrees(void) const -{ - return _headingDegreesFact->value().toDouble(); -} - -void MissionItem::setHeadingDegrees(double headingDegrees) -{ - if (_headingDegreesFact->value().toDouble() != headingDegrees) { - _headingDegreesFact->setValue(headingDegrees); - emit valueStringsChanged(valueStrings()); - emit headingDegreesChanged(headingDegrees); - } -} - - -double MissionItem::_yawRadians(void) const -{ - return _headingDegreesFact->value().toDouble() * (M_PI / 180.0); -} - -void MissionItem::_setYawRadians(double yawRadians) -{ - setHeadingDegrees(yawRadians * (180 / M_PI)); -} - -QGeoCoordinate MissionItem::coordinate(void) const -{ - return QGeoCoordinate(latitude(), longitude(), altitude()); -} - -void MissionItem::setCoordinate(const QGeoCoordinate& coordinate) -{ - setLatitude(coordinate.latitude()); - setLongitude(coordinate.longitude()); - setAltitude(coordinate.altitude()); -} - -bool MissionItem::canEdit(void) -{ - bool found = false; - - for (int i=0; i<_cMavCmd2Name; i++) { - if (_rgMavCmd2Name[i].command == (MAV_CMD)_command) { - found = true; - break; - } - } - - if (found) { - if (!_autocontinue) { - qCDebug(MissionItemLog) << "canEdit false due to _autocontinue != true"; - return false; - } - - if (_frame != MAV_FRAME_GLOBAL && _frame != MAV_FRAME_GLOBAL_RELATIVE_ALT && _frame != MAV_FRAME_MISSION) { - qCDebug(MissionItemLog) << "canEdit false due unsupported frame type:" << _frame; - return false; - } - - return true; - } else { - qCDebug(MissionItemLog) << "canEdit false due unsupported command:" << _command; - return false; - } -} - -void MissionItem::setDirty(bool dirty) -{ - if (!_homePositionSpecialCase || !dirty) { - // Home position never affects dirty bit - - _dirty = dirty; - // We want to emit dirtyChanged even if _dirty didn't change. This can be handy signal for - // any value within the item changing. - emit dirtyChanged(_dirty); - } -} - -void MissionItem::_factValueChanged(QVariant value) -{ - Q_UNUSED(value); - setDirty(true); -} - -void MissionItem::_coordinateFactChanged(QVariant value) -{ - Q_UNUSED(value); - emit coordinateChanged(coordinate()); -} - -bool MissionItem::specifiesHeading(void) const -{ - switch ((MAV_CMD)_command) { - case MAV_CMD_NAV_LAND: - case MAV_CMD_NAV_TAKEOFF: - return true; - default: - return false; - } -} - -void MissionItem::_headingDegreesFactChanged(QVariant value) -{ - emit headingDegreesChanged(value.toDouble()); -} - -void MissionItem::_altitudeRelativeToHomeFactChanged(QVariant value) -{ - // Don't call setFrame, that will cause a signalling loop - - int frame = value.toBool() ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL; - if (_frame != frame) { - _frame = frame; - emit frameChanged(_frame); - } -} - -void MissionItem::setHomePositionValid(bool homePositionValid) -{ - _homePositionValid = homePositionValid; - emit homePositionValidChanged(_homePositionValid); -} - -void MissionItem::setDistance(double distance) -{ - _distance = distance; - emit distanceChanged(_distance); -} diff --git a/src/MissionItem.h b/src/MissionItem.h deleted file mode 100644 index a3b5101a4..000000000 --- a/src/MissionItem.h +++ /dev/null @@ -1,320 +0,0 @@ -/*===================================================================== - - QGroundControl Open Source Ground Control Station - - (c) 2009 - 2014 QGROUNDCONTROL PROJECT - - This file is part of the QGROUNDCONTROL project - - QGROUNDCONTROL is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - QGROUNDCONTROL is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with QGROUNDCONTROL. If not, see . - - ======================================================================*/ - -#ifndef MissionItem_H -#define MissionItem_H - -#include -#include -#include -#include -#include - -#include "QGCMAVLink.h" -#include "QGC.h" -#include "MavlinkQmlSingleton.h" -#include "QmlObjectListModel.h" -#include "Fact.h" -#include "QGCLoggingCategory.h" -#include "QmlObjectListModel.h" - -Q_DECLARE_LOGGING_CATEGORY(MissionItemLog) - -class MissionItem : public QObject -{ - Q_OBJECT - -public: - MissionItem(QObject *parent = 0, - int sequenceNumber = 0, - QGeoCoordinate coordiante = QGeoCoordinate(), - int action = MAV_CMD_NAV_WAYPOINT, - double param1 = 0.0, - double param2 = defaultAcceptanceRadius, - double param3 = defaultLoiterOrbitRadius, - double param4 = defaultHeading, - bool autocontinue = true, - bool isCurrentItem = false, - int frame = MAV_FRAME_GLOBAL_RELATIVE_ALT); - - MissionItem(const MissionItem& other, QObject* parent = NULL); - ~MissionItem(); - - const MissionItem& operator=(const MissionItem& other); - - /// Returns true if the item has been modified since the last time dirty was false - Q_PROPERTY(bool dirty READ dirty WRITE setDirty NOTIFY dirtyChanged) - - Q_PROPERTY(int sequenceNumber READ sequenceNumber WRITE setSequenceNumber NOTIFY sequenceNumberChanged) - Q_PROPERTY(bool isCurrentItem READ isCurrentItem WRITE setIsCurrentItem NOTIFY isCurrentItemChanged) - - Q_PROPERTY(bool specifiesCoordinate READ specifiesCoordinate NOTIFY commandChanged) - Q_PROPERTY(QGeoCoordinate coordinate READ coordinate WRITE setCoordinate NOTIFY coordinateChanged) - - Q_PROPERTY(bool specifiesHeading READ specifiesHeading NOTIFY commandChanged) - Q_PROPERTY(double heading READ headingDegrees WRITE setHeadingDegrees NOTIFY headingDegreesChanged) - - Q_PROPERTY(QStringList commandNames READ commandNames CONSTANT) - Q_PROPERTY(QString commandName READ commandName NOTIFY commandChanged) - Q_PROPERTY(QString commandDescription READ commandDescription NOTIFY commandChanged) - Q_PROPERTY(QStringList valueLabels READ valueLabels NOTIFY commandChanged) - Q_PROPERTY(QStringList valueStrings READ valueStrings NOTIFY valueStringsChanged) - Q_PROPERTY(int commandByIndex READ commandByIndex WRITE setCommandByIndex NOTIFY commandChanged) - Q_PROPERTY(QmlObjectListModel* textFieldFacts READ textFieldFacts NOTIFY commandChanged) - Q_PROPERTY(QmlObjectListModel* checkboxFacts READ checkboxFacts NOTIFY commandChanged) - Q_PROPERTY(MavlinkQmlSingleton::Qml_MAV_CMD command READ command WRITE setCommand NOTIFY commandChanged) - Q_PROPERTY(QmlObjectListModel* childItems READ childItems CONSTANT) - - /// true: this item is being used as a home position indicator - Q_PROPERTY(bool homePosition READ homePosition CONSTANT) - - /// true: home position should be shown - Q_PROPERTY(bool homePositionValid READ homePositionValid WRITE setHomePositionValid NOTIFY homePositionValidChanged) - - /// Distance to previous waypoint, set by UI controller - Q_PROPERTY(double distance READ distance WRITE setDistance NOTIFY distanceChanged) - - // Property accesors - - int sequenceNumber(void) const { return _sequenceNumber; } - void setSequenceNumber(int sequenceNumber); - - bool isCurrentItem(void) const { return _isCurrentItem; } - void setIsCurrentItem(bool isCurrentItem); - - bool specifiesCoordinate(void) const; - QGeoCoordinate coordinate(void) const; - void setCoordinate(const QGeoCoordinate& coordinate); - - bool specifiesHeading(void) const; - double headingDegrees(void) const; - void setHeadingDegrees(double headingDegrees); - - // This is public for unit testing - double _yawRadians(void) const; - - QStringList commandNames(void); - QString commandName(void); - QString commandDescription(void); - - int commandByIndex(void); - void setCommandByIndex(int index); - - MavlinkQmlSingleton::Qml_MAV_CMD command(void) { return (MavlinkQmlSingleton::Qml_MAV_CMD)_command; }; - void setCommand(MavlinkQmlSingleton::Qml_MAV_CMD command) { setAction(command); } - - QStringList valueLabels(void); - QStringList valueStrings(void); - - QmlObjectListModel* textFieldFacts(void); - QmlObjectListModel* checkboxFacts(void); - - bool dirty(void) { return _dirty; } - void setDirty(bool dirty); - - QmlObjectListModel* childItems(void) { return &_childItems; } - - bool homePosition(void) { return _homePositionSpecialCase; } - bool homePositionValid(void) { return _homePositionValid; } - void setHomePositionValid(bool homePositionValid); - - double distance(void) { return _distance; } - void setDistance(double distance); - - // C++ only methods - - /// Returns true if this item can be edited in the ui - bool canEdit(void); - - double latitude(void) const { return _latitudeFact->value().toDouble(); } - double longitude(void) const { return _longitudeFact->value().toDouble(); } - double altitude(void) const { return _altitudeFact->value().toDouble(); } - - void setLatitude(double latitude); - void setLongitude(double longitude); - void setAltitude(double altitude); - - double x(void) const { return latitude(); } - double y(void) const { return longitude(); } - double z(void) const { return altitude(); } - - void setX(double x); - void setY(double y); - void setZ(double z); - - bool autoContinue() const { - return _autocontinue; - } - double loiterOrbitRadius() const { - return _loiterOrbitRadiusFact->value().toDouble(); - } - double acceptanceRadius() const { - return param2(); - } - double holdTime() const { - return param1(); - } - double param1() const { - return _param1Fact->value().toDouble(); - } - double param2() const { - return _param2Fact->value().toDouble(); - } - double param3() const { - return loiterOrbitRadius(); - } - double param4() const { - return _yawRadians(); - } - double param5() const { - return latitude(); - } - double param6() const { - return longitude(); - } - double param7() const { - return altitude(); - } - // MAV_FRAME - int frame() const; - - // MAV_CMD - int command() const { - return _command; - } - /** @brief Returns true if x, y, z contain reasonable navigation data */ - bool isNavigationType(); - - /** @brief Get the time this waypoint was reached */ - quint64 reachedTime() const { return _reachedTime; } - - void save(QTextStream &saveStream); - bool load(QTextStream &loadStream); - - void setHomePositionSpecialCase(bool homePositionSpecialCase) { _homePositionSpecialCase = homePositionSpecialCase; } - - bool relativeAltitude(void) { return _frame == MAV_FRAME_GLOBAL_RELATIVE_ALT; } - - static const double defaultPitch; - static const double defaultHeading; - static const double defaultAltitude; - static const double defaultAcceptanceRadius; - static const double defaultLoiterOrbitRadius; - static const double defaultLoiterTurns; - -signals: - void sequenceNumberChanged(int sequenceNumber); - void isCurrentItemChanged(bool isCurrentItem); - void coordinateChanged(const QGeoCoordinate& coordinate); - void headingDegreesChanged(double heading); - void dirtyChanged(bool dirty); - void homePositionValidChanged(bool homePostionValid); - void distanceChanged(float distance); - void frameChanged(int frame); - void commandNameChanged(QString type); - void commandChanged(MavlinkQmlSingleton::Qml_MAV_CMD command); - void valueLabelsChanged(QStringList valueLabels); - void valueStringsChanged(QStringList valueStrings); - bool autoContinueChanged(bool autoContinue); - -public: - /** @brief Set the waypoint action */ - void setAction (int _action); - void setFrame (int _frame); - void setAutocontinue(bool autoContinue); - void setCurrent (bool _current); - void setLoiterOrbitRadius (double radius); - void setParam1 (double _param1); - void setParam2 (double _param2); - void setParam3 (double param3); - void setParam4 (double param4); - void setParam5 (double param5); - void setParam6 (double param6); - void setParam7 (double param7); - void setAcceptanceRadius(double radius); - void setHoldTime (int holdTime); - void setHoldTime (double holdTime); - /** @brief Set waypoint as reached */ - void setReached () { _reachedTime = QGC::groundTimeMilliseconds(); } - /** @brief Wether this waypoint has been reached yet */ - bool isReached () { return (_reachedTime > 0); } - -private slots: - void _factValueChanged(QVariant value); - void _coordinateFactChanged(QVariant value); - void _headingDegreesFactChanged(QVariant value); - void _altitudeRelativeToHomeFactChanged(QVariant value); - -private: - QString _oneDecimalString(double value); - void _connectSignals(void); - void _setYawRadians(double yawRadians); - -private: - typedef struct { - MAV_CMD command; - const char* name; - } MavCmd2Name_t; - - int _sequenceNumber; - int _frame; - MavlinkQmlSingleton::Qml_MAV_CMD _command; - bool _autocontinue; - bool _isCurrentItem; - quint64 _reachedTime; - double _distance; - - Fact* _latitudeFact; - Fact* _longitudeFact; - Fact* _altitudeFact; - Fact* _headingDegreesFact; - Fact* _loiterOrbitRadiusFact; - Fact* _param1Fact; - Fact* _param2Fact; - Fact* _altitudeRelativeToHomeFact; - - FactMetaData* _pitchMetaData; - FactMetaData* _acceptanceRadiusMetaData; - FactMetaData* _holdTimeMetaData; - FactMetaData* _loiterTurnsMetaData; - FactMetaData* _loiterSecondsMetaData; - FactMetaData* _delaySecondsMetaData; - FactMetaData* _jumpSequenceMetaData; - FactMetaData* _jumpRepeatMetaData; - - bool _dirty; - - bool _homePositionSpecialCase; ///< true: this item is being used as a ui home position indicator - bool _homePositionValid; ///< true: home psition should be displayed - - /// This is used to reference any subsequent mission items which do not specify a coordinate. - QmlObjectListModel _childItems; - - static const int _cMavCmd2Name = 9; - static const MavCmd2Name_t _rgMavCmd2Name[_cMavCmd2Name]; -}; - -QDebug operator<<(QDebug dbg, const MissionItem& missionItem); -QDebug operator<<(QDebug dbg, const MissionItem* missionItem); - -#endif diff --git a/src/MissionManager/CogWheel.svg b/src/MissionManager/CogWheel.svg new file mode 100644 index 000000000..8069a7e4e --- /dev/null +++ b/src/MissionManager/CogWheel.svg @@ -0,0 +1,84 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 31 + + 31 + + + + + + + + + + + + + + + + + diff --git a/src/MissionManager/MavCmdInfo.json b/src/MissionManager/MavCmdInfo.json new file mode 100644 index 000000000..cf621ef24 --- /dev/null +++ b/src/MissionManager/MavCmdInfo.json @@ -0,0 +1,200 @@ +{ + "version": 1, + + "mavCmdInfo": [ + { + "comment": "MAV_CMD_NAV_LAST: Used for fake home position waypoint", + "id": 95, + "rawName": "Home", + "friendlyName": "Home", + "description": "Home Position", + "specifiesCoordinate": true, + "friendlyEdit": true + }, + { + "id": 16, + "rawName": "MAV_CMD_NAV_WAYPOINT", + "friendlyName": "Waypoint", + "description": "Travel to a position in 3D space.", + "specifiesCoordinate": true, + "friendlyEdit": true, + "param1": { + "label": "Hold:", + "units": "seconds", + "decimalPlaces": 0 + } + }, + { + "id": 17, + "rawName": "MAV_CMD_NAV_LOITER_UNLIM", + "friendlyName": "Loiter", + "description": "Travel to a position and Loiter around the specified radius indefinitely.", + "specifiesCoordinate": true, + "friendlyEdit": true, + "param3": { + "label": "Radius:", + "units": "meters", + "decimalPlaces": 2 + } + }, + { + "id": 18, + "rawName": "MAV_CMD_NAV_LOITER_TURNS", + "friendlyName": "Loiter (turns)", + "description": "Travel to a position and Loiter around the specified radius for a number of turns.", + "specifiesCoordinate": true, + "friendlyEdit": true, + "param1": { + "label": "Turns:", + "decimalPlaces": 0 + }, + "param3": { + "label": "Radius:", + "units": "meters", + "decimalPlaces": 2 + } + }, + { + "id": 19, + "rawName": "MAV_CMD_NAV_LOITER_TIME", + "friendlyName": "Loiter (time)", + "description": "Travel to a position and Loiter around the specified radius for an amount of time.", + "specifiesCoordinate": true, + "friendlyEdit": true, + "param1": { + "label": "Hold:", + "units": "seconds", + "decimalPlaces": 0 + }, + "param3": { + "label": "Radius:", + "units": "meters", + "decimalPlaces": 2 + } + }, + { + "id": 20, + "rawName": "MAV_CMD_NAV_RETURN_TO_LAUNCH", + "friendlyName": "Return Home", + "description": "Send the vehicle back to the home position.", + "friendlyEdit": true + }, + { + "id": 21, + "rawName": "MAV_CMD_NAV_LAND", + "friendlyName": "Land", + "description": "Land vehicle at the specified location.", + "specifiesCoordinate": true, + "friendlyEdit": true, + "param1": { + "label": "Abort Alt:", + "units": "meters", + "decimalPlaces": 3 + }, + "param4": { + "label": "Heading:", + "units": "degrees", + "decimalPlaces": 2 + } + }, + { + "id": 22, + "rawName": "MAV_CMD_NAV_TAKEOFF", + "friendlyName": "Takeoff", + "description": "Take off from the ground and travel towards the specified position.", + "specifiesCoordinate": true, + "friendlyEdit": true, + "param1": { + "label": "Pitch:", + "units": "degrees", + "decimalPlaces": 2 + }, + "param4": { + "label": "Heading:", + "units": "degrees", + "decimalPlaces": 2 + } + }, + { "id": 23, "rawName": "MAV_CMD_NAV_LAND_LOCAL", "friendlyName": "MAV_CMD_NAV_LAND_LOCAL" }, + { "id": 24, "rawName": "MAV_CMD_NAV_TAKEOFF_LOCAL", "friendlyName": "MAV_CMD_NAV_TAKEOFF_LOCAL" }, + { "id": 25, "rawName": "MAV_CMD_NAV_FOLLOW", "friendlyName": "MAV_CMD_NAV_FOLLOW" }, + { "id": 30, "rawName": "MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT", "friendlyName": "MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT" }, + { "id": 31, "rawName": "MAV_CMD_NAV_LOITER_TO_ALT" }, + { "id": 80, "rawName": "MAV_CMD_NAV_ROI", "friendlyName": "MAV_CMD_NAV_ROI" }, + { "id": 81, "rawName": "MAV_CMD_NAV_PATHPLANNING", "friendlyName": "MAV_CMD_NAV_PATHPLANNING" }, + { "id": 82, "rawName": "MAV_CMD_NAV_SPLINE_WAYPOINT", "friendlyName": "MAV_CMD_NAV_SPLINE_WAYPOINT" }, + { "id": 83, "rawName": "MAV_CMD_NAV_ALTITUDE_WAIT", "friendlyName": "MAV_CMD_NAV_ALTITUDE_WAIT" }, + { "id": 92, "rawName": "MAV_CMD_NAV_GUIDED_ENABLE", "friendlyName": "MAV_CMD_NAV_GUIDED_ENABLE" }, + { + "id": 112, + "rawName": "MAV_CMD_CONDITION_DELAY", + "friendlyName": "Delay", + "description": "Delay the mission for the number of seconds.", + "friendlyEdit": true, + "param1": { + "label": "Hold:", + "units": "seconds", + "decimalPlaces": 0 + } + }, + { "id": 113, "rawName": "MAV_CMD_CONDITION_CHANGE_ALT", "friendlyName": "MAV_CMD_CONDITION_CHANGE_ALT" }, + { "id": 114, "rawName": "MAV_CMD_CONDITION_DISTANCE", "friendlyName": "MAV_CMD_CONDITION_DISTANCE" }, + { "id": 115, "rawName": "MAV_CMD_CONDITION_YAW", "friendlyName": "MAV_CMD_CONDITION_YAW" }, + { "id": 159, "rawName": "MAV_CMD_CONDITION_LAST", "friendlyName": "MAV_CMD_CONDITION_LAST" }, + { "id": 176, "rawName": "MAV_CMD_DO_SET_MODE", "friendlyName": "MAV_CMD_DO_SET_MODE" }, + { + "id": 177, + "rawName": "MAV_CMD_DO_JUMP", + "friendlyName": "Jump to item", + "description": "Mission will continue at the specified item.", + "friendlyEdit": true, + "param1": { + "label": "Seq #:", + "decimalPlaces": 0 + }, + "param2": { + "label": "Repeat:", + "decimalPlaces": 0 + } + }, + { "id": 178, "rawName": "MAV_CMD_DO_CHANGE_SPEED", "friendlyName": "MAV_CMD_DO_CHANGE_SPEED" }, + { "id": 179, "rawName": "MAV_CMD_DO_SET_HOME", "friendlyName": "MAV_CMD_DO_SET_HOME" }, + { "id": 180, "rawName": "MAV_CMD_DO_SET_PARAMETER", "friendlyName": "MAV_CMD_DO_SET_PARAMETER" }, + { "id": 181, "rawName": "MAV_CMD_DO_SET_RELAY", "friendlyName": "MAV_CMD_DO_SET_RELAY" }, + { "id": 182, "rawName": "MAV_CMD_DO_REPEAT_RELAY", "friendlyName": "MAV_CMD_DO_REPEAT_RELAY" }, + { "id": 183, "rawName": "MAV_CMD_DO_SET_SERVO", "friendlyName": "MAV_CMD_DO_SET_SERVO" }, + { "id": 184, "rawName": "MAV_CMD_DO_REPEAT_SERVO", "friendlyName": "MAV_CMD_DO_REPEAT_SERVO" }, + { "id": 185, "rawName": "MAV_CMD_DO_FLIGHTTERMINATION", "friendlyName": "MAV_CMD_DO_FLIGHTTERMINATION" }, + { "id": 189, "rawName": "MAV_CMD_DO_LAND_START", "friendlyName": "MAV_CMD_DO_LAND_START" }, + { "id": 190, "rawName": "MAV_CMD_DO_RALLY_LAND", "friendlyName": "MAV_CMD_DO_RALLY_LAND" }, + { "id": 191, "rawName": "MAV_CMD_DO_GO_AROUND", "friendlyName": "MAV_CMD_DO_GO_AROUND" }, + { "id": 200, "rawName": "MAV_CMD_DO_CONTROL_VIDEO", "friendlyName": "MAV_CMD_DO_CONTROL_VIDEO" }, + { "id": 201, "rawName": "MAV_CMD_DO_SET_ROI", "friendlyName": "MAV_CMD_DO_SET_ROI" }, + { "id": 202, "rawName": "MAV_CMD_DO_DIGICAM_CONFIGURE", "friendlyName": "MAV_CMD_DO_DIGICAM_CONFIGURE" }, + { "id": 203, "rawName": "MAV_CMD_DO_DIGICAM_CONTROL", "friendlyName": "MAV_CMD_DO_DIGICAM_CONTROL" }, + { "id": 204, "rawName": "MAV_CMD_DO_MOUNT_CONFIGURE", "friendlyName": "MAV_CMD_DO_MOUNT_CONFIGURE" }, + { "id": 205, "rawName": "MAV_CMD_DO_MOUNT_CONTROL", "friendlyName": "MAV_CMD_DO_MOUNT_CONTROL" }, + { "id": 206, "rawName": "MAV_CMD_DO_SET_CAM_TRIGG_DIST", "friendlyName": "MAV_CMD_DO_SET_CAM_TRIGG_DIST" }, + { "id": 207, "rawName": "MAV_CMD_DO_FENCE_ENABLE", "friendlyName": "MAV_CMD_DO_FENCE_ENABLE" }, + { "id": 208, "rawName": "MAV_CMD_DO_PARACHUTE", "friendlyName": "MAV_CMD_DO_PARACHUTE" }, + { "id": 209, "rawName": "MAV_CMD_DO_MOTOR_TEST", "friendlyName": "MAV_CMD_DO_MOTOR_TEST" }, + { "id": 210, "rawName": "MAV_CMD_DO_INVERTED_FLIGHT", "friendlyName": "MAV_CMD_DO_INVERTED_FLIGHT" }, + { "id": 211, "rawName": "MAV_CMD_DO_GRIPPER", "friendlyName": "MAV_CMD_DO_GRIPPER" }, + { "id": 212, "rawName": "MAV_CMD_DO_AUTOTUNE_ENABLE", "friendlyName": "MAV_CMD_DO_AUTOTUNE_ENABLE" }, + { "id": 220, "rawName": "MAV_CMD_DO_MOUNT_CONTROL_QUAT", "friendlyName": "MAV_CMD_DO_MOUNT_CONTROL_QUAT" }, + { "id": 221, "rawName": "MAV_CMD_DO_GUIDED_MASTER", "friendlyName": "MAV_CMD_DO_GUIDED_MASTER" }, + { "id": 222, "rawName": "MAV_CMD_DO_GUIDED_LIMITS", "friendlyName": "MAV_CMD_DO_GUIDED_LIMITS" }, + { "id": 252, "rawName": "MAV_CMD_OVERRIDE_GOTO", "friendlyName": "MAV_CMD_OVERRIDE_GOTO" }, + { "id": 300, "rawName": "MAV_CMD_MISSION_START", "friendlyName": "MAV_CMD_MISSION_START" }, + { "id": 400, "rawName": "MAV_CMD_COMPONENT_ARM_DISARM", "friendlyName": "MAV_CMD_COMPONENT_ARM_DISARM" }, + { "id": 2000, "rawName": "MAV_CMD_IMAGE_START_CAPTURE", "friendlyName": "MAV_CMD_IMAGE_START_CAPTURE" }, + { "id": 2001, "rawName": "MAV_CMD_IMAGE_STOP_CAPTURE", "friendlyName": "MAV_CMD_IMAGE_STOP_CAPTURE" }, + { "id": 2003, "rawName": "MAV_CMD_DO_TRIGGER_CONTROL", "friendlyName": "MAV_CMD_DO_TRIGGER_CONTROL" }, + { "id": 2500, "rawName": "MAV_CMD_VIDEO_START_CAPTURE", "friendlyName": "MAV_CMD_VIDEO_START_CAPTURE" }, + { "id": 2501, "rawName": "MAV_CMD_VIDEO_STOP_CAPTURE", "friendlyName": "MAV_CMD_VIDEO_STOP_CAPTURE" }, + { "id": 2800, "rawName": "MAV_CMD_PANORAMA_CREATE", "friendlyName": "MAV_CMD_PANORAMA_CREATE" }, + { "id": 3000, "rawName": "MAV_CMD_DO_VTOL_TRANSITION", "friendlyName": "MAV_CMD_DO_VTOL_TRANSITION" }, + { "id": 30001, "rawName": "MAV_CMD_PAYLOAD_PREPARE_DEPLOY", "friendlyName": "MAV_CMD_PAYLOAD_PREPARE_DEPLOY" }, + { "id": 30002, "rawName": "MAV_CMD_PAYLOAD_CONTROL_DEPLOY", "friendlyName": "MAV_CMD_PAYLOAD_CONTROL_DEPLOY" } + ] +} diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index bd4188152..7bb30c6c9 100644 --- a/src/MissionManager/MissionController.cc +++ b/src/MissionManager/MissionController.cc @@ -37,7 +37,6 @@ MissionController::MissionController(QObject *parent) : QObject(parent) , _editMode(false) , _missionItems(NULL) - , _canEdit(true) , _activeVehicle(NULL) , _liveHomePositionAvailable(false) , _autoSync(false) @@ -51,7 +50,6 @@ MissionController::MissionController(QObject *parent) MissionController::~MissionController() { // Start with empty list - _canEdit = true; _missionItems = new QmlObjectListModel(this); _initAllMissionItems(); } @@ -129,11 +127,9 @@ void MissionController::_setupMissionItems(bool loadFromVehicle, bool forceLoad) } if (!missionManager || !loadFromVehicle || missionManager->inProgress()) { - _canEdit = true; _missionItems = new QmlObjectListModel(this); qCDebug(MissionControllerLog) << "creating empty set"; } else { - _canEdit = missionManager->canEdit(); _missionItems = missionManager->copyMissionItems(); qCDebug(MissionControllerLog) << "loading from vehicle count"<< _missionItems->count(); } @@ -163,18 +159,15 @@ void MissionController::sendMissionItems(void) int MissionController::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); + MissionItem * newItem = new MissionItem(this); + newItem->setSequenceNumber(_missionItems->count()); + newItem->setCoordinate(coordinate); + newItem->setCommand(MAV_CMD_NAV_WAYPOINT); _initMissionItem(newItem); if (_missionItems->count() == 1) { newItem->setCommand(MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF); } + newItem->setDefaultsForCommand(); _missionItems->append(newItem); _recalcAll(); @@ -184,11 +177,6 @@ int MissionController::addMissionItem(QGeoCoordinate coordinate) void MissionController::removeMissionItem(int index) { - if (!_canEdit) { - qWarning() << "addMissionItem called with _canEdit == false"; - return; - } - MissionItem* item = qobject_cast(_missionItems->removeAt(index)); _deinitMissionItem(item); @@ -221,8 +209,6 @@ void MissionController::loadMissionFromFile(void) } _missionItems = new QmlObjectListModel(this); - _canEdit = true; - // FIXME: This needs to handle APM files which have WP 0 in them QFile file(filename); @@ -242,10 +228,6 @@ void MissionController::loadMissionFromFile(void) if (item->load(in)) { _missionItems->append(item); - - if (!item->canEdit()) { - _canEdit = false; - } } else { errorString = "The mission file is corrupted."; break; @@ -422,7 +404,9 @@ void MissionController::_initAllMissionItems(void) // Add the home position item to the front homeItem = new MissionItem(this); homeItem->setHomePositionSpecialCase(true); - homeItem->setCommand(MavlinkQmlSingleton::MAV_CMD_NAV_WAYPOINT); + homeItem->setCommand(MavlinkQmlSingleton::MAV_CMD_NAV_LAST); + homeItem->setFrame(MAV_FRAME_GLOBAL_RELATIVE_ALT); + homeItem->setSequenceNumber(0); _missionItems->insert(0, homeItem); } homeItem->setHomePositionValid(false); @@ -434,7 +418,6 @@ void MissionController::_initAllMissionItems(void) _recalcAll(); emit missionItemsChanged(); - emit canEditChanged(_canEdit); _missionItems->setDirty(false); diff --git a/src/MissionManager/MissionController.h b/src/MissionManager/MissionController.h index 939d5fa80..dd11398d0 100644 --- a/src/MissionManager/MissionController.h +++ b/src/MissionManager/MissionController.h @@ -42,7 +42,6 @@ public: 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) @@ -60,7 +59,6 @@ public: 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; } @@ -68,7 +66,6 @@ public: signals: void missionItemsChanged(void); - void canEditChanged(bool canEdit); void waypointLinesChanged(void); void liveHomePositionAvailableChanged(bool homePositionAvailable); void liveHomePositionChanged(const QGeoCoordinate& homePosition); @@ -103,7 +100,6 @@ 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 _liveHomePositionAvailable; QGeoCoordinate _liveHomePosition; diff --git a/src/MissionManager/MissionControllerManagerTest.cc b/src/MissionManager/MissionControllerManagerTest.cc index 4618e0700..b53ce51da 100644 --- a/src/MissionManager/MissionControllerManagerTest.cc +++ b/src/MissionManager/MissionControllerManagerTest.cc @@ -48,7 +48,6 @@ void MissionControllerManagerTest::_initForFirmwareType(MAV_AUTOPILOT firmwareTy _missionManager = qgcApp()->toolbox()->multiVehicleManager()->activeVehicle()->missionManager(); QVERIFY(_missionManager); - _rgMissionManagerSignals[canEditChangedSignalIndex] = SIGNAL(canEditChanged(bool)); _rgMissionManagerSignals[newMissionItemsAvailableSignalIndex] = SIGNAL(newMissionItemsAvailable(void)); _rgMissionManagerSignals[inProgressChangedSignalIndex] = SIGNAL(inProgressChanged(bool)); _rgMissionManagerSignals[errorSignalIndex] = SIGNAL(error(int, const QString&)); @@ -61,7 +60,6 @@ void MissionControllerManagerTest::_initForFirmwareType(MAV_AUTOPILOT firmwareTy _multiSpyMissionManager->waitForSignalByIndex(newMissionItemsAvailableSignalIndex, _missionManagerSignalWaitTime); _multiSpyMissionManager->waitForSignalByIndex(inProgressChangedSignalIndex, _missionManagerSignalWaitTime); QCOMPARE(_multiSpyMissionManager->checkSignalByMask(newMissionItemsAvailableSignalMask | inProgressChangedSignalMask), true); - QCOMPARE(_multiSpyMissionManager->checkNoSignalByMask(canEditChangedSignalIndex), true); } QVERIFY(!_missionManager->inProgress()); diff --git a/src/MissionManager/MissionControllerManagerTest.h b/src/MissionManager/MissionControllerManagerTest.h index 32230873c..c9f81e10e 100644 --- a/src/MissionManager/MissionControllerManagerTest.h +++ b/src/MissionManager/MissionControllerManagerTest.h @@ -49,16 +49,16 @@ protected: MissionManager* _missionManager; typedef struct { - int sequenceNumber; - QGeoCoordinate coordinate; - int command; - double param1; - double param2; - double param3; - double param4; - bool autocontinue; - bool isCurrentItem; - int frame; + int sequenceNumber; + QGeoCoordinate coordinate; + MAV_CMD command; + double param1; + double param2; + double param3; + double param4; + bool autocontinue; + bool isCurrentItem; + MAV_FRAME frame; } ItemInfo_t; typedef struct { @@ -67,15 +67,13 @@ protected: } TestCase_t; typedef enum { - canEditChangedSignalIndex = 0, - newMissionItemsAvailableSignalIndex, + newMissionItemsAvailableSignalIndex = 0, inProgressChangedSignalIndex, errorSignalIndex, maxSignalIndex } MissionManagerSignalIndex_t; typedef enum { - canEditChangedSignalMask = 1 << canEditChangedSignalIndex, newMissionItemsAvailableSignalMask = 1 << newMissionItemsAvailableSignalIndex, inProgressChangedSignalMask = 1 << inProgressChangedSignalIndex, errorSignalMask = 1 << errorSignalIndex, diff --git a/src/MissionManager/MissionItem.cc b/src/MissionManager/MissionItem.cc new file mode 100644 index 000000000..543ed6d5a --- /dev/null +++ b/src/MissionManager/MissionItem.cc @@ -0,0 +1,916 @@ +/*=================================================================== +QGroundControl Open Source Ground Control Station + +(c) 2009, 2010 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 +#include +#include +#include + +#include "MissionItem.h" +#include "FirmwarePluginManager.h" +#include "QGCApplication.h" + +QGC_LOGGING_CATEGORY(MissionItemLog, "MissionItemLog") + +const double MissionItem::defaultTakeoffPitch = 15.0; +const double MissionItem::defaultHeading = 0.0; +const double MissionItem::defaultAltitude = 25.0; +const double MissionItem::defaultAcceptanceRadius = 3.0; +const double MissionItem::defaultLoiterOrbitRadius = 10.0; +const double MissionItem::defaultLoiterTurns = 1.0; + +FactMetaData* MissionItem::_altitudeMetaData = NULL; +FactMetaData* MissionItem::_commandMetaData = NULL; +FactMetaData* MissionItem::_defaultParamMetaData = NULL; +FactMetaData* MissionItem::_frameMetaData = NULL; +FactMetaData* MissionItem::_latitudeMetaData = NULL; +FactMetaData* MissionItem::_longitudeMetaData = NULL; +FactMetaData* MissionItem::_supportedCommandMetaData = NULL; + +const QString MissionItem::_decimalPlacesJsonKey (QStringLiteral("decimalPlaces")); +const QString MissionItem::_descriptionJsonKey (QStringLiteral("description")); +const QString MissionItem::_friendlyEditJsonKey (QStringLiteral("friendlyEdit")); +const QString MissionItem::_friendlyNameJsonKey (QStringLiteral("friendlyName")); +const QString MissionItem::_idJsonKey (QStringLiteral("id")); +const QString MissionItem::_labelJsonKey (QStringLiteral("label")); +const QString MissionItem::_mavCmdInfoJsonKey (QStringLiteral("mavCmdInfo")); +const QString MissionItem::_param1JsonKey (QStringLiteral("param1")); +const QString MissionItem::_param2JsonKey (QStringLiteral("param2")); +const QString MissionItem::_param3JsonKey (QStringLiteral("param3")); +const QString MissionItem::_param4JsonKey (QStringLiteral("param4")); +const QString MissionItem::_paramJsonKeyFormat (QStringLiteral("param%1")); +const QString MissionItem::_rawNameJsonKey (QStringLiteral("rawName")); +const QString MissionItem::_specifiesCoordinateJsonKey (QStringLiteral("specifiesCoordinate")); +const QString MissionItem::_unitsJsonKey (QStringLiteral("units")); +const QString MissionItem::_versionJsonKey (QStringLiteral("version")); + +const QString MissionItem::_degreesUnits (QStringLiteral("degrees")); + +QMap MissionItem::_mavCmdInfoMap; + +struct EnumInfo_s { + const char * label; + MAV_FRAME frame; +}; + +static const struct EnumInfo_s _rgMavFrameInfo[] = { + { "MAV_FRAME_GLOBAL", MAV_FRAME_GLOBAL }, + { "MAV_FRAME_LOCAL_NED", MAV_FRAME_LOCAL_NED }, + { "MAV_FRAME_MISSION", MAV_FRAME_MISSION }, + { "MAV_FRAME_GLOBAL_RELATIVE_ALT", MAV_FRAME_GLOBAL_RELATIVE_ALT }, + { "MAV_FRAME_LOCAL_ENU", MAV_FRAME_LOCAL_ENU }, + { "MAV_FRAME_GLOBAL_INT", MAV_FRAME_GLOBAL_INT }, + { "MAV_FRAME_GLOBAL_RELATIVE_ALT_INT", MAV_FRAME_GLOBAL_RELATIVE_ALT_INT }, + { "MAV_FRAME_LOCAL_OFFSET_NED", MAV_FRAME_LOCAL_OFFSET_NED }, + { "MAV_FRAME_BODY_NED", MAV_FRAME_BODY_NED }, + { "MAV_FRAME_BODY_OFFSET_NED", MAV_FRAME_BODY_OFFSET_NED }, + { "MAV_FRAME_GLOBAL_TERRAIN_ALT", MAV_FRAME_GLOBAL_TERRAIN_ALT }, + { "MAV_FRAME_GLOBAL_TERRAIN_ALT_INT", MAV_FRAME_GLOBAL_TERRAIN_ALT_INT }, +}; + +QDebug operator<<(QDebug dbg, const MissionItem& missionItem) +{ + QDebugStateSaver saver(dbg); + dbg.nospace() << "MissionItem(" << missionItem.coordinate() << ")"; + + return dbg; +} + +QDebug operator<<(QDebug dbg, const MissionItem* missionItem) +{ + QDebugStateSaver saver(dbg); + dbg.nospace() << "MissionItem(" << missionItem->coordinate() << ")"; + + return dbg; +} + +MissionItem::MissionItem(QObject* parent) + : QObject(parent) + , _rawEdit(false) + , _dirty(false) + , _sequenceNumber(0) + , _isCurrentItem(false) + , _distance(0.0) + , _homePositionSpecialCase(false) + , _homePositionValid(false) + , _altitudeRelativeToHomeFact (0, "Altitude is relative to home", FactMetaData::valueTypeUint32) + , _autoContinueFact (0, "AutoContinue", FactMetaData::valueTypeUint32) + , _commandFact (0, "Command:", FactMetaData::valueTypeUint32) + , _frameFact (0, "Frame:", FactMetaData::valueTypeUint32) + , _param1Fact (0, "Param1:", FactMetaData::valueTypeDouble) + , _param2Fact (0, "Param2:", FactMetaData::valueTypeDouble) + , _param3Fact (0, "Param3:", FactMetaData::valueTypeDouble) + , _param4Fact (0, "Param4:", FactMetaData::valueTypeDouble) + , _param5Fact (0, "Latitude:", FactMetaData::valueTypeDouble) + , _param6Fact (0, "Longitude:", FactMetaData::valueTypeDouble) + , _param7Fact (0, "Altitude:", FactMetaData::valueTypeDouble) + , _supportedCommandFact (0, "Command:", FactMetaData::valueTypeUint32) + , _param1MetaData(FactMetaData::valueTypeDouble) + , _param2MetaData(FactMetaData::valueTypeDouble) + , _param3MetaData(FactMetaData::valueTypeDouble) + , _param4MetaData(FactMetaData::valueTypeDouble) + , _syncingAltitudeRelativeToHomeAndFrame (false) + , _syncingHeadingDegreesAndParam4 (false) + , _syncingSupportedCommandAndCommand (false) +{ + _setupMetaData(); + _connectSignals(); + + setAutoContinue(true); +} + +MissionItem::MissionItem(int sequenceNumber, + MAV_CMD command, + MAV_FRAME frame, + double param1, + double param2, + double param3, + double param4, + double param5, + double param6, + double param7, + bool autoContinue, + bool isCurrentItem, + QObject* parent) + : QObject(parent) + , _rawEdit(false) + , _dirty(false) + , _sequenceNumber(sequenceNumber) + , _isCurrentItem(isCurrentItem) + , _distance(0.0) + , _homePositionSpecialCase(false) + , _homePositionValid(false) + , _altitudeRelativeToHomeFact (0, "Altitude is relative to home", FactMetaData::valueTypeUint32) + , _commandFact (0, "Command:", FactMetaData::valueTypeUint32) + , _frameFact (0, "Frame:", FactMetaData::valueTypeUint32) + , _param1Fact (0, "Param1:", FactMetaData::valueTypeDouble) + , _param2Fact (0, "Param2:", FactMetaData::valueTypeDouble) + , _param3Fact (0, "Param3:", FactMetaData::valueTypeDouble) + , _param4Fact (0, "Param4:", FactMetaData::valueTypeDouble) + , _param5Fact (0, "Lat/X:", FactMetaData::valueTypeDouble) + , _param6Fact (0, "Lon/Y:", FactMetaData::valueTypeDouble) + , _param7Fact (0, "Alt/Z:", FactMetaData::valueTypeDouble) + , _supportedCommandFact (0, "Command:", FactMetaData::valueTypeUint32) + , _param1MetaData(FactMetaData::valueTypeDouble) + , _param2MetaData(FactMetaData::valueTypeDouble) + , _param3MetaData(FactMetaData::valueTypeDouble) + , _param4MetaData(FactMetaData::valueTypeDouble) + , _syncingAltitudeRelativeToHomeAndFrame (false) + , _syncingHeadingDegreesAndParam4 (false) + , _syncingSupportedCommandAndCommand (false) +{ + _setupMetaData(); + _connectSignals(); + + setCommand(command); + setFrame(frame); + setAutoContinue(autoContinue); + + _syncFrameToAltitudeRelativeToHome(); + _syncCommandToSupportedCommand(QVariant(this->command())); + + _param1Fact.setRawValue(param1); + _param2Fact.setRawValue(param2); + _param3Fact.setRawValue(param3); + _param4Fact.setRawValue(param4); + _param5Fact.setValue(param5); + _param6Fact.setValue(param6); + _param7Fact.setValue(param7); +} + +MissionItem::MissionItem(const MissionItem& other, QObject* parent) + : QObject(parent) + , _syncingAltitudeRelativeToHomeAndFrame (false) + , _syncingHeadingDegreesAndParam4 (false) + , _syncingSupportedCommandAndCommand (false) +{ + _setupMetaData(); + _connectSignals(); + + *this = other; +} + +const MissionItem& MissionItem::operator=(const MissionItem& other) +{ + setCommand(other.command()); + setFrame(other.frame()); + setRawEdit(other._rawEdit); + setDirty(other._dirty); + setSequenceNumber(other._sequenceNumber); + setAutoContinue(other.autoContinue()); + setIsCurrentItem(other._isCurrentItem); + setDistance(other._distance); + setHomePositionSpecialCase(other._homePositionSpecialCase); + setHomePositionValid(other._homePositionValid); + + _syncFrameToAltitudeRelativeToHome(); + _syncCommandToSupportedCommand(QVariant(this->command())); + + _param1Fact.setValue(other._param1Fact.value()); + _param2Fact.setValue(other._param2Fact.value()); + _param3Fact.setValue(other._param3Fact.value()); + _param4Fact.setValue(other._param4Fact.value()); + _param5Fact.setValue(other._param5Fact.value()); + _param6Fact.setValue(other._param6Fact.value()); + _param7Fact.setValue(other._param7Fact.value()); + + return *this; +} + +void MissionItem::_connectSignals(void) +{ + // Connect to change signals to track dirty state + connect(&_param1Fact, &Fact::valueChanged, this, &MissionItem::_setDirtyFromSignal); + connect(&_param2Fact, &Fact::valueChanged, this, &MissionItem::_setDirtyFromSignal); + connect(&_param3Fact, &Fact::valueChanged, this, &MissionItem::_setDirtyFromSignal); + connect(&_param4Fact, &Fact::valueChanged, this, &MissionItem::_setDirtyFromSignal); + connect(&_param5Fact, &Fact::valueChanged, this, &MissionItem::_setDirtyFromSignal); + connect(&_param6Fact, &Fact::valueChanged, this, &MissionItem::_setDirtyFromSignal); + connect(&_param7Fact, &Fact::valueChanged, this, &MissionItem::_setDirtyFromSignal); + connect(this, &MissionItem::commandChanged, this, &MissionItem::_setDirtyFromSignal); + connect(this, &MissionItem::frameChanged, this, &MissionItem::_setDirtyFromSignal); + connect(this, &MissionItem::sequenceNumberChanged, this, &MissionItem::_setDirtyFromSignal); + + // Values from these facts must propogate back and forth between the real object storage + connect(&_supportedCommandFact, &Fact::valueChanged, this, &MissionItem::_syncSupportedCommandToCommand); + connect(&_commandFact, &Fact::valueChanged, this, &MissionItem::_syncCommandToSupportedCommand); + connect(&_altitudeRelativeToHomeFact, &Fact::valueChanged, this, &MissionItem::_syncAltitudeRelativeToHomeToFrame); + connect(this, &MissionItem::frameChanged, this, &MissionItem::_syncFrameToAltitudeRelativeToHome); + + // These are parameter coordinates, they must emit coordinateChanged signal + connect(&_param5Fact, &Fact::valueChanged, this, &MissionItem::_sendCoordinateChanged); + connect(&_param6Fact, &Fact::valueChanged, this, &MissionItem::_sendCoordinateChanged); + connect(&_param7Fact, &Fact::valueChanged, this, &MissionItem::_sendCoordinateChanged); + + // The following changes may also change friendlyEditAllowed + connect(&_autoContinueFact, &Fact::valueChanged, this, &MissionItem::_sendFriendlyEditAllowedChanged); + connect(&_commandFact, &Fact::valueChanged, this, &MissionItem::_sendFriendlyEditAllowedChanged); + connect(this, &MissionItem::frameChanged, this, &MissionItem::_sendFriendlyEditAllowedChanged); + + // Whenever these properties change the ui model changes as well + connect(this, &MissionItem::commandChanged, this, &MissionItem::_sendUiModelChanged); + connect(this, &MissionItem::rawEditChanged, this, &MissionItem::_sendUiModelChanged); + + connect(&_commandFact, &Fact::valueChanged, this, &MissionItem::_sendCommandChanged); + connect(&_frameFact, &Fact::valueChanged, this, &MissionItem::_sendFrameChanged); +} + +bool MissionItem::_validateKeyTypes(QJsonObject& jsonObject, const QStringList& keys, const QList& types) +{ + for (int i=0; i types; + keys << _idJsonKey << _rawNameJsonKey << _friendlyNameJsonKey << _descriptionJsonKey << _specifiesCoordinateJsonKey << _friendlyEditJsonKey + << _param1JsonKey << _param2JsonKey << _param3JsonKey << _param4JsonKey; + types << QJsonValue::Double << QJsonValue::String << QJsonValue::String<< QJsonValue::String << QJsonValue::Bool << QJsonValue::Bool + << QJsonValue::Object << QJsonValue::Object << QJsonValue::Object << QJsonValue::Object; + if (!_validateKeyTypes(jsonObject, keys, types)) { + return false; + } + + MavCmdInfo_t mavCmdInfo; + + mavCmdInfo.command = (MAV_CMD) jsonObject.value(_idJsonKey).toInt(); + mavCmdInfo.rawName = jsonObject.value(_rawNameJsonKey).toString(); + mavCmdInfo.friendlyName = jsonObject.value(_friendlyNameJsonKey).toString(QString()); + mavCmdInfo.description = jsonObject.value(_descriptionJsonKey).toString(QString()); + mavCmdInfo.specifiesCoordinate = jsonObject.value(_specifiesCoordinateJsonKey).toBool(false); + mavCmdInfo.friendlyEdit = jsonObject.value(_friendlyEditJsonKey).toBool(false); + + if (_mavCmdInfoMap.contains(mavCmdInfo.command)) { + qWarning() << "Duplicate command" << mavCmdInfo.command; + return false; + } + + _mavCmdInfoMap[mavCmdInfo.command] = mavCmdInfo; + + // Read params + + for (int i=1; i<5; i++) { + QString paramKey = QString(_paramJsonKeyFormat).arg(i); + + if (jsonObject.contains(paramKey)) { + QJsonObject paramObject = jsonObject.value(paramKey).toObject(); + + // Validate key types + QStringList keys; + QList types; + keys << _labelJsonKey << _unitsJsonKey << _decimalPlacesJsonKey; + types << QJsonValue::String << QJsonValue::String<< QJsonValue::Double; + if (!_validateKeyTypes(paramObject, keys, types)) { + return false; + } + + if (paramObject.contains(_labelJsonKey)) { + _mavCmdInfoMap[mavCmdInfo.command].paramInfoMap[i].label = paramObject.value(_labelJsonKey).toString(); + } else { + qWarning() << "param object missing label key" << mavCmdInfo.rawName << paramKey; + return false; + } + + _mavCmdInfoMap[mavCmdInfo.command].paramInfoMap[i].units = paramObject.value(_unitsJsonKey).toString(); + _mavCmdInfoMap[mavCmdInfo.command].paramInfoMap[i].decimalPlaces = paramObject.value(_decimalPlacesJsonKey).toInt(FactMetaData::defaultDecimalPlaces); + } + } + } + + return true; +} + +void MissionItem::_setupMetaData(void) +{ + QStringList enumStrings; + QVariantList enumValues; + + if (!_altitudeMetaData) { + _loadMavCmdInfoJson(); + + _altitudeMetaData = new FactMetaData(FactMetaData::valueTypeDouble); + _altitudeMetaData->setUnits("meters"); + _altitudeMetaData->setDecimalPlaces(3); + + enumStrings.clear(); + enumValues.clear(); + foreach (MavCmdInfo_t mavCmdInfo, _mavCmdInfoMap) { + enumStrings.append(mavCmdInfo.rawName); + enumValues.append(QVariant(mavCmdInfo.command)); + } + _commandMetaData = new FactMetaData(FactMetaData::valueTypeUint32); + _commandMetaData->setEnumInfo(enumStrings, enumValues); + + _defaultParamMetaData = new FactMetaData(FactMetaData::valueTypeDouble); + _defaultParamMetaData->setDecimalPlaces(7); + + enumStrings.clear(); + enumValues.clear(); + for (size_t i=0; ilabel); + enumValues.append(QVariant(mavFrameInfo->frame)); + } + _frameMetaData = new FactMetaData(FactMetaData::valueTypeUint32); + _frameMetaData->setEnumInfo(enumStrings, enumValues); + + _latitudeMetaData = new FactMetaData(FactMetaData::valueTypeDouble); + _latitudeMetaData->setUnits("deg"); + _latitudeMetaData->setDecimalPlaces(7); + + _longitudeMetaData = new FactMetaData(FactMetaData::valueTypeDouble); + _longitudeMetaData->setUnits("deg"); + _longitudeMetaData->setDecimalPlaces(7); + + enumStrings.clear(); + enumValues.clear(); + // FIXME: Hack hardcode tp PX4 + QList supportedCommands = qgcApp()->toolbox()->firmwarePluginManager()->firmwarePluginForAutopilot(MAV_AUTOPILOT_PX4, MAV_TYPE_QUADROTOR)->supportedMissionCommands(); + if (supportedCommands.count()) { + foreach (MAV_CMD command, supportedCommands) { + enumStrings.append(_mavCmdInfoMap[command].friendlyName); + enumValues.append(QVariant(command)); + } + } else { + foreach (MavCmdInfo_t mavCmdInfo, _mavCmdInfoMap) { + enumStrings.append(mavCmdInfo.friendlyName); + enumValues.append(QVariant(mavCmdInfo.command)); + } + } + _supportedCommandMetaData = new FactMetaData(FactMetaData::valueTypeUint32); + _supportedCommandMetaData->setEnumInfo(enumStrings, enumValues); + } + + _commandFact.setMetaData(_commandMetaData); + _frameFact.setMetaData(_frameMetaData); + _supportedCommandFact.setMetaData(_supportedCommandMetaData); +} + +MissionItem::~MissionItem() +{ +} + +void MissionItem::save(QTextStream &saveStream) +{ + // FORMAT: + // as documented here: http://qgroundcontrol.org/waypoint_protocol + saveStream << sequenceNumber() << "\t" + << isCurrentItem() << "\t" + << frame() << "\t" + << command() << "\t" + << QString("%1").arg(param1(), 0, 'g', 18) << "\t" + << QString("%1").arg(param2(), 0, 'g', 18) << "\t" + << QString("%1").arg(param3(), 0, 'g', 18) << "\t" + << QString("%1").arg(param4(), 0, 'g', 18) << "\t" + << QString("%1").arg(param5(), 0, 'g', 18) << "\t" + << QString("%1").arg(param6(), 0, 'g', 18) << "\t" + << QString("%1").arg(param7(), 0, 'g', 18) << "\t" + << this->autoContinue() << "\r\n"; +} + +bool MissionItem::load(QTextStream &loadStream) +{ + const QStringList &wpParams = loadStream.readLine().split("\t"); + if (wpParams.size() == 12) { + setSequenceNumber(wpParams[0].toInt()); + setIsCurrentItem(wpParams[1].toInt() == 1 ? true : false); + setFrame((MAV_FRAME)wpParams[2].toInt()); + setCommand((MAV_CMD)wpParams[3].toInt()); + setParam1(wpParams[4].toDouble()); + setParam2(wpParams[5].toDouble()); + setParam3(wpParams[6].toDouble()); + setParam4(wpParams[7].toDouble()); + setParam5(wpParams[8].toDouble()); + setParam6(wpParams[9].toDouble()); + setParam7(wpParams[10].toDouble()); + setAutoContinue(wpParams[11].toInt() == 1 ? true : false); + return true; + } + return false; +} + + +void MissionItem::setSequenceNumber(int sequenceNumber) +{ + _sequenceNumber = sequenceNumber; + emit sequenceNumberChanged(_sequenceNumber); +} + +void MissionItem::setCommand(MAV_CMD command) +{ + if ((MAV_CMD)this->command() != command) { + _commandFact.setValue(command); + emit commandChanged(this->command()); + } +} + +void MissionItem::setCommand(MavlinkQmlSingleton::Qml_MAV_CMD command) +{ + setCommand((MAV_CMD)command); +} + + +void MissionItem::setFrame(MAV_FRAME frame) +{ + if (this->frame() != frame) { + _frameFact.setValue(frame); + } +} + +void MissionItem::setAutoContinue(bool autoContinue) +{ + if (this->autoContinue() != autoContinue) { + _autoContinueFact.setValue(autoContinue); + } +} + +void MissionItem::setIsCurrentItem(bool isCurrentItem) +{ + if (_isCurrentItem != isCurrentItem) { + _isCurrentItem = isCurrentItem; + emit isCurrentItemChanged(isCurrentItem); + } +} + +void MissionItem::setParam1(double param) +{ + if (param1() != param) { + _param1Fact.setRawValue(param); + } +} + +void MissionItem::setParam2(double param) +{ + if (param2() != param) { + _param2Fact.setRawValue(param); + } +} + +void MissionItem::setParam3(double param) +{ + if (param3() != param) { + _param3Fact.setRawValue(param); + } +} + +void MissionItem::setParam4(double param) +{ + if (param4() != param) { + _param4Fact.setRawValue(param); + } +} + +void MissionItem::setParam5(double param) +{ + if (param5() != param) { + _param5Fact.setRawValue(param); + } +} + +void MissionItem::setParam6(double param) +{ + if (param6() != param) { + _param6Fact.setRawValue(param); + } +} + +void MissionItem::setParam7(double param) +{ + if (param7() != param) { + _param7Fact.setRawValue(param); + } +} + +bool MissionItem::specifiesCoordinate(void) const +{ + return _mavCmdInfoMap[(MAV_CMD)command()].specifiesCoordinate; +} + +QString MissionItem::commandDescription(void) const +{ + return _mavCmdInfoMap[(MAV_CMD)command()].description; +} + +void MissionItem::_clearParamMetaData(void) +{ + _param1MetaData.setUnits(""); + _param1MetaData.setDecimalPlaces(FactMetaData::defaultDecimalPlaces); + _param1MetaData.setTranslators(FactMetaData::defaultTranslator, FactMetaData::defaultTranslator); + _param2MetaData.setUnits(""); + _param2MetaData.setDecimalPlaces(FactMetaData::defaultDecimalPlaces); + _param2MetaData.setTranslators(FactMetaData::defaultTranslator, FactMetaData::defaultTranslator); + _param3MetaData.setUnits(""); + _param3MetaData.setDecimalPlaces(FactMetaData::defaultDecimalPlaces); + _param3MetaData.setTranslators(FactMetaData::defaultTranslator, FactMetaData::defaultTranslator); + _param4MetaData.setUnits(""); + _param4MetaData.setDecimalPlaces(FactMetaData::defaultDecimalPlaces); + _param4MetaData.setTranslators(FactMetaData::defaultTranslator, FactMetaData::defaultTranslator); +} + +QmlObjectListModel* MissionItem::textFieldFacts(void) +{ + QmlObjectListModel* model = new QmlObjectListModel(this); + + if (rawEdit()) { + _param1Fact._setName("Param1:"); + _param1Fact.setMetaData(_defaultParamMetaData); + model->append(&_param1Fact); + _param2Fact._setName("Param2:"); + _param2Fact.setMetaData(_defaultParamMetaData); + model->append(&_param2Fact); + _param3Fact._setName("Param3:"); + _param3Fact.setMetaData(_defaultParamMetaData); + model->append(&_param3Fact); + _param4Fact._setName("Param4:"); + _param4Fact.setMetaData(_defaultParamMetaData); + model->append(&_param4Fact); + _param5Fact._setName("Lat/X:"); + _param5Fact.setMetaData(_defaultParamMetaData); + model->append(&_param5Fact); + _param6Fact._setName("Lon/Y:"); + _param6Fact.setMetaData(_defaultParamMetaData); + model->append(&_param6Fact); + _param7Fact._setName("Alt/Z:"); + _param7Fact.setMetaData(_defaultParamMetaData); + model->append(&_param7Fact); + } else { + _clearParamMetaData(); + + MAV_CMD command = (MAV_CMD)this->command(); + + if (_mavCmdInfoMap[command].paramInfoMap.contains(1)) { + _param1Fact._setName(_mavCmdInfoMap[command].paramInfoMap[1].label); + _param1MetaData.setUnits(_mavCmdInfoMap[command].paramInfoMap[1].units); + _param1MetaData.setDecimalPlaces(_mavCmdInfoMap[command].paramInfoMap[1].decimalPlaces); + if (_mavCmdInfoMap[command].paramInfoMap[1].units == _degreesUnits) { + _param1MetaData.setTranslators(_radiansToDegrees, _degreesToRadians); + } + _param1Fact.setMetaData(&_param1MetaData); + model->append(&_param1Fact); + } + + if (_mavCmdInfoMap[command].paramInfoMap.contains(2)) { + _param2Fact._setName(_mavCmdInfoMap[command].paramInfoMap[2].label); + _param2MetaData.setUnits(_mavCmdInfoMap[command].paramInfoMap[2].units); + _param2MetaData.setDecimalPlaces(_mavCmdInfoMap[command].paramInfoMap[2].decimalPlaces); + if (_mavCmdInfoMap[command].paramInfoMap[2].units == _degreesUnits) { + _param2MetaData.setTranslators(_radiansToDegrees, _degreesToRadians); + } + _param2Fact.setMetaData(&_param2MetaData); + model->append(&_param2Fact); + } + + if (_mavCmdInfoMap[command].paramInfoMap.contains(3)) { + _param3Fact._setName(_mavCmdInfoMap[command].paramInfoMap[3].label); + _param3MetaData.setUnits(_mavCmdInfoMap[command].paramInfoMap[3].units); + _param3MetaData.setDecimalPlaces(_mavCmdInfoMap[command].paramInfoMap[3].decimalPlaces); + if (_mavCmdInfoMap[command].paramInfoMap[3].units == _degreesUnits) { + _param3MetaData.setTranslators(_radiansToDegrees, _degreesToRadians); + } + _param3Fact.setMetaData(&_param3MetaData); + model->append(&_param3Fact); + } + + if (_mavCmdInfoMap[command].paramInfoMap.contains(4)) { + _param4Fact._setName(_mavCmdInfoMap[command].paramInfoMap[4].label); + _param4MetaData.setUnits(_mavCmdInfoMap[command].paramInfoMap[4].units); + _param4MetaData.setDecimalPlaces(_mavCmdInfoMap[command].paramInfoMap[4].decimalPlaces); + if (_mavCmdInfoMap[command].paramInfoMap[4].units == _degreesUnits) { + _param4MetaData.setTranslators(_radiansToDegrees, _degreesToRadians); + } + _param4Fact.setMetaData(&_param4MetaData); + model->append(&_param4Fact); + } + + if (specifiesCoordinate()) { + _param7Fact._setName("Altitude:"); + _param7Fact.setMetaData(_altitudeMetaData); + model->append(&_param7Fact); + } + } + + return model; +} + +QmlObjectListModel* MissionItem::checkboxFacts(void) +{ + QmlObjectListModel* model = new QmlObjectListModel(this); + + + if (rawEdit()) { + model->append(&_autoContinueFact); + } else if (specifiesCoordinate()) { + model->append(&_altitudeRelativeToHomeFact); + } + + return model; +} + +QmlObjectListModel* MissionItem::comboboxFacts(void) +{ + QmlObjectListModel* model = new QmlObjectListModel(this); + + if (rawEdit()) { + model->append(&_commandFact); + model->append(&_frameFact); + } + + return model; +} + +QGeoCoordinate MissionItem::coordinate(void) const +{ + return QGeoCoordinate(_param5Fact.value().toDouble(), _param6Fact.value().toDouble(), _param7Fact.value().toDouble()); +} + +void MissionItem::setCoordinate(const QGeoCoordinate& coordinate) +{ + setParam5(coordinate.latitude()); + setParam6(coordinate.longitude()); + setParam7(coordinate.altitude()); +} + +bool MissionItem::friendlyEditAllowed(void) const +{ + if (_mavCmdInfoMap[(MAV_CMD)command()].friendlyEdit) { + if (!autoContinue()) { + return false; + } + + if (specifiesCoordinate()) { + return frame() == MAV_FRAME_GLOBAL || frame() == MAV_FRAME_GLOBAL_RELATIVE_ALT; + } else { + return frame() == MAV_FRAME_MISSION; + } + } + + return false; +} + +bool MissionItem::rawEdit(void) const +{ + return _rawEdit || !friendlyEditAllowed(); +} + +void MissionItem::setRawEdit(bool rawEdit) +{ + if (_rawEdit != rawEdit) { + _rawEdit = rawEdit; + emit rawEditChanged(this->rawEdit()); + } +} + +void MissionItem::setDirty(bool dirty) +{ + if (!_homePositionSpecialCase || !dirty) { + // Home position never affects dirty bit + + _dirty = dirty; + // We want to emit dirtyChanged even if _dirty didn't change. This can be handy signal for + // any value within the item changing. + emit dirtyChanged(_dirty); + } +} + +void MissionItem::_setDirtyFromSignal(void) +{ + setDirty(true); +} + +void MissionItem::setHomePositionValid(bool homePositionValid) +{ + _homePositionValid = homePositionValid; + emit homePositionValidChanged(_homePositionValid); +} + +void MissionItem::setDistance(double distance) +{ + _distance = distance; + emit distanceChanged(_distance); +} + +void MissionItem::_sendCoordinateChanged(void) +{ + emit coordinateChanged(coordinate()); +} + +void MissionItem::_syncAltitudeRelativeToHomeToFrame(const QVariant& value) +{ + if (!_syncingAltitudeRelativeToHomeAndFrame) { + _syncingAltitudeRelativeToHomeAndFrame = true; + setFrame(value.toBool() ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_GLOBAL); + _syncingAltitudeRelativeToHomeAndFrame = false; + } +} + +void MissionItem::_syncFrameToAltitudeRelativeToHome(void) +{ + if (!_syncingAltitudeRelativeToHomeAndFrame) { + _syncingAltitudeRelativeToHomeAndFrame = true; + _altitudeRelativeToHomeFact.setValue(relativeAltitude()); + _syncingAltitudeRelativeToHomeAndFrame = false; + } +} + +void MissionItem::_syncSupportedCommandToCommand(const QVariant& value) +{ + if (!_syncingSupportedCommandAndCommand) { + qDebug() << value.toInt(); + _syncingSupportedCommandAndCommand = true; + _commandFact.setValue(value.toInt()); + qDebug() << _commandFact.value().toInt(); + _syncingSupportedCommandAndCommand = false; + } +} + +void MissionItem::_syncCommandToSupportedCommand(const QVariant& value) +{ + if (!_syncingSupportedCommandAndCommand) { + _syncingSupportedCommandAndCommand = true; + _supportedCommandFact.setValue(value.toInt()); + _syncingSupportedCommandAndCommand = false; + } +} + +void MissionItem::setDefaultsForCommand(void) +{ + switch ((MAV_CMD)command()) { + case MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF: + setParam1(_degreesToRadians(defaultTakeoffPitch).toDouble()); + break; + case MavlinkQmlSingleton::MAV_CMD_NAV_WAYPOINT: + setParam2(defaultAcceptanceRadius); + break; + case MavlinkQmlSingleton::MAV_CMD_NAV_LOITER_UNLIM: + case MavlinkQmlSingleton::MAV_CMD_NAV_LOITER_TIME: + setParam3(defaultLoiterOrbitRadius); + break; + case MavlinkQmlSingleton::MAV_CMD_NAV_LOITER_TURNS: + setParam1(defaultLoiterTurns); + setParam3(defaultLoiterOrbitRadius); + break; + default: + break; + } + + setParam4(_degreesToRadians(defaultHeading).toDouble()); + setParam7(defaultAltitude); + + setFrame(specifiesCoordinate() ? MAV_FRAME_GLOBAL_RELATIVE_ALT : MAV_FRAME_MISSION); +} + +void MissionItem::_sendUiModelChanged(void) +{ + emit uiModelChanged(); +} + +void MissionItem::_sendFrameChanged(void) +{ + emit frameChanged(frame()); +} + +void MissionItem::_sendCommandChanged(void) +{ + emit commandChanged(command()); +} + +QString MissionItem::commandName(void) const +{ + MavCmdInfo_t& mavCmdInfo = _mavCmdInfoMap[(MAV_CMD)command()]; + + return mavCmdInfo.friendlyName.isEmpty() ? mavCmdInfo.rawName : mavCmdInfo.friendlyName; +} + +QVariant MissionItem::_degreesToRadians(const QVariant& degrees) +{ + return QVariant(degrees.toDouble() * (M_PI / 180.0)); +} + +QVariant MissionItem::_radiansToDegrees(const QVariant& radians) +{ + return QVariant(radians.toDouble() * (180 / M_PI)); +} + +void MissionItem::_sendFriendlyEditAllowedChanged(void) +{ + emit friendlyEditAllowedChanged(friendlyEditAllowed()); +} diff --git a/src/MissionManager/MissionItem.h b/src/MissionManager/MissionItem.h new file mode 100644 index 000000000..9a1589f89 --- /dev/null +++ b/src/MissionManager/MissionItem.h @@ -0,0 +1,294 @@ +/*===================================================================== + + QGroundControl Open Source Ground Control Station + + (c) 2009 - 2014 QGROUNDCONTROL PROJECT + + This file is part of the QGROUNDCONTROL project + + QGROUNDCONTROL is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + QGROUNDCONTROL is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with QGROUNDCONTROL. If not, see . + + ======================================================================*/ + +#ifndef MissionItem_H +#define MissionItem_H + +#include +#include +#include +#include +#include +#include +#include + +#include "QGCMAVLink.h" +#include "QGC.h" +#include "MavlinkQmlSingleton.h" +#include "QmlObjectListModel.h" +#include "Fact.h" +#include "QGCLoggingCategory.h" +#include "QmlObjectListModel.h" + +Q_DECLARE_LOGGING_CATEGORY(MissionItemLog) + +class MissionItem : public QObject +{ + Q_OBJECT + +public: + MissionItem(QObject* parent = NULL); + + MissionItem(int sequenceNumber, + MAV_CMD command, + MAV_FRAME frame, + double param1, + double param2, + double param3, + double param4, + double param5, + double param6, + double param7, + bool autoContinue, + bool isCurrentItem, + QObject* parent = NULL); + + MissionItem(const MissionItem& other, QObject* parent = NULL); + + ~MissionItem(); + + const MissionItem& operator=(const MissionItem& other); + + Q_PROPERTY(MavlinkQmlSingleton::Qml_MAV_CMD command READ command WRITE setCommand NOTIFY commandChanged) + Q_PROPERTY(QString commandDescription READ commandDescription NOTIFY commandChanged) + Q_PROPERTY(QString commandName READ commandName NOTIFY commandChanged) + Q_PROPERTY(QGeoCoordinate coordinate READ coordinate WRITE setCoordinate NOTIFY coordinateChanged) + Q_PROPERTY(bool dirty READ dirty WRITE setDirty NOTIFY dirtyChanged) + Q_PROPERTY(double distance READ distance WRITE setDistance NOTIFY distanceChanged) ///< Distance to previous waypoint + Q_PROPERTY(bool friendlyEditAllowed READ friendlyEditAllowed NOTIFY friendlyEditAllowedChanged) + Q_PROPERTY(bool homePosition READ homePosition CONSTANT) ///< true: This item is being used as a home position indicator + Q_PROPERTY(bool homePositionValid READ homePositionValid WRITE setHomePositionValid NOTIFY homePositionValidChanged) ///< true: Home position should be shown + Q_PROPERTY(bool isCurrentItem READ isCurrentItem WRITE setIsCurrentItem NOTIFY isCurrentItemChanged) + Q_PROPERTY(bool rawEdit READ rawEdit WRITE setRawEdit NOTIFY rawEditChanged) ///< true: raw item editing with all params + Q_PROPERTY(int sequenceNumber READ sequenceNumber WRITE setSequenceNumber NOTIFY sequenceNumberChanged) + Q_PROPERTY(bool specifiesCoordinate READ specifiesCoordinate NOTIFY commandChanged) + Q_PROPERTY(Fact* supportedCommand READ supportedCommand NOTIFY commandChanged) + + // These properties are used to display the editing ui + Q_PROPERTY(QmlObjectListModel* checkboxFacts READ checkboxFacts NOTIFY uiModelChanged) + Q_PROPERTY(QmlObjectListModel* comboboxFacts READ comboboxFacts NOTIFY uiModelChanged) + Q_PROPERTY(QmlObjectListModel* textFieldFacts READ textFieldFacts NOTIFY uiModelChanged) + + /// List of child mission items. Child mission item are subsequent mision items which do not specify a coordinate. They + /// are shown next to the part item in the ui. + Q_PROPERTY(QmlObjectListModel* childItems READ childItems CONSTANT) + + Q_INVOKABLE void setDefaultsForCommand(void); + + // Property accesors + + MavlinkQmlSingleton::Qml_MAV_CMD command(void) const { return (MavlinkQmlSingleton::Qml_MAV_CMD)_commandFact.value().toInt(); }; + QString commandDescription (void) const; + QString commandName (void) const; + QGeoCoordinate coordinate (void) const; + bool dirty (void) const { return _dirty; } + double distance (void) const { return _distance; } + bool friendlyEditAllowed (void) const; + bool homePosition (void) const { return _homePositionSpecialCase; } + bool homePositionValid (void) const { return _homePositionValid; } + bool isCurrentItem (void) const { return _isCurrentItem; } + bool rawEdit (void) const; + int sequenceNumber (void) const { return _sequenceNumber; } + bool specifiesCoordinate (void) const; + Fact* supportedCommand (void) { return &_supportedCommandFact; } + + + QmlObjectListModel* textFieldFacts (void); + QmlObjectListModel* checkboxFacts (void); + QmlObjectListModel* comboboxFacts (void); + QmlObjectListModel* childItems (void) { return &_childItems; } + + void setRawEdit(bool rawEdit); + void setDirty(bool dirty); + void setSequenceNumber(int sequenceNumber); + + void setIsCurrentItem(bool isCurrentItem); + + void setCoordinate(const QGeoCoordinate& coordinate); + + void setCommandByIndex(int index); + + void setCommand(MavlinkQmlSingleton::Qml_MAV_CMD command); + + void setHomePositionValid(bool homePositionValid); + void setHomePositionSpecialCase(bool homePositionSpecialCase) { _homePositionSpecialCase = homePositionSpecialCase; } + + void setDistance(double distance); + + // C++ only methods + + MAV_FRAME frame (void) const { return (MAV_FRAME)_frameFact.value().toInt(); } + bool autoContinue(void) const { return _autoContinueFact.value().toBool(); } + double param1 (void) const { return _param1Fact.rawValue().toDouble(); } + double param2 (void) const { return _param2Fact.rawValue().toDouble(); } + double param3 (void) const { return _param3Fact.rawValue().toDouble(); } + double param4 (void) const { return _param4Fact.rawValue().toDouble(); } + double param5 (void) const { return _param5Fact.rawValue().toDouble(); } + double param6 (void) const { return _param6Fact.rawValue().toDouble(); } + double param7 (void) const { return _param7Fact.rawValue().toDouble(); } + + void setCommand (MAV_CMD command); + void setFrame (MAV_FRAME frame); + void setAutoContinue(bool autoContinue); + void setParam1 (double param1); + void setParam2 (double param2); + void setParam3 (double param3); + void setParam4 (double param4); + void setParam5 (double param5); + void setParam6 (double param6); + void setParam7 (double param7); + + // C++ only methods + + void save(QTextStream &saveStream); + bool load(QTextStream &loadStream); + + bool relativeAltitude(void) { return frame() == MAV_FRAME_GLOBAL_RELATIVE_ALT; } + + static const double defaultTakeoffPitch; + static const double defaultHeading; + static const double defaultAltitude; + static const double defaultAcceptanceRadius; + static const double defaultLoiterOrbitRadius; + static const double defaultLoiterTurns; + +signals: + void commandChanged (MavlinkQmlSingleton::Qml_MAV_CMD command); + void coordinateChanged (const QGeoCoordinate& coordinate); + void dirtyChanged (bool dirty); + void distanceChanged (float distance); + void frameChanged (int frame); + void friendlyEditAllowedChanged (bool friendlyEditAllowed); + void headingDegreesChanged (double heading); + void homePositionValidChanged (bool homePostionValid); + void isCurrentItemChanged (bool isCurrentItem); + void rawEditChanged (bool rawEdit); + void sequenceNumberChanged (int sequenceNumber); + void uiModelChanged (void); + +private slots: + void _setDirtyFromSignal(void); + void _sendCommandChanged(void); + void _sendCoordinateChanged(void); + void _sendFrameChanged(void); + void _sendFriendlyEditAllowedChanged(void); + void _sendUiModelChanged(void); + void _syncAltitudeRelativeToHomeToFrame(const QVariant& value); + void _syncCommandToSupportedCommand(const QVariant& value); + void _syncFrameToAltitudeRelativeToHome(void); + void _syncSupportedCommandToCommand(const QVariant& value); + +private: + void _clearParamMetaData(void); + void _connectSignals(void); + bool _loadMavCmdInfoJson(void); + void _setupMetaData(void); + bool _validateKeyTypes(QJsonObject& jsonObject, const QStringList& keys, const QList& types); + + static QVariant _degreesToRadians(const QVariant& degrees); + static QVariant _radiansToDegrees(const QVariant& radians); + +private: + typedef struct { + QString label; + QString units; + int decimalPlaces; + } ParamInfo_t; + + typedef struct { + MAV_CMD command; + QString rawName; + QString friendlyName; + QString description; + bool specifiesCoordinate; + bool friendlyEdit; + QMap paramInfoMap; + } MavCmdInfo_t; + + bool _rawEdit; + bool _dirty; + int _sequenceNumber; + bool _isCurrentItem; + double _distance; ///< Distance to previous waypoint + bool _homePositionSpecialCase; ///< true: This item is being used as a ui home position indicator + bool _homePositionValid; ///< true: Home psition should be displayed + + Fact _altitudeRelativeToHomeFact; + Fact _autoContinueFact; + Fact _commandFact; + Fact _frameFact; + Fact _param1Fact; + Fact _param2Fact; + Fact _param3Fact; + Fact _param4Fact; + Fact _param5Fact; + Fact _param6Fact; + Fact _param7Fact; + Fact _supportedCommandFact; + + static FactMetaData* _altitudeMetaData; + static FactMetaData* _commandMetaData; + static FactMetaData* _defaultParamMetaData; + static FactMetaData* _frameMetaData; + static FactMetaData* _latitudeMetaData; + static FactMetaData* _longitudeMetaData; + static FactMetaData* _supportedCommandMetaData; + + FactMetaData _param1MetaData; + FactMetaData _param2MetaData; + FactMetaData _param3MetaData; + FactMetaData _param4MetaData; + + /// This is used to reference any subsequent mission items which do not specify a coordinate. + QmlObjectListModel _childItems; + + bool _syncingAltitudeRelativeToHomeAndFrame; ///< true: already in a sync signal, prevents signal loop + bool _syncingHeadingDegreesAndParam4; ///< true: already in a sync signal, prevents signal loop + bool _syncingSupportedCommandAndCommand; ///< true: already in a sync signal, prevents signal loop + + static QMap _mavCmdInfoMap; + + static const QString _decimalPlacesJsonKey; + static const QString _descriptionJsonKey; + static const QString _friendlyNameJsonKey; + static const QString _friendlyEditJsonKey; + static const QString _idJsonKey; + static const QString _labelJsonKey; + static const QString _mavCmdInfoJsonKey; + static const QString _param1JsonKey; + static const QString _param2JsonKey; + static const QString _param3JsonKey; + static const QString _param4JsonKey; + static const QString _paramJsonKeyFormat; + static const QString _rawNameJsonKey; + static const QString _specifiesCoordinateJsonKey; + static const QString _unitsJsonKey; + static const QString _versionJsonKey; + + static const QString _degreesUnits; +}; + +QDebug operator<<(QDebug dbg, const MissionItem& missionItem); +QDebug operator<<(QDebug dbg, const MissionItem* missionItem); + +#endif diff --git a/src/MissionItemTest.cc b/src/MissionManager/MissionItemTest.cc similarity index 92% rename from src/MissionItemTest.cc rename to src/MissionManager/MissionItemTest.cc index 378d2da29..58f5bca42 100644 --- a/src/MissionItemTest.cc +++ b/src/MissionManager/MissionItemTest.cc @@ -54,11 +54,12 @@ const MissionItemTest::FactValue_t MissionItemTest::_rgFactValuesLoiterTurns[] = const MissionItemTest::FactValue_t MissionItemTest::_rgFactValuesLoiterTime[] = { { "Altitude:", -30.0 }, { "Radius:", 30.0 }, - { "Seconds:", 10.0 }, + { "Hold:", 10.0 }, }; const MissionItemTest::FactValue_t MissionItemTest::_rgFactValuesLand[] = { { "Altitude:", -30.0 }, + { "Abort Alt:", 10.0 }, { "Heading:", 1.0 }, }; @@ -69,7 +70,7 @@ const MissionItemTest::FactValue_t MissionItemTest::_rgFactValuesTakeoff[] = { }; const MissionItemTest::FactValue_t MissionItemTest::_rgFactValuesConditionDelay[] = { - { "Seconds:", 10.0 }, + { "Hold:", 10.0 }, }; const MissionItemTest::FactValue_t MissionItemTest::_rgFactValuesDoJump[] = { @@ -101,18 +102,19 @@ void MissionItemTest::_test(void) qDebug() << "Command:" << info->command; - MissionItem* item = new MissionItem(NULL, - info->sequenceNumber, - info->coordinate, + MissionItem* item = new MissionItem(info->sequenceNumber, info->command, + info->frame, info->param1, info->param2, info->param3, info->param4, + info->coordinate.latitude(), + info->coordinate.longitude(), + info->coordinate.altitude(), info->autocontinue, - info->isCurrentItem, - info->frame); - + info->isCurrentItem); + // Validate the saving is working correctly QString savedItemString; QTextStream saveStream(&savedItemString, QIODevice::WriteOnly); @@ -123,6 +125,7 @@ void MissionItemTest::_test(void) size_t factCount = 0; for (int i=0; itextFieldFacts()->count(); i++) { Fact* fact = qobject_cast(item->textFieldFacts()->get(i)); + qDebug() << fact->name(); bool found = false; for (size_t j=0; jcFactValues; j++) { @@ -130,11 +133,7 @@ void MissionItemTest::_test(void) if (factValue->name == fact->name()) { qDebug() << factValue->name; - if (strcmp(factValue->name, "Heading:") == 0) { - QCOMPARE(fact->value().toDouble() * (M_PI / 180.0), item->_yawRadians()); - } else { - QCOMPARE(fact->value().toDouble(), factValue->value); - } + QCOMPARE(fact->rawValue().toDouble(), factValue->value); factCount ++; found = true; break; diff --git a/src/MissionItemTest.h b/src/MissionManager/MissionItemTest.h similarity index 97% rename from src/MissionItemTest.h rename to src/MissionManager/MissionItemTest.h index ec91ec27d..ca400da08 100644 --- a/src/MissionItemTest.h +++ b/src/MissionManager/MissionItemTest.h @@ -49,14 +49,14 @@ private: typedef struct { int sequenceNumber; QGeoCoordinate coordinate; - int command; + MAV_CMD command; double param1; double param2; double param3; double param4; bool autocontinue; bool isCurrentItem; - int frame; + MAV_FRAME frame; } ItemInfo_t; typedef struct { diff --git a/src/MissionManager/MissionManager.cc b/src/MissionManager/MissionManager.cc index a9af8316c..faea0d813 100644 --- a/src/MissionManager/MissionManager.cc +++ b/src/MissionManager/MissionManager.cc @@ -35,7 +35,6 @@ QGC_LOGGING_CATEGORY(MissionManagerLog, "MissionManagerLog") MissionManager::MissionManager(Vehicle* vehicle) : _vehicle(vehicle) , _cMissionItems(0) - , _canEdit(true) , _ackTimeoutTimer(NULL) , _retryAck(AckNone) { @@ -287,24 +286,21 @@ void MissionManager::_handleMissionItem(const mavlink_message_t& message) return; } - MissionItem* item = new MissionItem(this, - missionItem.seq, - QGeoCoordinate(missionItem.x, missionItem.y, missionItem.z), - missionItem.command, + MissionItem* item = new MissionItem(missionItem.seq, + (MAV_CMD)missionItem.command, + (MAV_FRAME)missionItem.frame, missionItem.param1, missionItem.param2, missionItem.param3, missionItem.param4, + missionItem.x, + missionItem.y, + missionItem.z, missionItem.autocontinue, missionItem.current, - missionItem.frame); + this); _missionItems.append(item); - if (!item->canEdit()) { - _canEdit = false; - emit canEditChanged(false); - } - int nextSequenceNumber = missionItem.seq + 1; if (nextSequenceNumber == _cMissionItems) { _readTransactionComplete(); diff --git a/src/MissionManager/MissionManager.h b/src/MissionManager/MissionManager.h index ab9c83ae5..43a23909e 100644 --- a/src/MissionManager/MissionManager.h +++ b/src/MissionManager/MissionManager.h @@ -49,13 +49,11 @@ public: Q_PROPERTY(bool inProgress READ inProgress NOTIFY inProgressChanged) Q_PROPERTY(QmlObjectListModel* missionItems READ missionItems CONSTANT) - Q_PROPERTY(bool canEdit READ canEdit NOTIFY canEditChanged) // Property accessors bool inProgress(void) { return _retryAck != AckNone; } QmlObjectListModel* missionItems(void) { return &_missionItems; } - bool canEdit(void) { return _canEdit; } // C++ methods @@ -85,8 +83,6 @@ public: static const int _maxRetryCount = 5; signals: - // Public signals - void canEditChanged(bool canEdit); void newMissionItemsAvailable(void); void inProgressChanged(bool inProgress); void error(int errorCode, const QString& errorMsg); @@ -123,7 +119,6 @@ private: Vehicle* _vehicle; int _cMissionItems; ///< Mission items on vehicle - bool _canEdit; ///< true: Mission items are editable in the ui QTimer* _ackTimeoutTimer; AckType_t _retryAck; diff --git a/src/MissionManager/MissionManagerTest.cc b/src/MissionManager/MissionManagerTest.cc index ec3bbd812..cbfbaddac 100644 --- a/src/MissionManager/MissionManagerTest.cc +++ b/src/MissionManager/MissionManagerTest.cc @@ -58,8 +58,7 @@ void MissionManagerTest::_writeItems(MockLinkMissionItemHandler::FailureMode_t f homeItem->setHomePositionSpecialCase(true); homeItem->setHomePositionValid(false); homeItem->setCommand(MavlinkQmlSingleton::MAV_CMD_NAV_WAYPOINT); - homeItem->setLatitude(47.3769); - homeItem->setLongitude(8.549444); + homeItem->setCoordinate(QGeoCoordinate(47.3769, 8.549444, 0)); homeItem->setSequenceNumber(0); list->insert(0, homeItem); @@ -137,8 +136,6 @@ void MissionManagerTest::_writeItems(MockLinkMissionItemHandler::FailureMode_t f checkExpectedMessageBox(); } - QCOMPARE(_missionManager->canEdit(), true); - delete list; list = NULL; _multiSpyMissionManager->clearAllSignals(); @@ -172,7 +169,6 @@ void MissionManagerTest::_roundTripItems(MockLinkMissionItemHandler::FailureMode // newMissionItemsAvailable signal _multiSpyMissionManager->waitForSignalByIndex(inProgressChangedSignalIndex, _missionManagerSignalWaitTime); QCOMPARE(_multiSpyMissionManager->checkSignalByMask(newMissionItemsAvailableSignalMask | inProgressChangedSignalMask), true); - QCOMPARE(_multiSpyMissionManager->checkNoSignalByMask(canEditChangedSignalMask), true); _checkInProgressValues(false); } else { @@ -233,7 +229,6 @@ void MissionManagerTest::_roundTripItems(MockLinkMissionItemHandler::FailureMode } QCOMPARE(_missionManager->missionItems()->count(), (int)cMissionItemsExpected); - QCOMPARE(_missionManager->canEdit(), true); size_t firstActualItem = 0; if (_mockLink->getFirmwareType() == MAV_AUTOPILOT_ARDUPILOTMEGA) { diff --git a/src/QmlControls/MissionItemEditor.qml b/src/QmlControls/MissionItemEditor.qml index 277063eaf..deb8283f0 100644 --- a/src/QmlControls/MissionItemEditor.qml +++ b/src/QmlControls/MissionItemEditor.qml @@ -40,6 +40,13 @@ Rectangle { anchors.right: parent.right height: valuesRect.visible ? valuesRect.y + valuesRect.height : valuesRect.y + MouseArea { + anchors.fill: parent + visible: !missionItem.isCurrentItem + + onClicked: _root.clicked() + } + MissionItemIndexLabel { id: label anchors.verticalCenter: commandPicker.verticalCenter @@ -47,21 +54,30 @@ Rectangle { label: missionItem.sequenceNumber == 0 ? "H" : missionItem.sequenceNumber } - MouseArea { - anchors.fill: parent - visible: !missionItem.isCurrentItem - - onClicked: _root.clicked() + Image { + id: rawEdit + anchors.leftMargin: ScreenTools.defaultFontPixelWidth * 8 + anchors.left: label.right + anchors.verticalCenter: commandPicker.verticalCenter + width: commandPicker.height + height: commandPicker.height + visible: missionItem.friendlyEditAllowed && missionItem.sequenceNumber != 0 && missionItem.isCurrentItem + source: "qrc:/qmlimages/CogWheel.svg" + + MouseArea { + anchors.fill: parent + onClicked: missionItem.rawEdit = !missionItem.rawEdit + } } - QGCComboBox { + FactComboBox { id: commandPicker - anchors.leftMargin: ScreenTools.defaultFontPixelWidth * 10 - anchors.left: label.right + anchors.leftMargin: ScreenTools.defaultFontPixelWidth + anchors.left: rawEdit.right anchors.right: parent.right - currentIndex: missionItem.commandByIndex - model: missionItem.commandNames - visible: missionItem.sequenceNumber != 0 && missionItem.isCurrentItem + indexModel: false + fact: missionItem.supportedCommand + visible: missionItem.sequenceNumber != 0 && missionItem.isCurrentItem && !missionItem.rawEdit onActivated: missionItem.commandByIndex = index } @@ -69,10 +85,9 @@ Rectangle { Rectangle { anchors.fill: commandPicker color: qgcPal.button - visible: !commandPicker.visible + visible: missionItem.sequenceNumber == 0 || !missionItem.isCurrentItem QGCLabel { - id: homeLabel anchors.leftMargin: ScreenTools.defaultFontPixelWidth anchors.fill: parent verticalAlignment: Text.AlignVCenter @@ -89,7 +104,7 @@ Rectangle { anchors.right: parent.right height: valuesItem.height color: qgcPal.windowShadeDark - visible: missionItem.isCurrentItem + visible: missionItem.sequenceNumber != 0 && missionItem.isCurrentItem radius: _radius Item { @@ -110,7 +125,20 @@ Rectangle { QGCLabel { width: parent.width wrapMode: Text.WordWrap - text: missionItem.commandDescription + text: missionItem.rawEdit ? + "Provides advanced access to all commands/parameters. Be very careful!" : + missionItem.commandDescription + } + + Repeater { + model: missionItem.comboboxFacts + + FactComboBox { + width: valuesColumn.width + indexModel: false + model: object.enumStrings + fact: object + } } Repeater { @@ -148,7 +176,6 @@ Rectangle { model: missionItem.checkboxFacts FactCheckBox { - id: textField text: object.name fact: object } diff --git a/src/QmlControls/MissionItemSummary.qml b/src/QmlControls/MissionItemSummary.qml deleted file mode 100644 index 630cfb821..000000000 --- a/src/QmlControls/MissionItemSummary.qml +++ /dev/null @@ -1,73 +0,0 @@ -import QtQuick 2.2 -import QtQuick.Controls 1.2 -import QtQuick.Controls.Styles 1.2 - -import QGroundControl.ScreenTools 1.0 -import QGroundControl.Vehicle 1.0 - -/// Mission item summary display control -Rectangle { - property var missionItem ///< Mission Item object - - width: ScreenTools.defaultFontPixelWidth * 15 - height: valueColumn.height + radius - border.width: 2 - border.color: "white" - color: "white" - radius: ScreenTools.defaultFontPixelWidth - - MissionItemIndexLabel { - id: _indexLabel - anchors.top: parent.top - anchors.right: parent.right - isCurrentItem: missionItem.isCurrentItem - label: missionItem.sequenceNumber - } - - Column { - anchors.margins: parent.radius / 2 - anchors.left: parent.left - anchors.right: parent.right - anchors.top: parent.top - - QGCLabel { - color: "black" - horizontalAlignment: Text.AlignTop - font.weight: Font.Bold - text: missionItem.commandName - } - - Repeater { - model: missionItem.valueLabels - - QGCLabel { - color: "black" - text: modelData - } - } - } - - Column { - id: valueColumn - anchors.margins: parent.radius / 2 - anchors.left: parent.left - anchors.right: parent.right - anchors.top: parent.top - - QGCLabel { - font.weight: Font.Bold - text: " " - } - - Repeater { - model: missionItem.valueStrings - - QGCLabel { - width: valueColumn.width - color: "black" - text: modelData - horizontalAlignment: Text.AlignRight - } - } - } -} diff --git a/src/QmlControls/ParameterEditorDialog.qml b/src/QmlControls/ParameterEditorDialog.qml index c581e61fd..66d94d630 100644 --- a/src/QmlControls/ParameterEditorDialog.qml +++ b/src/QmlControls/ParameterEditorDialog.qml @@ -59,7 +59,8 @@ QGCViewDialog { validationError.text = fact.validate(validateValue, false /* convertOnly */) forceSave.visible = true } - valueField.forceActiveFocus(); + // This was causing problems where it would never give up focus even when hidden! + //valueField.forceActiveFocus() } Column { @@ -83,6 +84,7 @@ QGCViewDialog { QGCTextField { id: valueField text: validate ? validateValue : fact.valueString + focus: true // At this point all Facts are numeric inputMethodHints: Qt.ImhFormattedNumbersOnly diff --git a/src/QmlControls/QGCView.qml b/src/QmlControls/QGCView.qml index d2e71ac79..2f5a51c49 100644 --- a/src/QmlControls/QGCView.qml +++ b/src/QmlControls/QGCView.qml @@ -37,8 +37,9 @@ import QGroundControl.FactControls 1.0 FactPanel { id: __rootItem - property var qgcView: __rootItem /// Used by Fact controls for validation dialogs + property var qgcView: __rootItem ///< Used by Fact controls for validation dialogs property bool completedSignalled: false + property real topDialogMargin: 0 ///< Set a top margin for dialog property var viewPanel @@ -278,11 +279,13 @@ FactPanel { // This is the main dialog panel which is anchored to the right edge Rectangle { - id: __dialogPanel - width: __dialogCharWidth == -1 ? parent.width : defaultTextWidth * __dialogCharWidth - height: parent.height - anchors.left: __transparentSection.right - color: __qgcPal.windowShadeDark + id: __dialogPanel + width: __dialogCharWidth == -1 ? parent.width : defaultTextWidth * __dialogCharWidth + anchors.topMargin: topDialogMargin + anchors.top: parent.top + anchors.bottom: parent.bottom + anchors.left: __transparentSection.right + color: __qgcPal.windowShadeDark Rectangle { id: __header @@ -344,4 +347,4 @@ FactPanel { message: __messageDialogText } } -} \ No newline at end of file +} diff --git a/src/QmlControls/QGroundControl.Controls.qmldir b/src/QmlControls/QGroundControl.Controls.qmldir index 0b96e7952..3e380c883 100644 --- a/src/QmlControls/QGroundControl.Controls.qmldir +++ b/src/QmlControls/QGroundControl.Controls.qmldir @@ -31,7 +31,6 @@ QGCViewDialog 1.0 QGCViewDialog.qml QGCViewMessage 1.0 QGCViewMessage.qml MissionItemIndexLabel 1.0 MissionItemIndexLabel.qml -MissionItemSummary 1.0 MissionItemSummary.qml MissionItemEditor 1.0 MissionItemEditor.qml MainToolBar 1.0 MainToolBar.qml -- 2.22.0