diff --git a/QGCApplication.pro b/QGCApplication.pro index ea05c690ceb0fef37dc96cdbbc752173558f5366..36861d5b64d1a6da1f2588b2a464cccf15c0b8e1 100644 --- a/QGCApplication.pro +++ b/QGCApplication.pro @@ -145,6 +145,7 @@ INCLUDEPATH += \ src/Joystick \ src/lib/qmapcontrol \ src/MissionEditor \ + src/MissionManager \ src/QmlControls \ src/uas \ src/ui \ @@ -246,6 +247,7 @@ HEADERS += \ src/LogCompressor.h \ src/MG.h \ src/MissionEditor/MissionEditor.h \ + src/MissionManager/MissionManager.h \ src/QGC.h \ src/QGCApplication.h \ src/QGCComboBox.h \ @@ -384,6 +386,7 @@ SOURCES += \ src/LogCompressor.cc \ src/main.cc \ src/MissionEditor/MissionEditor.cc \ + src/MissionManager/MissionManager.cc \ src/QGC.cc \ src/QGCApplication.cc \ src/QGCComboBox.cc \ diff --git a/src/AutoPilotPlugins/PX4/AirframeComponent.qml b/src/AutoPilotPlugins/PX4/AirframeComponent.qml index 00be126a389855452320600583ce105b75b2af13..53d12db5d1d6cc17ab1a72fc385fc57186cfd629 100644 --- a/src/AutoPilotPlugins/PX4/AirframeComponent.qml +++ b/src/AutoPilotPlugins/PX4/AirframeComponent.qml @@ -34,7 +34,7 @@ import QGroundControl.Controllers 1.0 import QGroundControl.ScreenTools 1.0 QGCView { - id: rootQGCView + id: qgcView viewPanel: panel QGCPalette { id: qgcPal; colorGroupEnabled: panel.enabled } diff --git a/src/FlightMap/FlightMap.qml b/src/FlightMap/FlightMap.qml index 5ded1bfdab38db15c748060db5151709a897fb47..1886f499f907dc2c826cafc86a09aa2043ec72d9 100644 --- a/src/FlightMap/FlightMap.qml +++ b/src/FlightMap/FlightMap.qml @@ -53,7 +53,6 @@ Map { property real lon: (longitude >= -180 && longitude <= 180) ? longitude : 0 property real lat: (latitude >= -90 && latitude <= 90) ? latitude : 0 - anchors.fill: parent zoomLevel: 18 center: QtPositioning.coordinate(lat, lon) gesture.flickDeceleration: 3000 diff --git a/src/MissionEditor/MissionEditor.cc b/src/MissionEditor/MissionEditor.cc index 91b2281774432f712cf99542eb05fe5a1ea00f3c..c998c59c8b49ab62dcbbad81797c1a652368e754 100644 --- a/src/MissionEditor/MissionEditor.cc +++ b/src/MissionEditor/MissionEditor.cc @@ -23,6 +23,8 @@ This file is part of the QGROUNDCONTROL project #include "MissionEditor.h" #include "ScreenToolsController.h" +#include "MultiVehicleManager.h" +#include "MissionManager.h" #include #include @@ -32,6 +34,7 @@ const char* MissionEditor::_settingsGroup = "MissionEditor"; MissionEditor::MissionEditor(QWidget *parent) : QGCQmlWidgetHolder(parent) + , _missionItems(NULL) { setSizePolicy(QSizePolicy::MinimumExpanding, QSizePolicy::MinimumExpanding); // Get rid of layout default margins @@ -44,6 +47,15 @@ MissionEditor::MissionEditor(QWidget *parent) setMinimumHeight(33 * ScreenToolsController::defaultFontPixelSize_s()); #endif + Vehicle* activeVehicle = MultiVehicleManager::instance()->activeVehicle(); + if (activeVehicle) { + MissionManager* missionManager = activeVehicle->missionManager(); + connect(missionManager, &MissionManager::newMissionItemsAvailable, this, &MissionEditor::_newMissionItemsAvailable); + _newMissionItemsAvailable(); + } else { + _missionItems = new QmlObjectListModel(this); + } + setContextPropertyObject("controller", this); setSource(QUrl::fromUserInput("qrc:/qml/MissionEditor.qml")); @@ -53,6 +65,34 @@ MissionEditor::~MissionEditor() { } +void MissionEditor::_newMissionItemsAvailable(void) +{ + if (_missionItems) { + _missionItems->deleteLater(); + } + + _missionItems = MultiVehicleManager::instance()->activeVehicle()->missionManager()->copyMissionItems(); + emit missionItemsChanged(); +} + +void MissionEditor::getMissionItems(void) +{ + Vehicle* activeVehicle = MultiVehicleManager::instance()->activeVehicle(); + + if (activeVehicle) { + activeVehicle->missionManager()->requestMissionItems(); + } +} + +void MissionEditor::setMissionItems(void) +{ + Vehicle* activeVehicle = MultiVehicleManager::instance()->activeVehicle(); + + if (activeVehicle) { + activeVehicle->missionManager()->writeMissionItems(*_missionItems); + } +} + void MissionEditor::saveSetting(const QString &name, const QString& value) { QSettings settings; @@ -73,10 +113,10 @@ QString MissionEditor::loadSetting(const QString &name, const QString& defaultVa void MissionEditor::addMissionItem(QGeoCoordinate coordinate) { - MissionItem * newItem = new MissionItem(this, _missionItems.count(), coordinate); - if (_missionItems.count() == 0) { + MissionItem * newItem = new MissionItem(this, _missionItems->count(), coordinate); + if (_missionItems->count() == 0) { newItem->setCommand(MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF); } qDebug() << "MissionItem" << newItem->coordinate(); - _missionItems.append(newItem); + _missionItems->append(newItem); } diff --git a/src/MissionEditor/MissionEditor.h b/src/MissionEditor/MissionEditor.h index 03e0c11abd51e242062e37e316213b25dab402ad..ceea6e8cb55c61bd730125cbc3965f22b1411c8d 100644 --- a/src/MissionEditor/MissionEditor.h +++ b/src/MissionEditor/MissionEditor.h @@ -35,19 +35,27 @@ public: MissionEditor(QWidget* parent = NULL); ~MissionEditor(); - Q_PROPERTY(QmlObjectListModel* missionItems READ missionItemsModel CONSTANT) + Q_PROPERTY(QmlObjectListModel* missionItems READ missionItemsModel NOTIFY missionItemsChanged) Q_INVOKABLE void addMissionItem(QGeoCoordinate coordinate); + Q_INVOKABLE void getMissionItems(void); + Q_INVOKABLE void setMissionItems(void); Q_INVOKABLE void saveSetting (const QString &key, const QString& value); Q_INVOKABLE QString loadSetting (const QString &key, const QString& defaultValue); // Property accessors - QmlObjectListModel* missionItemsModel(void) { return &_missionItems; } + QmlObjectListModel* missionItemsModel(void) { return _missionItems; } + +signals: + void missionItemsChanged(void); + +private slots: + void _newMissionItemsAvailable(); private: - QmlObjectListModel _missionItems; + QmlObjectListModel* _missionItems; static const char* _settingsGroup; }; diff --git a/src/MissionEditor/MissionEditor.qml b/src/MissionEditor/MissionEditor.qml index e5f0e9c842a99ea17b875aecb75a6b4b2b9024e2..e27ebe721947ebc36e6bccaec1500e95d7efa5cd 100644 --- a/src/MissionEditor/MissionEditor.qml +++ b/src/MissionEditor/MissionEditor.qml @@ -34,82 +34,147 @@ import QGroundControl.Palette 1.0 /// Mission Editor -Item { - // For some reason we can't have FlightMap as the top level control. If you do that it doesn't draw. - // So we have an Item at the top to work around that. +QGCView { + viewPanel: panel readonly property real _defaultLatitude: 37.803784 readonly property real _defaultLongitude: -122.462276 readonly property int _decimalPlaces: 7 + readonly property real _horizontalMargin: ScreenTools.defaultFontPixelWidth / 2 + readonly property real _verticalMargin: ScreenTools.defaultFontPixelHeight / 2 + readonly property var _activeVehicle: multiVehicleManager.activeVehicle - FlightMap { - id: editorMap - anchors.fill: parent - mapName: "MissionEditor" - latitude: _defaultLatitude - longitude: _defaultLongitude - - QGCLabel { text: "WIP: Non functional"; font.pixelSize: ScreenTools.largeFontPixelSize } + QGCPalette { id: _qgcPal; colorGroupEnabled: enabled } + QGCViewPanel { + id: panel + anchors.fill: parent - MouseArea { + Item { anchors.fill: parent - onClicked: { - var coordinate = editorMap.toCoordinate(Qt.point(mouse.x, mouse.y)) - coordinate.latitude = coordinate.latitude.toFixed(_decimalPlaces) - coordinate.longitude = coordinate.longitude.toFixed(_decimalPlaces) - coordinate.altitude = 0 - controller.addMissionItem(coordinate) - } - } - - // Add the mission items to the map - MapItemView { - model: controller.missionItems - - delegate: - MissionItemIndicator { - label: object.sequenceNumber - isCurrentItem: object.isCurrentItem - coordinate: object.coordinate - - Component.onCompleted: console.log("Indicator", object.coordinate) + FlightMap { + id: editorMap + anchors.left: parent.left + anchors.right: missionItemView.left + anchors.top: parent.top + anchors.bottom: parent.bottom + mapName: "MissionEditor" + latitude: _defaultLatitude + longitude: _defaultLongitude + + QGCLabel { + anchors.right: parent.right + text: "WIP: Danger, do not fly with this!"; font.pixelSize: ScreenTools.largeFontPixelSize } + + + MouseArea { + anchors.fill: parent + + onClicked: { + var coordinate = editorMap.toCoordinate(Qt.point(mouse.x, mouse.y)) + coordinate.latitude = coordinate.latitude.toFixed(_decimalPlaces) + coordinate.longitude = coordinate.longitude.toFixed(_decimalPlaces) + coordinate.altitude = 0 + controller.addMissionItem(coordinate) + } } - } - - // Mission item list - ListView { - id: missionItemSummaryList - anchors.margins: ScreenTools.defaultFontPixelHeight - anchors.top: parent.top - anchors.bottom: editorMap.mapWidgets.top - anchors.right: parent.right - width: ScreenTools.defaultFontPixelWidth * 30 - spacing: ScreenTools.defaultFontPixelHeight / 2 - orientation: ListView.Vertical - model: controller.missionItems - - property real _maxItemHeight: 0 - - delegate: - MissionItemEditor { - missionItem: object + + // Add the mission items to the map + MapItemView { + model: controller.missionItems + + delegate: + MissionItemIndicator { + label: object.sequenceNumber + isCurrentItem: object.isCurrentItem + coordinate: object.coordinate + + Component.onCompleted: console.log("Indicator", object.coordinate) + } + } + + Column { + id: controlWidgets + anchors.margins: ScreenTools.defaultFontPixelWidth + anchors.right: parent.left + anchors.bottom: parent.top + spacing: ScreenTools.defaultFontPixelWidth / 2 + + QGCButton { + id: addMode + text: "+" + checkable: true + } } - } - - Column { - id: controlWidgets - anchors.margins: ScreenTools.defaultFontPixelWidth - anchors.right: parent.left - anchors.bottom: parent.top - spacing: ScreenTools.defaultFontPixelWidth / 2 - - QGCButton { - id: addMode - text: "+" - checkable: true - } - } - } -} + } // FlightMap + + Rectangle { + id: missionItemView + anchors.right: parent.right + anchors.top: parent.top + anchors.bottom: parent.bottom + width: ScreenTools.defaultFontPixelWidth * 30 + color: _qgcPal.window + + Item { + anchors.margins: _verticalMargin + anchors.fill: parent + + QGCButton { + id: toolsButton + text: "Tools" + menu : toolMenu + + Menu { + id: toolMenu + + MenuItem { + text: "Get mission items from vehicle" + enabled: _activeVehicle && !_activeVehicle.missionManager.inProgress + + onTriggered: controller.getMissionItems() + } + + MenuItem { + text: "Send mission items to vehicle" + enabled: _activeVehicle && !_activeVehicle.missionManager.inProgress + + onTriggered: controller.setMissionItems() + } + + MenuItem { + text: "Load mission items from disk" + } + + MenuItem { + text: "Save mission items to disk" + } + } + } + + // Mission item list + ListView { + id: missionItemSummaryList + anchors.topMargin: _verticalMargin + anchors.left: parent.left + anchors.right: parent.right + anchors.top: toolsButton.bottom + anchors.bottom: parent.bottom + spacing: _verticalMargin + orientation: ListView.Vertical + model: controller.missionItems + + property real _maxItemHeight: 0 + + delegate: + MissionItemEditor { + missionItem: object + width: parent.width + } + } // ListView + } // Item + } // Rectangle - mission item list + } // Item - split view container + } // QGCViewPanel +} // QGCVIew diff --git a/src/MissionItem.cc b/src/MissionItem.cc index 99aba903bd0776d334165f53549e0ef78a9da9bf..67c73b74f2f32b148d36d66e39846513672a46e0 100644 --- a/src/MissionItem.cc +++ b/src/MissionItem.cc @@ -56,12 +56,12 @@ MissionItem::MissionItem(QObject* parent, bool autocontinue, bool isCurrentItem, int frame, - int action) + int command) : QObject(parent) , _sequenceNumber(sequenceNumber) , _coordinate(coordinate) , _frame(frame) - , _action(action) + , _command((MavlinkQmlSingleton::Qml_MAV_CMD)command) , _autocontinue(autocontinue) , _isCurrentItem(isCurrentItem) , _reachedTime(0) @@ -145,7 +145,7 @@ const MissionItem& MissionItem::operator=(const MissionItem& other) _isCurrentItem = other._isCurrentItem; _coordinate = other._coordinate; _frame = other._frame; - _action = other._action; + _command = other._command; _autocontinue = other._autocontinue; _reachedTime = other._reachedTime; @@ -168,7 +168,7 @@ const MissionItem& MissionItem::operator=(const MissionItem& other) bool MissionItem::isNavigationType() { - return (_action < MAV_CMD_NAV_LAST); + return (_command < MAV_CMD_NAV_LAST); } void MissionItem::save(QTextStream &saveStream) @@ -178,10 +178,10 @@ void MissionItem::save(QTextStream &saveStream) position = position.arg(y(), 0, 'g', 18); position = position.arg(z(), 0, 'g', 18); QString parameters("%1\t%2\t%3\t%4"); - parameters = parameters.arg(getParam2(), 0, 'g', 18).arg(getParam2(), 0, 'g', 18).arg(loiterOrbitRadius(), 0, 'g', 18).arg(yawRadians(), 0, 'g', 18); + parameters = parameters.arg(param2(), 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->getFrame() << "\t" << this->getAction() << "\t" << parameters << "\t" << position << "\t" << this->getAutoContinue() << "\r\n"; //"\t" << this->getDescription() << "\r\n"; + 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) @@ -191,7 +191,7 @@ bool MissionItem::load(QTextStream &loadStream) setSequenceNumber(wpParams[0].toInt()); setIsCurrentItem(wpParams[1].toInt() == 1 ? true : false); _frame = (MAV_FRAME) wpParams[2].toInt(); - _action = (MAV_CMD) wpParams[3].toInt(); + setAction(wpParams[3].toInt()); setParam1(wpParams[4].toDouble()); setParam2(wpParams[5].toDouble()); setLoiterOrbitRadius(wpParams[6].toDouble()); @@ -270,19 +270,19 @@ void MissionItem::setAltitude(double altitude) void MissionItem::setAction(int /*MAV_CMD*/ action) { - if (_action != action) { - _action = action; + if (_command != action) { + _command = (MavlinkQmlSingleton::Qml_MAV_CMD)action; // Flick defaults according to WP type - if (_action == MAV_CMD_NAV_TAKEOFF) { + if (_command == MAV_CMD_NAV_TAKEOFF) { // We default to 15 degrees minimum takeoff pitch setParam1(15.0); } emit changed(this); emit commandNameChanged(commandName()); - emit commandChanged((MavlinkQmlSingleton::Qml_MAV_CMD)_action); + emit commandChanged((MavlinkQmlSingleton::Qml_MAV_CMD)_command); emit specifiesCoordinateChanged(specifiesCoordinate()); emit valueLabelsChanged(valueLabels()); emit valueStringsChanged(valueStrings()); @@ -318,21 +318,21 @@ void MissionItem::setAcceptanceRadius(double radius) setParam2(radius); } -void MissionItem::setParam1(double param1) +void MissionItem::setParam1(double param) { - if (getParam1() != param1) + if (param1() != param) { - _param1Fact->setValue(param1); + _param1Fact->setValue(param); emit changed(this); emit valueStringsChanged(valueStrings()); } } -void MissionItem::setParam2(double param2) +void MissionItem::setParam2(double param) { - if (getParam2() != param2) + if (param2() != param) { - _param2Fact->setValue(param2); + _param2Fact->setValue(param); emit valueStringsChanged(valueStrings()); emit changed(this); } @@ -396,7 +396,7 @@ void MissionItem::setHoldTime(double holdTime) bool MissionItem::specifiesCoordinate(void) const { - switch (_action) { + switch (_command) { case MAV_CMD_NAV_WAYPOINT: case MAV_CMD_NAV_LOITER_UNLIM: case MAV_CMD_NAV_LOITER_TURNS: @@ -413,7 +413,7 @@ QString MissionItem::commandName(void) { QString type; - switch (_action) { + switch (_command) { case MAV_CMD_NAV_WAYPOINT: type = "Waypoint"; break; @@ -438,7 +438,7 @@ QString MissionItem::commandName(void) type = "Jump To Command"; break; default: - type = QString("Unknown (%1)").arg(_action); + type = QString("Unknown (%1)").arg(_command); break; } @@ -449,9 +449,9 @@ QStringList MissionItem::valueLabels(void) { QStringList labels; - switch (_action) { + switch (_command) { case MAV_CMD_NAV_WAYPOINT: - if (getFrame() == MAV_FRAME_GLOBAL_RELATIVE_ALT) { + if (frame() == MAV_FRAME_GLOBAL_RELATIVE_ALT) { labels << "Alt (rel):"; } else { labels << "Alt:"; @@ -470,7 +470,7 @@ QStringList MissionItem::valueLabels(void) case MAV_CMD_NAV_RETURN_TO_LAUNCH: break; case MAV_CMD_NAV_LAND: - if (getFrame() == MAV_FRAME_GLOBAL_RELATIVE_ALT) { + if (frame() == MAV_FRAME_GLOBAL_RELATIVE_ALT) { labels << "Alt (rel):"; } else { labels << "Alt:"; @@ -478,7 +478,7 @@ QStringList MissionItem::valueLabels(void) labels << "Heading:"; break; case MAV_CMD_NAV_TAKEOFF: - if (getFrame() == MAV_FRAME_GLOBAL_RELATIVE_ALT) { + if (frame() == MAV_FRAME_GLOBAL_RELATIVE_ALT) { labels << "Alt (rel):"; } else { labels << "Alt:"; @@ -507,18 +507,18 @@ QStringList MissionItem::valueStrings(void) { QStringList list; - switch (_action) { + switch (_command) { case MAV_CMD_NAV_WAYPOINT: - list << _oneDecimalString(_coordinate.altitude()) << _oneDecimalString(yawDegrees()) << _oneDecimalString(getParam2()) << _oneDecimalString(getParam1()); + list << _oneDecimalString(_coordinate.altitude()) << _oneDecimalString(yawDegrees()) << _oneDecimalString(param2()) << _oneDecimalString(param1()); break; case MAV_CMD_NAV_LOITER_UNLIM: list << _oneDecimalString(yawRadians() * (180.0 / M_PI)) << _oneDecimalString(loiterOrbitRadius()); break; case MAV_CMD_NAV_LOITER_TURNS: - list << _oneDecimalString(yawRadians() * (180.0 / M_PI)) << _oneDecimalString(loiterOrbitRadius()) << _oneDecimalString(getParam1()); + list << _oneDecimalString(yawRadians() * (180.0 / M_PI)) << _oneDecimalString(loiterOrbitRadius()) << _oneDecimalString(param1()); break; case MAV_CMD_NAV_LOITER_TIME: - list << _oneDecimalString(yawRadians() * (180.0 / M_PI)) << _oneDecimalString(loiterOrbitRadius()) << _oneDecimalString(getParam1()); + list << _oneDecimalString(yawRadians() * (180.0 / M_PI)) << _oneDecimalString(loiterOrbitRadius()) << _oneDecimalString(param1()); break; case MAV_CMD_NAV_RETURN_TO_LAUNCH: break; @@ -526,13 +526,13 @@ QStringList MissionItem::valueStrings(void) list << _oneDecimalString(_coordinate.altitude()) << _oneDecimalString(yawRadians() * (180.0 / M_PI)); break; case MAV_CMD_NAV_TAKEOFF: - list << _oneDecimalString(_coordinate.altitude()) << _oneDecimalString(yawRadians() * (180.0 / M_PI)) << _oneDecimalString(getParam1()); + list << _oneDecimalString(_coordinate.altitude()) << _oneDecimalString(yawRadians() * (180.0 / M_PI)) << _oneDecimalString(param1()); break; case MAV_CMD_CONDITION_DELAY: - list << _oneDecimalString(getParam1()); + list << _oneDecimalString(param1()); break; case MAV_CMD_DO_JUMP: - list << _oneDecimalString(getParam1()) << _oneDecimalString(getParam2()); + list << _oneDecimalString(param1()) << _oneDecimalString(param2()); break; default: break; @@ -561,7 +561,7 @@ QStringList MissionItem::commandNames(void) { int MissionItem::commandByIndex(void) { for (int i=0; i<_cMavCmd2Name; i++) { - if (_rgMavCmd2Name[i].command == _action) { + if (_rgMavCmd2Name[i].command == _command) { return i; } } @@ -583,7 +583,7 @@ QmlObjectListModel* MissionItem::facts(void) { QmlObjectListModel* model = new QmlObjectListModel(this); - switch (_action) { + switch ((MAV_CMD)_command) { case MAV_CMD_NAV_WAYPOINT: _param2Fact->_setName("Radius:"); _param2Fact->setMetaData(_acceptanceRadiusMetaData); @@ -635,11 +635,39 @@ QmlObjectListModel* MissionItem::facts(void) model->append(_param1Fact); model->append(_param2Fact); break; + default: + break; } return model; } +int MissionItem::factCount(void) +{ + switch ((MAV_CMD)_command) { + case MAV_CMD_NAV_WAYPOINT: + return 3; + case MAV_CMD_NAV_LOITER_UNLIM: + return 2; + case MAV_CMD_NAV_LOITER_TURNS: + return 3; + case MAV_CMD_NAV_LOITER_TIME: + return 3; + case MAV_CMD_NAV_RETURN_TO_LAUNCH: + return 0; + case MAV_CMD_NAV_LAND: + return 1; + case MAV_CMD_NAV_TAKEOFF: + return 2; + case MAV_CMD_CONDITION_DELAY: + return 1; + case MAV_CMD_DO_JUMP: + return 2; + default: + return 0; + } +} + double MissionItem::yawRadians(void) const { return _yawRadiansFact->value().toDouble(); diff --git a/src/MissionItem.h b/src/MissionItem.h index 7283f008c1b12bfc1e8da975be00bd345338d997..ee877a1e8b21b2eedaa4b2d6e796fb8b770b4df6 100644 --- a/src/MissionItem.h +++ b/src/MissionItem.h @@ -69,6 +69,7 @@ public: Q_PROPERTY(QStringList valueStrings READ valueStrings NOTIFY valueStringsChanged) Q_PROPERTY(int commandByIndex READ commandByIndex WRITE setCommandByIndex NOTIFY commandChanged) Q_PROPERTY(QmlObjectListModel* facts READ facts NOTIFY commandChanged) + Q_PROPERTY(int factCount READ factCount NOTIFY commandChanged) Q_PROPERTY(MavlinkQmlSingleton::Qml_MAV_CMD command READ command WRITE setCommand NOTIFY commandChanged) // Property accesors @@ -90,13 +91,14 @@ public: int commandByIndex(void); void setCommandByIndex(int index); - MavlinkQmlSingleton::Qml_MAV_CMD command(void) { return (MavlinkQmlSingleton::Qml_MAV_CMD)getAction(); }; + 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* facts(void); + int factCount(void); double yawDegrees(void) const; void setYawDegrees(double yaw); @@ -122,57 +124,56 @@ public: double yawRadians(void) const; void setYawRadians(double yaw); - bool getAutoContinue() const { + bool autoContinue() const { return _autocontinue; } double loiterOrbitRadius() const { return _loiterOrbitRadiusFact->value().toDouble(); } - double getAcceptanceRadius() const { - return getParam2(); + double acceptanceRadius() const { + return param2(); } - double getHoldTime() const { - return getParam1(); + double holdTime() const { + return param1(); } - double getParam1() const { + double param1() const { return _param1Fact->value().toDouble(); } - double getParam2() const { + double param2() const { return _param2Fact->value().toDouble(); } - double getParam3() const { + double param3() const { return loiterOrbitRadius(); } - double getParam4() const { + double param4() const { return yawRadians(); } - double getParam5() const { + double param5() const { return latitude(); } - double getParam6() const { + double param6() const { return longitude(); } - double getParam7() const { + double param7() const { return altitude(); } // MAV_FRAME - int getFrame() const { + int frame() const { return _frame; } // MAV_CMD - int getAction() const { - return _action; + 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 getReachedTime() const { return _reachedTime; } + quint64 reachedTime() const { return _reachedTime; } void save(QTextStream &saveStream); bool load(QTextStream &loadStream); - signals: void sequenceNumberChanged(int sequenceNumber); void specifiesCoordinateChanged(bool specifiesCoordinate); @@ -224,13 +225,13 @@ private: const char* name; } MavCmd2Name_t; - int _sequenceNumber; - QGeoCoordinate _coordinate; - int _frame; - int _action; - bool _autocontinue; - bool _isCurrentItem; - quint64 _reachedTime; + int _sequenceNumber; + QGeoCoordinate _coordinate; + int _frame; + MavlinkQmlSingleton::Qml_MAV_CMD _command; + bool _autocontinue; + bool _isCurrentItem; + quint64 _reachedTime; Fact* _yawRadiansFact; Fact* _loiterOrbitRadiusFact; diff --git a/src/MissionManager/MissionManager.cc b/src/MissionManager/MissionManager.cc new file mode 100644 index 0000000000000000000000000000000000000000..37a466b13ca553fbc8c4ffbaa541d98cc2afba39 --- /dev/null +++ b/src/MissionManager/MissionManager.cc @@ -0,0 +1,371 @@ +/*===================================================================== + + 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 . + + ======================================================================*/ + +/// @file +/// @author Don Gagne + +#include "MissionManager.h" +#include "Vehicle.h" +#include "MAVLinkProtocol.h" + +QGC_LOGGING_CATEGORY(MissionManagerLog, "MissionManagerLog") + +MissionManager::MissionManager(Vehicle* vehicle) + : QThread() + , _vehicle(vehicle) + , _cMissionItems(0) + , _ackTimeoutTimer(NULL) + , _retryAck(AckNone) +{ + moveToThread(this); + + connect(_vehicle, &Vehicle::mavlinkMessageReceived, this, &MissionManager::_mavlinkMessageReceived); + + connect(this, &MissionManager::_writeMissionItemsOnThread, this, &MissionManager::_writeMissionItems); + connect(this, &MissionManager::_requestMissionItemsOnThread, this, &MissionManager::_requestMissionItems); + + start(); +} + +MissionManager::~MissionManager() +{ + +} + +void MissionManager::run(void) +{ + _ackTimeoutTimer = new QTimer(this); + _ackTimeoutTimer->setSingleShot(true); + _ackTimeoutTimer->setInterval(_ackTimeoutMilliseconds); + + connect(_ackTimeoutTimer, &QTimer::timeout, this, &MissionManager::_ackTimeout); + + _requestMissionItems(); + + exec(); +} + +void MissionManager::requestMissionItems(void) +{ + emit _requestMissionItemsOnThread(); +} + +void MissionManager::writeMissionItems(const QmlObjectListModel& missionItems) +{ + _missionItems.clear(); + for (int i=0; i(missionItems[i]))); + } + + emit _writeMissionItemsOnThread(); +} + +void MissionManager::_requestMissionItems(void) +{ + qCDebug(MissionManagerLog) << "_requestMissionItems"; + + mavlink_message_t message; + mavlink_mission_request_list_t request; + + _clearMissionItems(); + + request.target_system = _vehicle->id(); + request.target_component = MAV_COMP_ID_MISSIONPLANNER; + + mavlink_msg_mission_request_list_encode(MAVLinkProtocol::instance()->getSystemId(), MAVLinkProtocol::instance()->getComponentId(), &message, &request); + + _vehicle->sendMessage(message); + _startAckTimeout(AckMissionCount, message); +} + +void MissionManager::_ackTimeout(void) +{ + if (_retryAck == AckNone) { + qCWarning(MissionManagerLog) << "_ackTimeout timeout with AckNone"; + return; + } + + if (++_retryCount <= _maxRetryCount) { + qCDebug(MissionManagerLog) << "_ackTimeout retry _retryAck:_retryCount" << _retryAck << _retryCount; + _vehicle->sendMessage(_retryMessage); + } else { + qCDebug(MissionManagerLog) << "_ackTimeout failed after max retries _retryAck:_retryCount" << _retryAck << _retryCount; + } +} + +void MissionManager::_startAckTimeout(AckType_t ack, const mavlink_message_t& message) +{ + _retryAck = ack; + _retryCount = 0; + _retryMessage = message; + + _ackTimeoutTimer->start(); +} + +bool MissionManager::_stopAckTimeout(AckType_t expectedAck) +{ + bool success; + + _ackTimeoutTimer->stop(); + + if (_retryAck != expectedAck) { + qCDebug(MissionManagerLog) << "Invalid ack sequence _retryAck:expectedAck" << _retryAck << expectedAck; + success = false; + } else { + success = true; + } + + _retryAck = AckNone; + + return success; +} + +void MissionManager::_sendTransactionComplete(void) +{ + qCDebug(MissionManagerLog) << "_sendTransactionComplete read sequence complete"; + + mavlink_message_t message; + mavlink_mission_ack_t missionAck; + + missionAck.target_system = _vehicle->id(); + missionAck.target_component = MAV_COMP_ID_MISSIONPLANNER; + missionAck.type = MAV_MISSION_ACCEPTED; + + mavlink_msg_mission_ack_encode(MAVLinkProtocol::instance()->getSystemId(), MAVLinkProtocol::instance()->getComponentId(), &message, &missionAck); + + _vehicle->sendMessage(message); + + emit newMissionItemsAvailable(); +} + +void MissionManager::_handleMissionCount(const mavlink_message_t& message) +{ + mavlink_mission_count_t missionCount; + + if (!_stopAckTimeout(AckMissionCount)) { + return; + } + + mavlink_msg_mission_count_decode(&message, &missionCount); + + _cMissionItems = missionCount.count; + qCDebug(MissionManagerLog) << "_handleMissionCount count:" << _cMissionItems; + + if (_cMissionItems == 0) { + _sendTransactionComplete(); + } else { + _requestNextMissionItem(0); + } +} + +void MissionManager::_requestNextMissionItem(int sequenceNumber) +{ + qCDebug(MissionManagerLog) << "_requestNextMissionItem sequenceNumber:" << sequenceNumber; + + if (sequenceNumber >= _cMissionItems) { + qCWarning(MissionManagerLog) << "_requestNextMissionItem requested seqeuence number > item count sequenceNumber::_cMissionItems" << sequenceNumber << _cMissionItems; + return; + } + + mavlink_message_t message; + mavlink_mission_request_t missionRequest; + + missionRequest.target_system = _vehicle->id(); + missionRequest.target_component = MAV_COMP_ID_MISSIONPLANNER; + missionRequest.seq = sequenceNumber; + _expectedSequenceNumber = sequenceNumber; + + mavlink_msg_mission_request_encode(MAVLinkProtocol::instance()->getSystemId(), MAVLinkProtocol::instance()->getComponentId(), &message, &missionRequest); + + _vehicle->sendMessage(message); + _startAckTimeout(AckMissionItem, message); +} + +void MissionManager::_handleMissionItem(const mavlink_message_t& message) +{ + mavlink_mission_item_t missionItem; + + if (!_stopAckTimeout(AckMissionItem)) { + return; + } + + mavlink_msg_mission_item_decode(&message, &missionItem); + + qCDebug(MissionManagerLog) << "_handleMissionItem sequenceNumber:" << missionItem.seq; + + if (missionItem.seq != _expectedSequenceNumber) { + qCDebug(MissionManagerLog) << "_handleMissionItem mission item received out of sequence expected:actual" << _expectedSequenceNumber << missionItem.seq; + return; + } + + MissionItem* item = new MissionItem(this, + missionItem.seq, + QGeoCoordinate(missionItem.x, missionItem.y, missionItem.z), + missionItem.param1, + missionItem.param2, + missionItem.param3, + missionItem.param3, + missionItem.autocontinue, + missionItem.current, + missionItem.frame, + missionItem.command); + _missionItems.append(item); + + int nextSequenceNumber = missionItem.seq + 1; + if (nextSequenceNumber == _cMissionItems) { + _sendTransactionComplete(); + } else { + _requestNextMissionItem(nextSequenceNumber); + } +} + +void MissionManager::_clearMissionItems(void) +{ + _cMissionItems = 0; + _missionItems.clear(); +} + +void MissionManager::_writeMissionItems(void) +{ + qCDebug(MissionManagerLog) << "writeMissionItems count:" << _missionItems.count(); + + if (inProgress()) { + qCDebug(MissionManagerLog) << "writeMissionItems called while transaction in progress"; + // FIXME: Better error handling + return; + } + + mavlink_message_t message; + mavlink_mission_count_t missionCount; + + missionCount.target_system = _vehicle->id(); + missionCount.target_component = MAV_COMP_ID_MISSIONPLANNER; + missionCount.count = _missionItems.count(); + + mavlink_msg_mission_count_encode(MAVLinkProtocol::instance()->getSystemId(), MAVLinkProtocol::instance()->getComponentId(), &message, &missionCount); + + _vehicle->sendMessage(message); + _startAckTimeout(AckMissionRequest, message); +} + +void MissionManager::_handleMissionRequest(const mavlink_message_t& message) +{ + mavlink_mission_request_t missionRequest; + + if (!_stopAckTimeout(AckMissionRequest)) { + return; + } + + mavlink_msg_mission_request_decode(&message, &missionRequest); + + qCDebug(MissionManagerLog) << "_handleMissionRequest sequenceNumber:" << missionRequest.seq; + + if (missionRequest.seq < 0 || missionRequest.seq >= _missionItems.count()) { + qCDebug(MissionManagerLog) << "_handleMissionRequest invalid sequence number requested:count" << missionRequest.seq << _missionItems.count(); + return; + } + + mavlink_message_t messageOut; + mavlink_mission_item_t missionItem; + + MissionItem* item = (MissionItem*)_missionItems[missionRequest.seq]; + + missionItem.target_system = _vehicle->id(); + missionItem.target_component = MAV_COMP_ID_MISSIONPLANNER; + missionItem.seq = missionRequest.seq; + missionItem.command = item->command(); + missionItem.x = item->coordinate().latitude(); + missionItem.y = item->coordinate().longitude(); + missionItem.z = item->coordinate().altitude(); + missionItem.param1 = item->param1(); + missionItem.param2 = item->param2(); + missionItem.param3 = item->param3(); + missionItem.param4 = item->param4(); + missionItem.frame = item->frame(); + missionItem.current = missionRequest.seq == 0; + missionItem.autocontinue = item->autoContinue(); + + mavlink_msg_mission_item_encode(MAVLinkProtocol::instance()->getSystemId(), MAVLinkProtocol::instance()->getComponentId(), &messageOut, &missionItem); + + _vehicle->sendMessage(messageOut); + // FIXME: This ack sequence isn't quite write + _startAckTimeout(AckMissionRequest, messageOut); +} + +void MissionManager::_handleMissionAck(const mavlink_message_t& message) +{ + mavlink_mission_ack_t missionAck; + + if (!_stopAckTimeout(AckMissionRequest)) { + return; + } + + mavlink_msg_mission_ack_decode(&message, &missionAck); + + if (missionAck.type == MAV_MISSION_ACCEPTED) { + qCDebug(MissionManagerLog) << "_handleMissionAck write sequence complete"; + } else { + qCDebug(MissionManagerLog) << "_handleMissionAck ack error:" << missionAck.type; + } +} + +/// Called when a new mavlink message for out vehicle is received +void MissionManager::_mavlinkMessageReceived(const mavlink_message_t& message) +{ + switch (message.msgid) { + case MAVLINK_MSG_ID_MISSION_COUNT: + _handleMissionCount(message); + break; + + case MAVLINK_MSG_ID_MISSION_ITEM: + _handleMissionItem(message); + break; + + case MAVLINK_MSG_ID_MISSION_REQUEST: + _handleMissionRequest(message); + break; + + case MAVLINK_MSG_ID_MISSION_ACK: + _handleMissionAck(message); + break; + + case MAVLINK_MSG_ID_MISSION_ITEM_REACHED: + // FIXME: NYI + break; + + case MAVLINK_MSG_ID_MISSION_CURRENT: + // FIXME: NYI + break; + } +} + +QmlObjectListModel* MissionManager::copyMissionItems(void) +{ + QmlObjectListModel* list = new QmlObjectListModel(); + + for (int i=0; i<_missionItems.count(); i++) { + list->append(new MissionItem(*qobject_cast(_missionItems[i]))); + } + + return list; +} diff --git a/src/MissionManager/MissionManager.h b/src/MissionManager/MissionManager.h new file mode 100644 index 0000000000000000000000000000000000000000..584f07b3a2a75533a04ca07e5a8153bc1b708a3e --- /dev/null +++ b/src/MissionManager/MissionManager.h @@ -0,0 +1,119 @@ +/*===================================================================== + + 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 MissionManager_H +#define MissionManager_H + +#include +#include +#include +#include +#include + +#include "QmlObjectListModel.h" +#include "QGCMAVLink.h" +#include "QGCLoggingCategory.h" + +class Vehicle; + +Q_DECLARE_LOGGING_CATEGORY(MissionManagerLog) + +class MissionManager : public QThread +{ + Q_OBJECT + +public: + /// @param uas Uas which this set of facts is associated with + MissionManager(Vehicle* vehicle); + ~MissionManager(); + + Q_PROPERTY(bool inProgress READ inProgress CONSTANT) + Q_PROPERTY(QmlObjectListModel* missionItems READ missionItems CONSTANT) + + // Property accessors + + bool inProgress(void) { return _retryAck != AckNone; } + QmlObjectListModel* missionItems(void) { return &_missionItems; } + + void requestMissionItems(void); + + /// Writes the specified set of mission items to the vehicle + void writeMissionItems(const QmlObjectListModel& missionItems); + + /// Returns a copy of the current set of mission items. Caller is responsible for + /// freeing returned object. + QmlObjectListModel* copyMissionItems(void); + +signals: + void newMissionItemsAvailable(void); + void _requestMissionItemsOnThread(void); + void _writeMissionItemsOnThread(void); + +private slots: + void _mavlinkMessageReceived(const mavlink_message_t& message); + void _ackTimeout(void); + +private: + typedef enum { + AckNone, ///< State machine is idle + AckMissionCount, ///< MISSION_COUNT message expected + AckMissionItem, ///< MISSION_ITEM expected + AckMissionRequest, ///< MISSION_REQUEST is expected, or MISSION_ACK to end sequence + } AckType_t; + + void _startAckTimeout(AckType_t ack, const mavlink_message_t& message); + bool _stopAckTimeout(AckType_t expectedAck); + void _sendTransactionComplete(void); + void _handleMissionCount(const mavlink_message_t& message); + void _handleMissionItem(const mavlink_message_t& message); + void _handleMissionRequest(const mavlink_message_t& message); + void _handleMissionAck(const mavlink_message_t& message); + void _requestNextMissionItem(int sequenceNumber); + void _clearMissionItems(void); + void _requestMissionItems(void); + void _writeMissionItems(void); + + // Overrides from QThread + void run(void); + +private: + Vehicle* _vehicle; + + int _cMissionItems; ///< Mission items on vehicle + + QTimer* _ackTimeoutTimer; + AckType_t _retryAck; + mavlink_message_t _retryMessage; + int _retryCount; + + int _expectedSequenceNumber; + + QMutex _dataMutex; + + QmlObjectListModel _missionItems; + + static const int _ackTimeoutMilliseconds= 1000; + static const int _maxRetryCount = 5; +}; + +#endif \ No newline at end of file diff --git a/src/QGCApplication.cc b/src/QGCApplication.cc index 31f3fe601c38686474e39f1c4bd10cb6c00aabd6..c32d35dd0850186bee6b9b84a1e2e001952cb3a0 100644 --- a/src/QGCApplication.cc +++ b/src/QGCApplication.cc @@ -79,6 +79,7 @@ #include "MavlinkQmlSingleton.h" #include "JoystickManager.h" #include "QmlObjectListModel.h" +#include "MissionManager.h" #ifndef __ios__ #include "SerialLink.h" @@ -294,6 +295,7 @@ void QGCApplication::_initCommon(void) qmlRegisterUncreatableType ("QGroundControl.AutoPilotPlugin", 1, 0, "VehicleComponent", "Can only reference, cannot create"); qmlRegisterUncreatableType ("QGroundControl.Vehicle", 1, 0, "Vehicle", "Can only reference, cannot create"); qmlRegisterUncreatableType ("QGroundControl.Vehicle", 1, 0, "MissionItem", "Can only reference, cannot create"); + qmlRegisterUncreatableType ("QGroundControl.Vehicle", 1, 0, "MissionManager", "Can only reference, cannot create"); qmlRegisterUncreatableType ("QGroundControl.JoystickManager", 1, 0, "JoystickManager", "Reference only"); qmlRegisterUncreatableType ("QGroundControl.JoystickManager", 1, 0, "Joystick", "Reference only"); qmlRegisterUncreatableType ("QGroundControl", 1, 0, "QmlObjectListModel", "Reference only"); diff --git a/src/QmlControls/MissionItemEditor.qml b/src/QmlControls/MissionItemEditor.qml index 8105c05c5b9e2f7a6a002eaff15de37996be1fde..e9aa869175e0c7169271b47df3716df3809684cc 100644 --- a/src/QmlControls/MissionItemEditor.qml +++ b/src/QmlControls/MissionItemEditor.qml @@ -6,142 +6,146 @@ import QGroundControl.ScreenTools 1.0 import QGroundControl.Vehicle 1.0 import QGroundControl.Controls 1.0 import QGroundControl.FactControls 1.0 +import QGroundControl.Palette 1.0 + /// Mission item edit control Rectangle { property var missionItem - width: _editFieldWidth + (ScreenTools.defaultFontPixelWidth * 10) - height: _valueColumn.y + _valueColumn.height + (radius / 2) - border.width: 2 - border.color: "white" - color: "white" - radius: ScreenTools.defaultFontPixelWidth - - readonly property real _editFieldWidth: ScreenTools.defaultFontPixelWidth * 13 - - MissionItemIndexLabel { - id: _label - anchors.top: parent.top - anchors.right: parent.right - isCurrentItem: missionItem.isCurrentItem - label: missionItem.sequenceNumber - } - - QGCComboBox { - id: _commandCombo - anchors.margins: parent.radius / 2 - anchors.left: parent.left - anchors.right: _label.left - anchors.top: parent.top - currentIndex: missionItem.commandByIndex - model: missionItem.commandNames + height: ((missionItem.factCount + 3) * (latitudeField.height + _margin)) + commandPicker.height + (_margin * 5) + color: missionItem.isCurrentItem ? qgcPal.buttonHighlight : qgcPal.windowShade - onActivated: missionItem.commandByIndex = index - } - Column { - id: _coordinateColumn - anchors.left: parent.left - anchors.right: parent.right - visible: missionItem.specifiesCoordinate + readonly property real _editFieldWidth: ScreenTools.defaultFontPixelWidth * 13 + readonly property real _margin: ScreenTools.defaultFontPixelWidth / 3 + QGCPalette { + id: qgcPal + colorGroupEnabled: enabled } - QGCTextField { - id: _latitudeField - anchors.margins: parent.radius / 2 - anchors.top: _commandCombo.bottom - anchors.right: parent.right - width: _editFieldWidth - text: missionItem.coordinate.latitude - visible: missionItem.specifiesCoordinate + Item { + anchors.margins: _margin + anchors.fill: parent - onAccepted: missionItem.coordinate.latitude = text - } + MissionItemIndexLabel { + id: label + isCurrentItem: missionItem.isCurrentItem + label: missionItem.sequenceNumber + } - QGCTextField { - id: _longitudeField - anchors.margins: parent.radius / 2 - anchors.top: _latitudeField.bottom - anchors.right: parent.right - width: _editFieldWidth - text: missionItem.coordinate.longitude - visible: missionItem.specifiesCoordinate + QGCComboBox { + id: commandPicker + anchors.leftMargin: ScreenTools.defaultFontPixelWidth * 4 + anchors.left: label.right + anchors.right: parent.right + currentIndex: missionItem.commandByIndex + model: missionItem.commandNames - onAccepted: missionItem.coordinate.longtitude = text - } + onActivated: missionItem.commandByIndex = index + } - QGCTextField { - id: _altitudeField - anchors.margins: parent.radius / 2 - anchors.top: _longitudeField.bottom - anchors.right: parent.right - width: _editFieldWidth - text: missionItem.coordinate.altitude - visible: missionItem.specifiesCoordinate - showUnits: true - unitsLabel: "meters" - - onAccepted: missionItem.coordinate.altitude = text - } + Rectangle { + anchors.margins: _margin + anchors.top: commandPicker.bottom + anchors.bottom: parent.bottom + anchors.left: parent.left + anchors.right: parent.right + color: qgcPal.windowShadeDark - QGCLabel { - anchors.margins: parent.radius / 2 - anchors.left: parent.left - anchors.baseline: _latitudeField.baseline - color: "black" - text: "Lat:" - visible: missionItem.specifiesCoordinate - } + Item { + anchors.margins: _margin + anchors.fill: parent - QGCLabel { - anchors.margins: parent.radius / 2 - anchors.left: parent.left - anchors.baseline: _longitudeField.baseline - color: "black" - text: "Long:" - visible: missionItem.specifiesCoordinate - } + QGCTextField { + id: latitudeField + anchors.right: parent.right + width: _editFieldWidth + text: missionItem.coordinate.latitude + visible: missionItem.specifiesCoordinate - QGCLabel { - anchors.margins: parent.radius / 2 - anchors.left: parent.left - anchors.baseline: _altitudeField.baseline - color: "black" - text: "Alt:" - visible: missionItem.specifiesCoordinate - } + onAccepted: missionItem.coordinate.latitude = text + } - Column { - id: _valueColumn - anchors.margins: parent.radius / 2 - anchors.left: parent.left - anchors.right: parent.right - anchors.top: missionItem.specifiesCoordinate ? _altitudeField.bottom : _commandCombo.bottom - spacing: parent.radius / 2 + QGCTextField { + id: longitudeField + anchors.topMargin: _margin + anchors.top: latitudeField.bottom + anchors.right: parent.right + width: _editFieldWidth + text: missionItem.coordinate.longitude + visible: missionItem.specifiesCoordinate - Repeater { - model: missionItem.facts + onAccepted: missionItem.coordinate.longtitude = text + } - Item { - width: _valueColumn.width - height: textField.height + QGCTextField { + id: altitudeField + anchors.topMargin: _margin + anchors.top: longitudeField.bottom + anchors.right: parent.right + width: _editFieldWidth + text: missionItem.coordinate.altitude + visible: missionItem.specifiesCoordinate + showUnits: true + unitsLabel: "meters" + + onAccepted: missionItem.coordinate.altitude = text + } QGCLabel { - anchors.baseline: textField.baseline - color: "black" - text: object.name + anchors.left: parent.left + anchors.baseline: latitudeField.baseline + text: "Lat:" + visible: missionItem.specifiesCoordinate } - FactTextField { - id: textField - anchors.right: parent.right - width: _editFieldWidth - showUnits: true - fact: object + QGCLabel { + anchors.left: parent.left + anchors.baseline: longitudeField.baseline + text: "Long:" + visible: missionItem.specifiesCoordinate } - } - } - } // Column - Values column + + QGCLabel { + anchors.left: parent.left + anchors.baseline: altitudeField.baseline + text: "Alt:" + visible: missionItem.specifiesCoordinate + } + + Column { + id: valueColumn + anchors.topMargin: _margin + anchors.left: parent.left + anchors.right: parent.right + anchors.top: missionItem.specifiesCoordinate ? altitudeField.bottom : commandPicker.bottom + spacing: _margin + + Repeater { + model: missionItem.facts + + Item { + width: valueColumn.width + height: textField.height + + QGCLabel { + anchors.baseline: textField.baseline + text: object.name + } + + FactTextField { + id: textField + anchors.right: parent.right + width: _editFieldWidth + showUnits: true + fact: object + } + } + } + } // Column - Values column + } // Item + } // Rectangle + } // Item } // Rectangle diff --git a/src/QmlControls/QmlObjectListModel.cc b/src/QmlControls/QmlObjectListModel.cc index 24d397d5382eb637d3aa24cf1c39c0f068264321..6d89c0a45aab1dc2c4e5827790ead0567ba5342d 100644 --- a/src/QmlControls/QmlObjectListModel.cc +++ b/src/QmlControls/QmlObjectListModel.cc @@ -112,7 +112,12 @@ bool QmlObjectListModel::removeRows(int position, int rows, const QModelIndex& p return true; } -QObject*& QmlObjectListModel::operator[](int index) +QObject* QmlObjectListModel::operator[](int index) +{ + return _objectList[index]; +} + +const QObject* QmlObjectListModel::operator[](int index) const { return _objectList[index]; } @@ -135,7 +140,7 @@ void QmlObjectListModel::append(QObject* object) insertRows(_objectList.count() - 1, 1); } -int QmlObjectListModel::count(void) +int QmlObjectListModel::count(void) const { return rowCount(); } diff --git a/src/QmlControls/QmlObjectListModel.h b/src/QmlControls/QmlObjectListModel.h index 52b86fbebe782d5d20249ae0fbf065b286bf7765..8f6cd946e6129e23784530d8a75af99bbfe78426 100644 --- a/src/QmlControls/QmlObjectListModel.h +++ b/src/QmlControls/QmlObjectListModel.h @@ -38,11 +38,12 @@ public: Q_PROPERTY(int count READ count NOTIFY countChanged) - int count(void); + int count(void) const; void append(QObject* object); void clear(void); void removeAt(int i); - QObject*& operator[](int i); + QObject* operator[](int i); + const QObject* operator[](int i) const; // Overrides from QAbstractListModel virtual int rowCount(const QModelIndex & parent = QModelIndex()) const; diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 0bd2c7a5e6a0eb533409192514d9e52835918dea..b43c78cc44401e7e8cee17a9abfdd5cbdb1f5718 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -30,6 +30,7 @@ #include "UASMessageHandler.h" #include "UAS.h" #include "JoystickManager.h" +#include "MissionManager.h" QGC_LOGGING_CATEGORY(VehicleLog, "VehicleLog") @@ -151,6 +152,10 @@ Vehicle::Vehicle(LinkInterface* link, int vehicleId, MAV_AUTOPILOT firmwareType) _waypointViewOnlyListChanged(); _loadSettings(); + + if (qgcApp()->useNewMissionEditor()) { + _missionManager = new MissionManager(this); + } } Vehicle::~Vehicle() @@ -187,7 +192,7 @@ Vehicle::~Vehicle() void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message) { - if (message.sysid != _id) { + if (message.sysid != _id && message.sysid != 0) { return; } @@ -195,6 +200,8 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes _addLink(link); } + emit mavlinkMessageReceived(message); + _uas->receiveMessage(message); } diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 289339ee22c65de5460863a085164c68af7d7e0b..06a362a3e51d500331ed15f780037b566c2f9101 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -41,6 +41,7 @@ class UASInterface; class FirmwarePlugin; class AutoPilotPlugin; class UASWaypointManager; +class MissionManager; Q_DECLARE_LOGGING_CATEGORY(VehicleLog) @@ -55,7 +56,8 @@ public: Q_PROPERTY(int id READ id CONSTANT) Q_PROPERTY(AutoPilotPlugin* autopilot MEMBER _autopilotPlugin CONSTANT) - Q_PROPERTY(QGeoCoordinate coordinate MEMBER _geoCoordinate NOTIFY coordinateChanged) + Q_PROPERTY(QGeoCoordinate coordinate MEMBER _geoCoordinate NOTIFY coordinateChanged) + Q_PROPERTY(MissionManager* missionManager MEMBER _missionManager CONSTANT) Q_INVOKABLE QString getMavIconColor(); @@ -147,6 +149,8 @@ public: int manualControlReservedButtonCount(void); + MissionManager* missionManager(void) { return _missionManager; } + typedef enum { MessageNone, MessageNormal, @@ -213,6 +217,7 @@ signals: void joystickModeChanged(int mode); void joystickEnabledChanged(bool enabled); void activeChanged(bool active); + void mavlinkMessageReceived(const mavlink_message_t& message); /// Used internally to move sendMessage call to main thread void _sendMessageOnThread(mavlink_message_t message); @@ -351,6 +356,7 @@ private: UASWaypointManager* _wpm; int _updateCount; + MissionManager* _missionManager; QmlObjectListModel _missionItems; static const char* _settingsGroup; diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index c4e4616f6d08caac5118fb9ba116032033c0d614..b0d3cec41ea314b4771d0580f09975b88bfc4ee7 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -132,7 +132,6 @@ UAS::UAS(MAVLinkProtocol* protocol, Vehicle* vehicle) : UASInterface(), airSpeed(std::numeric_limits::quiet_NaN()), groundSpeed(std::numeric_limits::quiet_NaN()), - waypointManager(vehicle, this), fileManager(this, vehicle), attitudeKnown(false), @@ -179,9 +178,14 @@ UAS::UAS(MAVLinkProtocol* protocol, Vehicle* vehicle) : UASInterface(), lastSendTimeGPS(0), lastSendTimeSensors(0), lastSendTimeOpticalFlow(0), + _waypointManager(NULL), _vehicle(vehicle) { + if (!qgcApp()->useNewMissionEditor()) { + _waypointManager = new UASWaypointManager(vehicle, this); + } + for (unsigned int i = 0; i<255;++i) { componentID[i] = -1; @@ -1070,144 +1074,156 @@ void UAS::receiveMessage(mavlink_message_t message) break; case MAVLINK_MSG_ID_MISSION_COUNT: { - mavlink_mission_count_t mc; - mavlink_msg_mission_count_decode(&message, &mc); + if (!qgcApp()->useNewMissionEditor()) { + mavlink_mission_count_t mc; + mavlink_msg_mission_count_decode(&message, &mc); - // Special case a 0 for the target system or component, it means that anyone is the target, so we should process this. - if (mc.target_system == 0) { - mc.target_system = mavlink->getSystemId(); - } - if (mc.target_component == 0) { - mc.target_component = mavlink->getComponentId(); - } + // Special case a 0 for the target system or component, it means that anyone is the target, so we should process this. + if (mc.target_system == 0) { + mc.target_system = mavlink->getSystemId(); + } + if (mc.target_component == 0) { + mc.target_component = mavlink->getComponentId(); + } - // Check that this message applies to the UAS. - if(mc.target_system == mavlink->getSystemId()) - { + // Check that this message applies to the UAS. + if(mc.target_system == mavlink->getSystemId()) + { - if (mc.target_component != mavlink->getComponentId()) { - qDebug() << "The target component ID is not set correctly. This is currently only a warning, but will be turned into an error."; - qDebug() << "Expecting" << mavlink->getComponentId() << "but got" << mc.target_component; - } + if (mc.target_component != mavlink->getComponentId()) { + qDebug() << "The target component ID is not set correctly. This is currently only a warning, but will be turned into an error."; + qDebug() << "Expecting" << mavlink->getComponentId() << "but got" << mc.target_component; + } - waypointManager.handleWaypointCount(message.sysid, message.compid, mc.count); - } - else - { - qDebug() << QString("Received mission count message, but was wrong system id. Expected %1, received %2").arg(mavlink->getSystemId()).arg(mc.target_system); + _waypointManager->handleWaypointCount(message.sysid, message.compid, mc.count); + } + else + { + qDebug() << QString("Received mission count message, but was wrong system id. Expected %1, received %2").arg(mavlink->getSystemId()).arg(mc.target_system); + } } } break; case MAVLINK_MSG_ID_MISSION_ITEM: { - mavlink_mission_item_t mi; - mavlink_msg_mission_item_decode(&message, &mi); + if (!qgcApp()->useNewMissionEditor()) { + mavlink_mission_item_t mi; + mavlink_msg_mission_item_decode(&message, &mi); - // Special case a 0 for the target system or component, it means that anyone is the target, so we should process this. - if (mi.target_system == 0) { - mi.target_system = mavlink->getSystemId(); - } - if (mi.target_component == 0) { - mi.target_component = mavlink->getComponentId(); - } + // Special case a 0 for the target system or component, it means that anyone is the target, so we should process this. + if (mi.target_system == 0) { + mi.target_system = mavlink->getSystemId(); + } + if (mi.target_component == 0) { + mi.target_component = mavlink->getComponentId(); + } - // Check that the item pertains to this UAS. - if(mi.target_system == mavlink->getSystemId()) - { + // Check that the item pertains to this UAS. + if(mi.target_system == mavlink->getSystemId()) + { - if (mi.target_component != mavlink->getComponentId()) { - qDebug() << "The target component ID is not set correctly. This is currently only a warning, but will be turned into an error."; - qDebug() << "Expecting" << mavlink->getComponentId() << "but got" << mi.target_component; - } + if (mi.target_component != mavlink->getComponentId()) { + qDebug() << "The target component ID is not set correctly. This is currently only a warning, but will be turned into an error."; + qDebug() << "Expecting" << mavlink->getComponentId() << "but got" << mi.target_component; + } - waypointManager.handleWaypoint(message.sysid, message.compid, &mi); - } - else - { - qDebug() << QString("Received mission item message, but was wrong system id. Expected %1, received %2").arg(mavlink->getSystemId()).arg(mi.target_system); + _waypointManager->handleWaypoint(message.sysid, message.compid, &mi); + } + else + { + qDebug() << QString("Received mission item message, but was wrong system id. Expected %1, received %2").arg(mavlink->getSystemId()).arg(mi.target_system); + } } } break; case MAVLINK_MSG_ID_MISSION_ACK: { - mavlink_mission_ack_t ma; - mavlink_msg_mission_ack_decode(&message, &ma); + if (!qgcApp()->useNewMissionEditor()) { + mavlink_mission_ack_t ma; + mavlink_msg_mission_ack_decode(&message, &ma); - // Special case a 0 for the target system or component, it means that anyone is the target, so we should process this. - if (ma.target_system == 0) { - ma.target_system = mavlink->getSystemId(); - } - if (ma.target_component == 0) { - ma.target_component = mavlink->getComponentId(); - } + // Special case a 0 for the target system or component, it means that anyone is the target, so we should process this. + if (ma.target_system == 0) { + ma.target_system = mavlink->getSystemId(); + } + if (ma.target_component == 0) { + ma.target_component = mavlink->getComponentId(); + } - // Check that the ack pertains to this UAS. - if(ma.target_system == mavlink->getSystemId()) - { + // Check that the ack pertains to this UAS. + if(ma.target_system == mavlink->getSystemId()) + { - if (ma.target_component != mavlink->getComponentId()) { - qDebug() << tr("The target component ID is not set correctly. This is currently only a warning, but will be turned into an error."); - qDebug() << "Expecting" << mavlink->getComponentId() << "but got" << ma.target_component; - } + if (ma.target_component != mavlink->getComponentId()) { + qDebug() << tr("The target component ID is not set correctly. This is currently only a warning, but will be turned into an error."); + qDebug() << "Expecting" << mavlink->getComponentId() << "but got" << ma.target_component; + } - waypointManager.handleWaypointAck(message.sysid, message.compid, &ma); - } - else - { - qDebug() << QString("Received mission ack message, but was wrong system id. Expected %1, received %2").arg(mavlink->getSystemId()).arg(ma.target_system); + _waypointManager->handleWaypointAck(message.sysid, message.compid, &ma); + } + else + { + qDebug() << QString("Received mission ack message, but was wrong system id. Expected %1, received %2").arg(mavlink->getSystemId()).arg(ma.target_system); + } } } break; case MAVLINK_MSG_ID_MISSION_REQUEST: { - mavlink_mission_request_t mr; - mavlink_msg_mission_request_decode(&message, &mr); + if (!qgcApp()->useNewMissionEditor()) { + mavlink_mission_request_t mr; + mavlink_msg_mission_request_decode(&message, &mr); - // Special case a 0 for the target system or component, it means that anyone is the target, so we should process this. - if (mr.target_system == 0) { - mr.target_system = mavlink->getSystemId(); - } - if (mr.target_component == 0) { - mr.target_component = mavlink->getComponentId(); - } + // Special case a 0 for the target system or component, it means that anyone is the target, so we should process this. + if (mr.target_system == 0) { + mr.target_system = mavlink->getSystemId(); + } + if (mr.target_component == 0) { + mr.target_component = mavlink->getComponentId(); + } - // Check that the request pertains to this UAS. - if(mr.target_system == mavlink->getSystemId()) - { + // Check that the request pertains to this UAS. + if(mr.target_system == mavlink->getSystemId()) + { - if (mr.target_component != mavlink->getComponentId()) { - qDebug() << QString("The target component ID is not set correctly. This is currently only a warning, but will be turned into an error."); - qDebug() << "Expecting" << mavlink->getComponentId() << "but got" << mr.target_component; - } + if (mr.target_component != mavlink->getComponentId()) { + qDebug() << QString("The target component ID is not set correctly. This is currently only a warning, but will be turned into an error."); + qDebug() << "Expecting" << mavlink->getComponentId() << "but got" << mr.target_component; + } - waypointManager.handleWaypointRequest(message.sysid, message.compid, &mr); - } - else - { - qDebug() << QString("Received mission request message, but was wrong system id. Expected %1, received %2").arg(mavlink->getSystemId()).arg(mr.target_system); + _waypointManager->handleWaypointRequest(message.sysid, message.compid, &mr); + } + else + { + qDebug() << QString("Received mission request message, but was wrong system id. Expected %1, received %2").arg(mavlink->getSystemId()).arg(mr.target_system); + } } } break; case MAVLINK_MSG_ID_MISSION_ITEM_REACHED: { - mavlink_mission_item_reached_t wpr; - mavlink_msg_mission_item_reached_decode(&message, &wpr); - waypointManager.handleWaypointReached(message.sysid, message.compid, &wpr); - QString text = QString("System %1 reached waypoint %2").arg(getUASID()).arg(wpr.seq); - _say(text); - emit textMessageReceived(message.sysid, message.compid, MAV_SEVERITY_INFO, text); + if (!qgcApp()->useNewMissionEditor()) { + mavlink_mission_item_reached_t wpr; + mavlink_msg_mission_item_reached_decode(&message, &wpr); + _waypointManager->handleWaypointReached(message.sysid, message.compid, &wpr); + QString text = QString("System %1 reached waypoint %2").arg(getUASID()).arg(wpr.seq); + _say(text); + emit textMessageReceived(message.sysid, message.compid, MAV_SEVERITY_INFO, text); + } } break; case MAVLINK_MSG_ID_MISSION_CURRENT: { - mavlink_mission_current_t wpc; - mavlink_msg_mission_current_decode(&message, &wpc); - waypointManager.handleWaypointCurrent(message.sysid, message.compid, &wpc); + if (!qgcApp()->useNewMissionEditor()) { + mavlink_mission_current_t wpc; + mavlink_msg_mission_current_decode(&message, &wpc); + _waypointManager->handleWaypointCurrent(message.sysid, message.compid, &wpc); + } } break; diff --git a/src/uas/UAS.h b/src/uas/UAS.h index 39862eb348c23c723659e2f1f69585c7fb6a9402..e1287f66afb24bc4c7dcc510cb147b56fb5156ea 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -485,7 +485,6 @@ protected: //COMMENTS FOR TEST UNIT double airSpeed; ///< Airspeed double groundSpeed; ///< Groundspeed double bearingToWaypoint; ///< Bearing to next waypoint - UASWaypointManager waypointManager; FileManager fileManager; /// ATTITUDE @@ -552,7 +551,7 @@ public: /** @brief Get reference to the waypoint manager **/ UASWaypointManager* getWaypointManager() { - return &waypointManager; + return _waypointManager; } virtual FileManager* getFileManager() { @@ -985,7 +984,8 @@ private: void _say(const QString& text, int severity = 6); private: - Vehicle* _vehicle; + UASWaypointManager* _waypointManager; + Vehicle* _vehicle; }; diff --git a/src/uas/UASWaypointManager.cc b/src/uas/UASWaypointManager.cc index 41516446f34a58ea19e3ee6f2952b06dbb85a508..bc48bdae063c6d19493ddb987c1af71e85509456 100644 --- a/src/uas/UASWaypointManager.cc +++ b/src/uas/UASWaypointManager.cc @@ -114,7 +114,7 @@ void UASWaypointManager::handleLocalPositionChanged(UASInterface* mav, double x, { Q_UNUSED(mav); Q_UNUSED(time); - if (waypointsEditable.count() > 0 && !currentWaypointEditable.isNull() && (currentWaypointEditable->getFrame() == MAV_FRAME_LOCAL_NED || currentWaypointEditable->getFrame() == MAV_FRAME_LOCAL_ENU)) + if (waypointsEditable.count() > 0 && !currentWaypointEditable.isNull() && (currentWaypointEditable->frame() == MAV_FRAME_LOCAL_NED || currentWaypointEditable->frame() == MAV_FRAME_LOCAL_ENU)) { double xdiff = x-currentWaypointEditable->x(); double ydiff = y-currentWaypointEditable->y(); @@ -132,7 +132,7 @@ void UASWaypointManager::handleGlobalPositionChanged(UASInterface* mav, double l Q_UNUSED(altWGS84); Q_UNUSED(lon); Q_UNUSED(lat); - if (waypointsEditable.count() > 0 && !currentWaypointEditable.isNull() && (currentWaypointEditable->getFrame() == MAV_FRAME_GLOBAL || currentWaypointEditable->getFrame() == MAV_FRAME_GLOBAL_RELATIVE_ALT)) + if (waypointsEditable.count() > 0 && !currentWaypointEditable.isNull() && (currentWaypointEditable->frame() == MAV_FRAME_GLOBAL || currentWaypointEditable->frame() == MAV_FRAME_GLOBAL_RELATIVE_ALT)) { // TODO FIXME Calculate distance double dist = 0; @@ -250,7 +250,7 @@ void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_ emit updateStatusString(tr("MissionItem ID mismatch, rejecting waypoint")); } } else if (systemId == current_partner_systemid - && wp->seq < waypointsViewOnly.size() && waypointsViewOnly[wp->seq]->getAction()) { + && wp->seq < waypointsViewOnly.size() && waypointsViewOnly[wp->seq]->command()) { // accept single sent waypoints because they can contain updates about remaining DO_JUMP repetitions // but only update view only side MissionItem *lwp_vo = new MissionItem(NULL, @@ -677,7 +677,7 @@ const QList UASWaypointManager::getGlobalFrameWaypointList() QList wps; foreach (MissionItem* wp, waypointsEditable) { - if (wp->getFrame() == MAV_FRAME_GLOBAL || wp->getFrame() == MAV_FRAME_GLOBAL_RELATIVE_ALT) + if (wp->frame() == MAV_FRAME_GLOBAL || wp->frame() == MAV_FRAME_GLOBAL_RELATIVE_ALT) { wps.append(wp); } @@ -693,7 +693,7 @@ const QList UASWaypointManager::getGlobalFrameAndNavTypeWaypointL QList wps; foreach (MissionItem* wp, waypointsEditable) { - if ((wp->getFrame() == MAV_FRAME_GLOBAL || wp->getFrame() == MAV_FRAME_GLOBAL_RELATIVE_ALT) && wp->isNavigationType()) + if ((wp->frame() == MAV_FRAME_GLOBAL || wp->frame() == MAV_FRAME_GLOBAL_RELATIVE_ALT) && wp->isNavigationType()) { wps.append(wp); } @@ -728,7 +728,7 @@ int UASWaypointManager::getGlobalFrameIndexOf(MissionItem* wp) // counting only those in global frame int i = 0; foreach (MissionItem* p, waypointsEditable) { - if (p->getFrame() == MAV_FRAME_GLOBAL || wp->getFrame() == MAV_FRAME_GLOBAL_RELATIVE_ALT) + if (p->frame() == MAV_FRAME_GLOBAL || wp->frame() == MAV_FRAME_GLOBAL_RELATIVE_ALT) { if (p == wp) { @@ -747,7 +747,7 @@ int UASWaypointManager::getGlobalFrameAndNavTypeIndexOf(MissionItem* wp) // counting only those in global frame int i = 0; foreach (MissionItem* p, waypointsEditable) { - if ((p->getFrame() == MAV_FRAME_GLOBAL || wp->getFrame() == MAV_FRAME_GLOBAL_RELATIVE_ALT) && p->isNavigationType()) + if ((p->frame() == MAV_FRAME_GLOBAL || wp->frame() == MAV_FRAME_GLOBAL_RELATIVE_ALT) && p->isNavigationType()) { if (p == wp) { @@ -787,7 +787,7 @@ int UASWaypointManager::getGlobalFrameCount() int i = 0; foreach (MissionItem* p, waypointsEditable) { - if (p->getFrame() == MAV_FRAME_GLOBAL || p->getFrame() == MAV_FRAME_GLOBAL_RELATIVE_ALT) + if (p->frame() == MAV_FRAME_GLOBAL || p->frame() == MAV_FRAME_GLOBAL_RELATIVE_ALT) { i++; } @@ -802,7 +802,7 @@ int UASWaypointManager::getGlobalFrameAndNavTypeCount() // counting only those in global frame int i = 0; foreach (MissionItem* p, waypointsEditable) { - if ((p->getFrame() == MAV_FRAME_GLOBAL || p->getFrame() == MAV_FRAME_GLOBAL_RELATIVE_ALT) && p->isNavigationType()) + if ((p->frame() == MAV_FRAME_GLOBAL || p->frame() == MAV_FRAME_GLOBAL_RELATIVE_ALT) && p->isNavigationType()) { i++; } @@ -832,7 +832,7 @@ int UASWaypointManager::getLocalFrameCount() int i = 0; foreach (MissionItem* p, waypointsEditable) { - if (p->getFrame() == MAV_FRAME_LOCAL_NED || p->getFrame() == MAV_FRAME_LOCAL_ENU) + if (p->frame() == MAV_FRAME_LOCAL_NED || p->frame() == MAV_FRAME_LOCAL_ENU) { i++; } @@ -848,7 +848,7 @@ int UASWaypointManager::getLocalFrameIndexOf(MissionItem* wp) int i = 0; foreach (MissionItem* p, waypointsEditable) { - if (p->getFrame() == MAV_FRAME_LOCAL_NED || p->getFrame() == MAV_FRAME_LOCAL_ENU) + if (p->frame() == MAV_FRAME_LOCAL_NED || p->frame() == MAV_FRAME_LOCAL_ENU) { if (p == wp) { @@ -868,7 +868,7 @@ int UASWaypointManager::getMissionFrameIndexOf(MissionItem* wp) int i = 0; foreach (MissionItem* p, waypointsEditable) { - if (p->getFrame() == MAV_FRAME_MISSION) + if (p->frame() == MAV_FRAME_MISSION) { if (p == wp) { @@ -941,12 +941,12 @@ void UASWaypointManager::goToWaypoint(MissionItem *wp) mission.autocontinue = 0; mission.current = 2; //2 for guided mode - mission.param1 = wp->getParam1(); - mission.param2 = wp->getParam2(); - mission.param3 = wp->getParam3(); - mission.param4 = wp->getParam4(); - mission.frame = wp->getFrame(); - mission.command = wp->getAction(); + mission.param1 = wp->param1(); + mission.param2 = wp->param2(); + mission.param3 = wp->param3(); + mission.param4 = wp->param4(); + mission.frame = wp->frame(); + mission.command = wp->command(); mission.seq = 0; // don't read out the sequence number of the waypoint class mission.x = wp->x(); mission.y = wp->y(); @@ -992,14 +992,14 @@ void UASWaypointManager::writeWaypoints() memset(cur_d, 0, sizeof(mavlink_mission_item_t)); //initialize with zeros const MissionItem *cur_s = waypointsEditable.at(i); - cur_d->autocontinue = cur_s->getAutoContinue(); + cur_d->autocontinue = cur_s->autoContinue(); cur_d->current = cur_s->isCurrentItem() & noCurrent; //make sure only one current waypoint is selected, the first selected will be chosen - cur_d->param1 = cur_s->getParam1(); - cur_d->param2 = cur_s->getParam2(); - cur_d->param3 = cur_s->getParam3(); - cur_d->param4 = cur_s->getParam4(); - cur_d->frame = cur_s->getFrame(); - cur_d->command = cur_s->getAction(); + cur_d->param1 = cur_s->param1(); + cur_d->param2 = cur_s->param2(); + cur_d->param3 = cur_s->param3(); + cur_d->param4 = cur_s->param4(); + cur_d->frame = cur_s->frame(); + cur_d->command = cur_s->command(); cur_d->seq = i; // don't read out the sequence number of the waypoint class cur_d->x = cur_s->x(); cur_d->y = cur_s->y(); @@ -1163,7 +1163,7 @@ float UASWaypointManager::getAltitudeRecommendation() int UASWaypointManager::getFrameRecommendation() { if (waypointsEditable.count() > 0) { - return static_cast(waypointsEditable.last()->getFrame()); + return static_cast(waypointsEditable.last()->frame()); } else { return MAV_FRAME_GLOBAL; } @@ -1173,7 +1173,7 @@ float UASWaypointManager::getAcceptanceRadiusRecommendation() { if (waypointsEditable.count() > 0) { - return waypointsEditable.last()->getAcceptanceRadius(); + return waypointsEditable.last()->acceptanceRadius(); } else { diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc index cce397a39a962c34986a2e0e14d804e5165448ed..5f5c2594af2e6a5b331d37337d021791dfeec821 100644 --- a/src/ui/HSIDisplay.cc +++ b/src/ui/HSIDisplay.cc @@ -1312,7 +1312,7 @@ void HSIDisplay::drawWaypoint(QPainter& painter, const QColor& color, float widt poly.replace(3, QPointF(p.x() - waypointSize/2.0f, p.y())); float radius = (waypointSize/2.0f) * 0.8 * (1/sqrt(2.0f)); - float acceptRadius = w->getAcceptanceRadius(); + float acceptRadius = w->acceptanceRadius(); double yawDiff = w->yawRadians()/180.0*M_PI-yaw; // Draw background @@ -1353,7 +1353,7 @@ void HSIDisplay::drawWaypoints(QPainter& painter) const MissionItem *w = list.at(i); QPointF in; // Use local coordinates as-is. - int frameRef = w->getFrame(); + int frameRef = w->frame(); if (frameRef == MAV_FRAME_LOCAL_NED) { in = QPointF(w->x(), w->y()); diff --git a/src/ui/WaypointEditableView.cc b/src/ui/WaypointEditableView.cc index 9974a0787e19f4f644e8426b528f181cc25d8837..88dd17c8d3c9da4256b14a319302a4ad239ffc04 100644 --- a/src/ui/WaypointEditableView.cc +++ b/src/ui/WaypointEditableView.cc @@ -93,13 +93,13 @@ WaypointEditableView::WaypointEditableView(MissionItem* wp, QWidget* parent) : connect(m_ui->selectedBox, SIGNAL(stateChanged(int)), this, SLOT(changedCurrent(int))); // Initialize view correctly - int actionID = wp->getAction(); + int actionID = wp->command(); initializeActionView(actionID); updateValues(); updateActionView(actionID); // Check for mission frame - if (wp->getFrame() == MAV_FRAME_MISSION) + if (wp->frame() == MAV_FRAME_MISSION) { m_ui->comboBox_action->setCurrentIndex(m_ui->comboBox_action->count()-1); } @@ -417,14 +417,14 @@ void WaypointEditableView::updateValues() // update frame - MAV_FRAME frame = (MAV_FRAME)wp->getFrame(); + MAV_FRAME frame = (MAV_FRAME)wp->frame(); int frame_index = m_ui->comboBox_frame->findData(frame); if (m_ui->comboBox_frame->currentIndex() != frame_index) { m_ui->comboBox_frame->setCurrentIndex(frame_index); } // Update action - MAV_CMD action = (MAV_CMD)wp->getAction(); + MAV_CMD action = (MAV_CMD)wp->command(); int action_index = m_ui->comboBox_action->findData(action); if (m_ui->comboBox_action->currentIndex() != action_index) { @@ -440,15 +440,15 @@ void WaypointEditableView::updateValues() } } - emit commandBroadcast(wp->getAction()); - emit frameBroadcast((MAV_FRAME)wp->getFrame()); - emit param1Broadcast(wp->getParam1()); - emit param2Broadcast(wp->getParam2()); - emit param3Broadcast(wp->getParam3()); - emit param4Broadcast(wp->getParam4()); - emit param5Broadcast(wp->getParam5()); - emit param6Broadcast(wp->getParam6()); - emit param7Broadcast(wp->getParam7()); + emit commandBroadcast(wp->command()); + emit frameBroadcast((MAV_FRAME)wp->frame()); + emit param1Broadcast(wp->param1()); + emit param2Broadcast(wp->param2()); + emit param3Broadcast(wp->param3()); + emit param4Broadcast(wp->param4()); + emit param5Broadcast(wp->param5()); + emit param6Broadcast(wp->param6()); + emit param7Broadcast(wp->param7()); if (m_ui->selectedBox->isChecked() != wp->isCurrentItem()) @@ -458,9 +458,9 @@ void WaypointEditableView::updateValues() m_ui->selectedBox->setChecked(wp->isCurrentItem()); m_ui->selectedBox->blockSignals(false); } - if (m_ui->autoContinue->isChecked() != wp->getAutoContinue()) + if (m_ui->autoContinue->isChecked() != wp->autoContinue()) { - m_ui->autoContinue->setChecked(wp->getAutoContinue()); + m_ui->autoContinue->setChecked(wp->autoContinue()); } m_ui->idLabel->setText(QString::number(wp->sequenceNumber())); diff --git a/src/ui/WaypointList.cc b/src/ui/WaypointList.cc index fe13b9f453be46b83b8db3126743bc01248bccbf..d70f94564d8237feb91a919a4f84c85c7f46dab3 100644 --- a/src/ui/WaypointList.cc +++ b/src/ui/WaypointList.cc @@ -268,7 +268,7 @@ void WaypointList::addEditable(bool onCurrentPosition) MissionItem *last = waypoints.last(); wp = WPM->createWaypoint(); // wp->blockSignals(true); - MAV_FRAME frame = (MAV_FRAME)last->getFrame(); + MAV_FRAME frame = (MAV_FRAME)last->frame(); wp->setFrame(frame); if (frame == MAV_FRAME_GLOBAL || frame == MAV_FRAME_GLOBAL_RELATIVE_ALT) { @@ -280,13 +280,13 @@ void WaypointList::addEditable(bool onCurrentPosition) wp->setY(last->y()); wp->setZ(last->z()); } - wp->setParam1(last->getParam1()); - wp->setParam2(last->getParam2()); - wp->setParam3(last->getParam3()); - wp->setParam4(last->getParam4()); - wp->setAutocontinue(last->getAutoContinue()); + wp->setParam1(last->param1()); + wp->setParam2(last->param2()); + wp->setParam3(last->param3()); + wp->setParam4(last->param4()); + wp->setAutocontinue(last->autoContinue()); // wp->blockSignals(false); - wp->setAction(last->getAction()); + wp->setAction(last->command()); // WPM->addWaypointEditable(wp); } else diff --git a/src/ui/WaypointViewOnlyView.cc b/src/ui/WaypointViewOnlyView.cc index b3319e7daca14bcb6bd40a66dc9670229078e07f..5f7d6e871fbc7523d033ff72185f4e0c52b7af56 100644 --- a/src/ui/WaypointViewOnlyView.cc +++ b/src/ui/WaypointViewOnlyView.cc @@ -108,7 +108,7 @@ void WaypointViewOnlyView::updateValues() // update frame m_ui->idLabel->setText(QString("%1").arg(wp->sequenceNumber())); - switch (wp->getFrame()) + switch (wp->frame()) { case MAV_FRAME_GLOBAL: { @@ -148,42 +148,42 @@ void WaypointViewOnlyView::updateValues() m_ui->current->setChecked(wp->isCurrentItem()); m_ui->current->blockSignals(false); } - if (m_ui->autoContinue->isChecked() != wp->getAutoContinue()) + if (m_ui->autoContinue->isChecked() != wp->autoContinue()) { m_ui->autoContinue->blockSignals(true); - m_ui->autoContinue->setChecked(wp->getAutoContinue()); + m_ui->autoContinue->setChecked(wp->autoContinue()); m_ui->autoContinue->blockSignals(false); } - switch (wp->getAction()) + switch (wp->command()) { case MAV_CMD_NAV_WAYPOINT: { - switch (wp->getFrame()) + switch (wp->frame()) { case MAV_FRAME_GLOBAL_RELATIVE_ALT: case MAV_FRAME_GLOBAL: { - if (wp->getParam1()>0) + if (wp->param1()>0) { - m_ui->displayBar->setText(QString("Go to (lat %1o, lon %2o, alt %3) and wait there for %4 sec; yaw: %5; rad: %6").arg(wp->x(),0, 'f', 7).arg(wp->y(),0, 'f', 7).arg(wp->z(),0, 'f', 2).arg(wp->getParam1()).arg(wp->getParam4()).arg(wp->getParam2())); + m_ui->displayBar->setText(QString("Go to (lat %1o, lon %2o, alt %3) and wait there for %4 sec; yaw: %5; rad: %6").arg(wp->x(),0, 'f', 7).arg(wp->y(),0, 'f', 7).arg(wp->z(),0, 'f', 2).arg(wp->param1()).arg(wp->param4()).arg(wp->param2())); } else { - m_ui->displayBar->setText(QString("Go to (lat %1o,lon %2o,alt %3); yaw: %4; rad: %5").arg(wp->x(),0, 'f', 7).arg(wp->y(),0, 'f', 7).arg(wp->z(),0, 'f', 2).arg(wp->getParam4()).arg(wp->getParam2())); + m_ui->displayBar->setText(QString("Go to (lat %1o,lon %2o,alt %3); yaw: %4; rad: %5").arg(wp->x(),0, 'f', 7).arg(wp->y(),0, 'f', 7).arg(wp->z(),0, 'f', 2).arg(wp->param4()).arg(wp->param2())); } break; } case MAV_FRAME_LOCAL_NED: default: { - if (wp->getParam1()>0) + if (wp->param1()>0) { - m_ui->displayBar->setText(QString("Go to (%1, %2, %3) and wait there for %4 sec; yaw: %5; rad: %6").arg(wp->x(),0, 'f', 2).arg(wp->y(),0, 'f', 2).arg(wp->z(),0, 'f',2).arg(wp->getParam1()).arg(wp->getParam4()).arg(wp->getParam2())); + m_ui->displayBar->setText(QString("Go to (%1, %2, %3) and wait there for %4 sec; yaw: %5; rad: %6").arg(wp->x(),0, 'f', 2).arg(wp->y(),0, 'f', 2).arg(wp->z(),0, 'f',2).arg(wp->param1()).arg(wp->param4()).arg(wp->param2())); } else { - m_ui->displayBar->setText(QString("Go to (%1, %2, %3); yaw: %4; rad: %5").arg(wp->x(),0, 'f', 2).arg(wp->y(),0, 'f', 2).arg(wp->z(),0, 'f', 2).arg(wp->getParam4()).arg(wp->getParam2())); + m_ui->displayBar->setText(QString("Go to (%1, %2, %3); yaw: %4; rad: %5").arg(wp->x(),0, 'f', 2).arg(wp->y(),0, 'f', 2).arg(wp->z(),0, 'f', 2).arg(wp->param4()).arg(wp->param2())); } break; } @@ -192,31 +192,31 @@ void WaypointViewOnlyView::updateValues() } case MAV_CMD_NAV_LOITER_UNLIM: { - switch (wp->getFrame()) + switch (wp->frame()) { case MAV_FRAME_GLOBAL_RELATIVE_ALT: case MAV_FRAME_GLOBAL: { - if (wp->getParam3()>=0) + if (wp->param3()>=0) { - m_ui->displayBar->setText(QString("Go to (lat %1o, lon %2o, alt %3) and loiter there indefinitely (clockwise); rad: %4").arg(wp->x(),0, 'f', 7).arg(wp->y(),0, 'f', 7).arg(wp->z(),0, 'f', 2).arg(wp->getParam3())); + m_ui->displayBar->setText(QString("Go to (lat %1o, lon %2o, alt %3) and loiter there indefinitely (clockwise); rad: %4").arg(wp->x(),0, 'f', 7).arg(wp->y(),0, 'f', 7).arg(wp->z(),0, 'f', 2).arg(wp->param3())); } else { - m_ui->displayBar->setText(QString("Go to (lat %1o, lon %2o, alt %3) and loiter there indefinitely (counter-clockwise); rad: %4").arg(wp->x(),0, 'f', 7).arg(wp->y(),0, 'f', 7).arg(wp->z(),0, 'f', 2).arg(-wp->getParam3())); + m_ui->displayBar->setText(QString("Go to (lat %1o, lon %2o, alt %3) and loiter there indefinitely (counter-clockwise); rad: %4").arg(wp->x(),0, 'f', 7).arg(wp->y(),0, 'f', 7).arg(wp->z(),0, 'f', 2).arg(-wp->param3())); } break; } case MAV_FRAME_LOCAL_NED: default: { - if (wp->getParam3()>=0) + if (wp->param3()>=0) { - m_ui->displayBar->setText(QString("Go to (%1, %2, %3) and loiter there indefinitely (clockwise); rad: %4").arg(wp->x(),0, 'f', 2).arg(wp->y(),0, 'f', 2).arg(wp->z(),0, 'f',2).arg(wp->getParam3())); + m_ui->displayBar->setText(QString("Go to (%1, %2, %3) and loiter there indefinitely (clockwise); rad: %4").arg(wp->x(),0, 'f', 2).arg(wp->y(),0, 'f', 2).arg(wp->z(),0, 'f',2).arg(wp->param3())); } else { - m_ui->displayBar->setText(QString("Go to (%1, %2, %3) and loiter there indefinitely (counter-clockwise); rad: %4").arg(wp->x(),0, 'f', 2).arg(wp->y(),0, 'f', 2).arg(wp->z(),0, 'f', 2).arg(-wp->getParam3())); + m_ui->displayBar->setText(QString("Go to (%1, %2, %3) and loiter there indefinitely (counter-clockwise); rad: %4").arg(wp->x(),0, 'f', 2).arg(wp->y(),0, 'f', 2).arg(wp->z(),0, 'f', 2).arg(-wp->param3())); } break; } @@ -225,31 +225,31 @@ void WaypointViewOnlyView::updateValues() } case MAV_CMD_NAV_LOITER_TURNS: { - switch (wp->getFrame()) + switch (wp->frame()) { case MAV_FRAME_GLOBAL_RELATIVE_ALT: case MAV_FRAME_GLOBAL: { - if (wp->getParam3()>=0) + if (wp->param3()>=0) { - m_ui->displayBar->setText(QString("Go to (lat %1o, lon %2o, alt %3) and loiter there for %5 turns (clockwise); rad: %4").arg(wp->x(),0, 'f', 7).arg(wp->y(),0, 'f', 7).arg(wp->z(),0, 'f', 2).arg(wp->getParam3()).arg(wp->getParam1())); + m_ui->displayBar->setText(QString("Go to (lat %1o, lon %2o, alt %3) and loiter there for %5 turns (clockwise); rad: %4").arg(wp->x(),0, 'f', 7).arg(wp->y(),0, 'f', 7).arg(wp->z(),0, 'f', 2).arg(wp->param3()).arg(wp->param1())); } else { - m_ui->displayBar->setText(QString("Go to (lat %1o, lon %2o, alt %3) and loiter there for %5 turns (counter-clockwise); rad: %4").arg(wp->x(),0, 'f', 7).arg(wp->y(),0, 'f', 7).arg(wp->z(),0, 'f', 2).arg(-wp->getParam3()).arg(wp->getParam1())); + m_ui->displayBar->setText(QString("Go to (lat %1o, lon %2o, alt %3) and loiter there for %5 turns (counter-clockwise); rad: %4").arg(wp->x(),0, 'f', 7).arg(wp->y(),0, 'f', 7).arg(wp->z(),0, 'f', 2).arg(-wp->param3()).arg(wp->param1())); } break; } case MAV_FRAME_LOCAL_NED: default: { - if (wp->getParam3()>=0) + if (wp->param3()>=0) { - m_ui->displayBar->setText(QString("Go to (%1, %2, %3) and loiter there for %5 turns (clockwise); rad: %4").arg(wp->x(),0, 'f', 2).arg(wp->y(),0, 'f', 2).arg(wp->z(),0, 'f',2).arg(wp->getParam3()).arg(wp->getParam1())); + m_ui->displayBar->setText(QString("Go to (%1, %2, %3) and loiter there for %5 turns (clockwise); rad: %4").arg(wp->x(),0, 'f', 2).arg(wp->y(),0, 'f', 2).arg(wp->z(),0, 'f',2).arg(wp->param3()).arg(wp->param1())); } else { - m_ui->displayBar->setText(QString("Go to (%1, %2, %3) and loiter there for %5 turns (counter-clockwise); rad: %4").arg(wp->x(),0, 'f', 2).arg(wp->y(),0, 'f', 2).arg(wp->z(),0, 'f', 2).arg(-wp->getParam3()).arg(wp->getParam1())); + m_ui->displayBar->setText(QString("Go to (%1, %2, %3) and loiter there for %5 turns (counter-clockwise); rad: %4").arg(wp->x(),0, 'f', 2).arg(wp->y(),0, 'f', 2).arg(wp->z(),0, 'f', 2).arg(-wp->param3()).arg(wp->param1())); } break; } @@ -258,31 +258,31 @@ void WaypointViewOnlyView::updateValues() } case MAV_CMD_NAV_LOITER_TIME: { - switch (wp->getFrame()) + switch (wp->frame()) { case MAV_FRAME_GLOBAL_RELATIVE_ALT: case MAV_FRAME_GLOBAL: { - if (wp->getParam3()>=0) + if (wp->param3()>=0) { - m_ui->displayBar->setText(QString("Go to (lat %1o, lon %2o, alt %3) and loiter there for %5s (clockwise); rad: %4").arg(wp->x(),0, 'f', 7).arg(wp->y(),0, 'f', 7).arg(wp->z(),0, 'f', 2).arg(wp->getParam3()).arg(wp->getParam1())); + m_ui->displayBar->setText(QString("Go to (lat %1o, lon %2o, alt %3) and loiter there for %5s (clockwise); rad: %4").arg(wp->x(),0, 'f', 7).arg(wp->y(),0, 'f', 7).arg(wp->z(),0, 'f', 2).arg(wp->param3()).arg(wp->param1())); } else { - m_ui->displayBar->setText(QString("Go to (lat %1o, lon %2o, alt %3) and loiter there for %5s (counter-clockwise); rad: %4").arg(wp->x(),0, 'f', 7).arg(wp->y(),0, 'f', 7).arg(wp->z(),0, 'f', 2).arg(-wp->getParam3()).arg(wp->getParam1())); + m_ui->displayBar->setText(QString("Go to (lat %1o, lon %2o, alt %3) and loiter there for %5s (counter-clockwise); rad: %4").arg(wp->x(),0, 'f', 7).arg(wp->y(),0, 'f', 7).arg(wp->z(),0, 'f', 2).arg(-wp->param3()).arg(wp->param1())); } break; } case MAV_FRAME_LOCAL_NED: default: { - if (wp->getParam3()>=0) + if (wp->param3()>=0) { - m_ui->displayBar->setText(QString("Go to (%1, %2, %3) and loiter there for %5s (clockwise); rad: %4").arg(wp->x(),0, 'f', 2).arg(wp->y(),0, 'f', 2).arg(wp->z(),0, 'f',2).arg(wp->getParam3()).arg(wp->getParam1())); + m_ui->displayBar->setText(QString("Go to (%1, %2, %3) and loiter there for %5s (clockwise); rad: %4").arg(wp->x(),0, 'f', 2).arg(wp->y(),0, 'f', 2).arg(wp->z(),0, 'f',2).arg(wp->param3()).arg(wp->param1())); } else { - m_ui->displayBar->setText(QString("Go to (%1, %2, %3) and loiter there for %5s (counter-clockwise); rad: %4").arg(wp->x(),0, 'f', 2).arg(wp->y(),0, 'f', 2).arg(wp->z(),0, 'f', 2).arg(-wp->getParam3()).arg(wp->getParam1())); + m_ui->displayBar->setText(QString("Go to (%1, %2, %3) and loiter there for %5s (counter-clockwise); rad: %4").arg(wp->x(),0, 'f', 2).arg(wp->y(),0, 'f', 2).arg(wp->z(),0, 'f', 2).arg(-wp->param3()).arg(wp->param1())); } break; } @@ -296,18 +296,18 @@ void WaypointViewOnlyView::updateValues() } case MAV_CMD_NAV_LAND: { - switch (wp->getFrame()) + switch (wp->frame()) { case MAV_FRAME_GLOBAL_RELATIVE_ALT: case MAV_FRAME_GLOBAL: { - m_ui->displayBar->setText(QString("LAND. Go to (lat %1o, lon %2o, alt %3) and descent; yaw: %4").arg(wp->x(),0, 'f', 7).arg(wp->y(),0, 'f', 7).arg(wp->z(),0, 'f', 2).arg(wp->getParam4())); + m_ui->displayBar->setText(QString("LAND. Go to (lat %1o, lon %2o, alt %3) and descent; yaw: %4").arg(wp->x(),0, 'f', 7).arg(wp->y(),0, 'f', 7).arg(wp->z(),0, 'f', 2).arg(wp->param4())); break; } case MAV_FRAME_LOCAL_NED: default: { - m_ui->displayBar->setText(QString("LAND. Go to (%1, %2, %3) and descent; yaw: %4").arg(wp->x(),0, 'f', 2).arg(wp->y(),0, 'f', 2).arg(wp->z(),0, 'f', 2).arg(wp->getParam4())); + m_ui->displayBar->setText(QString("LAND. Go to (%1, %2, %3) and descent; yaw: %4").arg(wp->x(),0, 'f', 2).arg(wp->y(),0, 'f', 2).arg(wp->z(),0, 'f', 2).arg(wp->param4())); break; } } //end Frame switch @@ -315,18 +315,18 @@ void WaypointViewOnlyView::updateValues() } case MAV_CMD_NAV_TAKEOFF: { - switch (wp->getFrame()) + switch (wp->frame()) { case MAV_FRAME_GLOBAL_RELATIVE_ALT: case MAV_FRAME_GLOBAL: { - m_ui->displayBar->setText(QString("TAKEOFF. Go to (lat %1o, lon %2o, alt %3); yaw: %4").arg(wp->x(),0, 'f', 7).arg(wp->y(),0, 'f', 7).arg(wp->z(),0, 'f', 2).arg(wp->getParam4())); + m_ui->displayBar->setText(QString("TAKEOFF. Go to (lat %1o, lon %2o, alt %3); yaw: %4").arg(wp->x(),0, 'f', 7).arg(wp->y(),0, 'f', 7).arg(wp->z(),0, 'f', 2).arg(wp->param4())); break; } case MAV_FRAME_LOCAL_NED: default: { - m_ui->displayBar->setText(QString("TAKEOFF. Go to (%1, %2, %3); yaw: %4").arg(wp->x(),0, 'f', 2).arg(wp->y(),0, 'f', 2).arg(wp->z(),0, 'f', 2).arg(wp->getParam4())); + m_ui->displayBar->setText(QString("TAKEOFF. Go to (%1, %2, %3); yaw: %4").arg(wp->x(),0, 'f', 2).arg(wp->y(),0, 'f', 2).arg(wp->z(),0, 'f', 2).arg(wp->param4())); break; } } //end Frame switch @@ -335,9 +335,9 @@ void WaypointViewOnlyView::updateValues() } case MAV_CMD_DO_JUMP: { - if (wp->getParam2()>0) + if (wp->param2()>0) { - m_ui->displayBar->setText(QString("Jump to waypoint %1. Jumps left: %2").arg(wp->getParam1()).arg(wp->getParam2())); + m_ui->displayBar->setText(QString("Jump to waypoint %1. Jumps left: %2").arg(wp->param1()).arg(wp->param2())); } else { @@ -347,12 +347,12 @@ void WaypointViewOnlyView::updateValues() } case MAV_CMD_CONDITION_DELAY: { - m_ui->displayBar->setText(QString("Delay: %1 sec").arg(wp->getParam1())); + m_ui->displayBar->setText(QString("Delay: %1 sec").arg(wp->param1())); break; } default: { - m_ui->displayBar->setText(QString("Unknown Command ID (%1) : %2, %3, %4, %5, %6, %7, %8").arg(wp->getAction()).arg(wp->getParam1()).arg(wp->getParam2()).arg(wp->getParam3()).arg(wp->getParam4()).arg(wp->getParam5()).arg(wp->getParam6()).arg(wp->getParam7())); + m_ui->displayBar->setText(QString("Unknown Command ID (%1) : %2, %3, %4, %5, %6, %7, %8").arg(wp->command()).arg(wp->param1()).arg(wp->param2()).arg(wp->param3()).arg(wp->param4()).arg(wp->param5()).arg(wp->param6()).arg(wp->param7())); break; } } diff --git a/src/ui/map/QGCMapWidget.cc b/src/ui/map/QGCMapWidget.cc index e7c29365a6bfc3e309674db49d9b2a40c3cbba96..fae9bca13dade616171b66266577bf5c15fa2c5e 100644 --- a/src/ui/map/QGCMapWidget.cc +++ b/src/ui/map/QGCMapWidget.cc @@ -678,7 +678,7 @@ void QGCMapWidget::updateWaypoint(int uas, MissionItem* wp) if (currWPManager) { // Only accept waypoints in global coordinate frame - if (((wp->getFrame() == MAV_FRAME_GLOBAL) || (wp->getFrame() == MAV_FRAME_GLOBAL_RELATIVE_ALT)) && wp->isNavigationType()) + if (((wp->frame() == MAV_FRAME_GLOBAL) || (wp->frame() == MAV_FRAME_GLOBAL_RELATIVE_ALT)) && wp->isNavigationType()) { // We're good, this is a global waypoint diff --git a/src/ui/map/Waypoint2DIcon.cc b/src/ui/map/Waypoint2DIcon.cc index ef879e9aeb3d2ccc33730c4897be3693ad53c94f..1681d7f3672f0a44fd76aa4f3efa1c874e48fdd4 100644 --- a/src/ui/map/Waypoint2DIcon.cc +++ b/src/ui/map/Waypoint2DIcon.cc @@ -98,11 +98,11 @@ QRectF Waypoint2DIcon::boundingRect() const internals::PointLatLng coord = (internals::PointLatLng)Coord(); if (!waypoint.isNull()) { - if (showAcceptanceRadius && (waypoint->getAction() == (int)MAV_CMD_NAV_WAYPOINT)) + if (showAcceptanceRadius && (waypoint->command() == (int)MAV_CMD_NAV_WAYPOINT)) { - acceptance = map->metersToPixels(waypoint->getAcceptanceRadius(), coord); + acceptance = map->metersToPixels(waypoint->acceptanceRadius(), coord); } - if (((waypoint->getAction() == (int)MAV_CMD_NAV_LOITER_UNLIM) || (waypoint->getAction() == (int)MAV_CMD_NAV_LOITER_TIME) || (waypoint->getAction() == (int)MAV_CMD_NAV_LOITER_TURNS))) + if (((waypoint->command() == (int)MAV_CMD_NAV_LOITER_UNLIM) || (waypoint->command() == (int)MAV_CMD_NAV_LOITER_TIME) || (waypoint->command() == (int)MAV_CMD_NAV_LOITER_TURNS))) { loiter = map->metersToPixels(waypoint->loiterOrbitRadius(), coord); } @@ -159,12 +159,12 @@ void Waypoint2DIcon::drawIcon() // or it is a waypoint, but not one where direction has no meaning // then draw the heading indicator if (waypoint.isNull() || (waypoint && ( - (waypoint->getAction() != (int)MAV_CMD_NAV_TAKEOFF) && - (waypoint->getAction() != (int)MAV_CMD_NAV_LAND) && - (waypoint->getAction() != (int)MAV_CMD_NAV_LOITER_UNLIM) && - (waypoint->getAction() != (int)MAV_CMD_NAV_LOITER_TIME) && - (waypoint->getAction() != (int)MAV_CMD_NAV_LOITER_TURNS) && - (waypoint->getAction() != (int)MAV_CMD_NAV_RETURN_TO_LAUNCH) + (waypoint->command() != (int)MAV_CMD_NAV_TAKEOFF) && + (waypoint->command() != (int)MAV_CMD_NAV_LAND) && + (waypoint->command() != (int)MAV_CMD_NAV_LOITER_UNLIM) && + (waypoint->command() != (int)MAV_CMD_NAV_LOITER_TIME) && + (waypoint->command() != (int)MAV_CMD_NAV_LOITER_TURNS) && + (waypoint->command() != (int)MAV_CMD_NAV_RETURN_TO_LAUNCH) ))) { painter.setPen(pen1); @@ -173,7 +173,7 @@ void Waypoint2DIcon::drawIcon() painter.drawLine(p.x(), p.y(), p.x()+sin(Heading()/180.0f*M_PI) * rad, p.y()-cos(Heading()/180.0f*M_PI) * rad); } - if (((!waypoint.isNull())) && (waypoint->getAction() == (int)MAV_CMD_NAV_TAKEOFF)) + if (((!waypoint.isNull())) && (waypoint->command() == (int)MAV_CMD_NAV_TAKEOFF)) { // Takeoff waypoint int width = picture.width()-penWidth; @@ -189,7 +189,7 @@ void Waypoint2DIcon::drawIcon() painter.setPen(pen2); painter.drawRect(width*0.3, height*0.3f, width*0.6f, height*0.6f); } - else if (((!waypoint.isNull())) && (waypoint->getAction() == (int)MAV_CMD_NAV_LAND)) + else if (((!waypoint.isNull())) && (waypoint->command() == (int)MAV_CMD_NAV_LAND)) { // Landing waypoint int width = (picture.width())/2-penWidth; @@ -201,7 +201,7 @@ void Waypoint2DIcon::drawIcon() painter.drawEllipse(p, width, height); painter.drawLine(p.x()-width/2, p.y()-height/2, 2*width, 2*height); } - else if (((!waypoint.isNull())) && ((waypoint->getAction() == (int)MAV_CMD_NAV_LOITER_UNLIM) || (waypoint->getAction() == (int)MAV_CMD_NAV_LOITER_TIME) || (waypoint->getAction() == (int)MAV_CMD_NAV_LOITER_TURNS))) + else if (((!waypoint.isNull())) && ((waypoint->command() == (int)MAV_CMD_NAV_LOITER_UNLIM) || (waypoint->command() == (int)MAV_CMD_NAV_LOITER_TIME) || (waypoint->command() == (int)MAV_CMD_NAV_LOITER_TURNS))) { // Loiter waypoint int width = (picture.width()-penWidth)/2; @@ -213,7 +213,7 @@ void Waypoint2DIcon::drawIcon() painter.drawEllipse(p, width, height); painter.drawPoint(p); } - else if (((!waypoint.isNull())) && (waypoint->getAction() == (int)MAV_CMD_NAV_RETURN_TO_LAUNCH)) + else if (((!waypoint.isNull())) && (waypoint->command() == (int)MAV_CMD_NAV_RETURN_TO_LAUNCH)) { // Return to launch waypoint int width = picture.width()-penWidth; @@ -286,13 +286,13 @@ void Waypoint2DIcon::paint(QPainter *painter, const QStyleOptionGraphicsItem *op penBlack.setWidth(4); pen.setColor(color); - if ((!waypoint.isNull()) && (waypoint->getAction() == (int)MAV_CMD_NAV_WAYPOINT) && showAcceptanceRadius) + if ((!waypoint.isNull()) && (waypoint->command() == (int)MAV_CMD_NAV_WAYPOINT) && showAcceptanceRadius) { QPen redPen = QPen(pen); redPen.setColor(Qt::yellow); redPen.setWidth(1); painter->setPen(redPen); - const int acceptance = map->metersToPixels(waypoint->getAcceptanceRadius(), Coord()); + const int acceptance = map->metersToPixels(waypoint->acceptanceRadius(), Coord()); if (acceptance > 0) { painter->setPen(penBlack); painter->drawEllipse(QPointF(0, 0), acceptance, acceptance); @@ -300,7 +300,7 @@ void Waypoint2DIcon::paint(QPainter *painter, const QStyleOptionGraphicsItem *op painter->drawEllipse(QPointF(0, 0), acceptance, acceptance); } } - if ((!waypoint.isNull()) && ((waypoint->getAction() == (int)MAV_CMD_NAV_LOITER_UNLIM) || (waypoint->getAction() == (int)MAV_CMD_NAV_LOITER_TIME) || (waypoint->getAction() == (int)MAV_CMD_NAV_LOITER_TURNS))) + if ((!waypoint.isNull()) && ((waypoint->command() == (int)MAV_CMD_NAV_LOITER_UNLIM) || (waypoint->command() == (int)MAV_CMD_NAV_LOITER_TIME) || (waypoint->command() == (int)MAV_CMD_NAV_LOITER_TURNS))) { QPen penDash(color); penDash.setWidth(1);