diff --git a/src/MissionEditor/MissionEditor.cc b/src/MissionEditor/MissionEditor.cc index 688ee145ce66848936d8fddaee5a1eb41227f079..44087a5289aec6c5e5630978dc3768dc50caa07b 100644 --- a/src/MissionEditor/MissionEditor.cc +++ b/src/MissionEditor/MissionEditor.cc @@ -52,7 +52,7 @@ MissionEditor::MissionEditor(QWidget *parent) _newMissionItemsAvailable(); } else { _missionItems = new QmlObjectListModel(this); - connect(_missionItems, &QmlObjectListModel::dirtyChanged, this, &MissionEditor::_missionListDirtyChanged); + _initAllMissionItems(); } setContextPropertyObject("controller", this); @@ -67,6 +67,7 @@ MissionEditor::~MissionEditor() void MissionEditor::_newMissionItemsAvailable(void) { if (_missionItems) { + _deinitAllMissionItems(); _missionItems->deleteLater(); } @@ -74,14 +75,8 @@ void MissionEditor::_newMissionItemsAvailable(void) _canEdit = missionManager->canEdit(); _missionItems = missionManager->copyMissionItems(); - _reSequence(); - _missionItems->setDirty(false); - - connect(_missionItems, &QmlObjectListModel::dirtyChanged, this, &MissionEditor::_missionListDirtyChanged); - _rebuildWaypointLines(); - emit missionItemsChanged(); - emit canEditChanged(_canEdit); + _initAllMissionItems(); } void MissionEditor::getMissionItems(void) @@ -97,10 +92,11 @@ void MissionEditor::getMissionItems(void) void MissionEditor::setMissionItems(void) { + // FIXME: Need to pull out home position Vehicle* activeVehicle = MultiVehicleManager::instance()->activeVehicle(); if (activeVehicle) { - activeVehicle->missionManager()->writeMissionItems(*_missionItems); + activeVehicle->missionManager()->writeMissionItems(*_missionItems, true /* skipFirstItem */); _missionItems->setDirty(false); } } @@ -112,23 +108,19 @@ int MissionEditor::addMissionItem(QGeoCoordinate coordinate) } MissionItem * newItem = new MissionItem(this, _missionItems->count(), coordinate, MAV_CMD_NAV_WAYPOINT); + _initMissionItem(newItem); newItem->setAltitude(30); - if (_missionItems->count() == 0) { + if (_missionItems->count() == 1) { newItem->setCommand(MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF); } qDebug() << "MissionItem" << newItem->coordinate(); _missionItems->append(newItem); + _recalcAll(); + return _missionItems->count() - 1; } -void MissionEditor::_reSequence(void) -{ - for (int i=0; i<_missionItems->count(); i++) { - qobject_cast(_missionItems->get(i))->setSequenceNumber(i); - } -} - void MissionEditor::removeMissionItem(int index) { if (!_canEdit) { @@ -136,8 +128,11 @@ void MissionEditor::removeMissionItem(int index) return; } - _missionItems->removeAt(index); - _reSequence(); + MissionItem* item = qobject_cast(_missionItems->removeAt(index)); + + _deinitMissionItem(item); + + _recalcAll(); } void MissionEditor::moveUp(int index) @@ -160,7 +155,7 @@ void MissionEditor::moveUp(int index) _missionItems->insert(index - 1, new MissionItem(item2, _missionItems)); _missionItems->insert(index, new MissionItem(item1, _missionItems)); - _reSequence(); + _recalcAll(); } void MissionEditor::moveDown(int index) @@ -183,7 +178,7 @@ void MissionEditor::moveDown(int index) _missionItems->insert(index, new MissionItem(item2, _missionItems)); _missionItems->insert(index + 1, new MissionItem(item1, _missionItems)); - _reSequence(); + _recalcAll(); } void MissionEditor::loadMissionFromFile(void) @@ -196,6 +191,7 @@ void MissionEditor::loadMissionFromFile(void) } if (_missionItems) { + _deinitAllMissionItems(); _missionItems->deleteLater(); } _missionItems = new QmlObjectListModel(this); @@ -236,11 +232,7 @@ void MissionEditor::loadMissionFromFile(void) _missionItems->clear(); } - _missionItems->setDirty(false); - emit canEditChanged(_canEdit); - - connect(_missionItems, &QmlObjectListModel::dirtyChanged, this, &MissionEditor::_missionListDirtyChanged); - _rebuildWaypointLines(); + _initAllMissionItems(); } void MissionEditor::saveMissionToFile(void) @@ -269,20 +261,131 @@ void MissionEditor::saveMissionToFile(void) _missionItems->setDirty(false); } -void MissionEditor::_rebuildWaypointLines(void) +void MissionEditor::_recalcWaypointLines(void) { + bool firstCoordinateItem = true; + MissionItem* lastCoordinateItem = qobject_cast(_missionItems->get(0)); + _waypointLines.clear(); + for (int i=1; i<_missionItems->count(); i++) { - MissionItem* item1 = qobject_cast(_missionItems->get(i-1)); - MissionItem* item2 = qobject_cast(_missionItems->get(i)); + MissionItem* item = qobject_cast(_missionItems->get(i)); - _waypointLines.append(new CoordinateVector(item1->coordinate(), item2->coordinate())); + if (item->specifiesCoordinate()) { + if (firstCoordinateItem) { + if (item->command() == MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF) { + // The first coordinate we hit is a takeoff command so link back to home position + _waypointLines.append(new CoordinateVector(qobject_cast(_missionItems->get(0))->coordinate(), item->coordinate())); + } else { + // First coordiante is not a takeoff command, it does not link backwards to anything + } + firstCoordinateItem = false; + } else { + // Subsequent coordinate items link to last coordinate item + _waypointLines.append(new CoordinateVector(lastCoordinateItem->coordinate(), item->coordinate())); + } + lastCoordinateItem = item; + } } + emit waypointLinesChanged(); } -void MissionEditor::_missionListDirtyChanged(bool dirty) +// This will update the sequence numbers to be sequential starting from 0 +void MissionEditor::_recalcSequence(void) +{ + MissionItem* currentParentItem = qobject_cast(_missionItems->get(0)); + + currentParentItem->childItems()->clear(); + + for (int i=0; i<_missionItems->count(); i++) { + MissionItem* item = qobject_cast(_missionItems->get(i)); + + // Setup ascending sequence numbers + item->setSequenceNumber(i); + } +} + +// This will update the child item hierarchy +void MissionEditor::_recalcChildItems(void) +{ + MissionItem* currentParentItem = qobject_cast(_missionItems->get(0)); + + currentParentItem->childItems()->clear(); + + for (int i=1; i<_missionItems->count(); i++) { + MissionItem* item = qobject_cast(_missionItems->get(i)); + + // Set up non-coordinate item child hierarchy + if (item->specifiesCoordinate()) { + item->childItems()->clear(); + currentParentItem = item; + } else { + currentParentItem->childItems()->append(item); + } + } +} + +void MissionEditor::_recalcAll(void) +{ + _recalcSequence(); + _recalcChildItems(); + _recalcWaypointLines(); +} + +/// Initializes a new set of mission items which may have come from the vehicle or have been loaded from a file +void MissionEditor::_initAllMissionItems(void) +{ + // Add the home position item to the front + MissionItem* homeItem = new MissionItem(this); + homeItem->setHomePositionSpecialCase(true); + homeItem->setCommand(MavlinkQmlSingleton::MAV_CMD_NAV_WAYPOINT); + _missionItems->insert(0, homeItem); + + for (int i=0; i<_missionItems->count(); i++) { + _initMissionItem(qobject_cast(_missionItems->get(i))); + } + + _recalcSequence(); + _recalcChildItems(); + _recalcWaypointLines(); + + emit missionItemsChanged(); + emit canEditChanged(_canEdit); + + _missionItems->setDirty(false); +} + +void MissionEditor::_deinitAllMissionItems(void) +{ + for (int i=0; i<_missionItems->count(); i++) { + _deinitMissionItem(qobject_cast(_missionItems->get(i))); + } +} + +void MissionEditor::_initMissionItem(MissionItem* item) +{ + _missionItems->setDirty(false); + + connect(item, &MissionItem::commandChanged, this, &MissionEditor::_itemCommandChanged); + connect(item, &MissionItem::coordinateChanged, this, &MissionEditor::_itemCoordinateChanged); +} + +void MissionEditor::_deinitMissionItem(MissionItem* item) +{ + disconnect(item, &MissionItem::commandChanged, this, &MissionEditor::_itemCommandChanged); + disconnect(item, &MissionItem::coordinateChanged, this, &MissionEditor::_itemCoordinateChanged); +} + +void MissionEditor::_itemCoordinateChanged(const QGeoCoordinate& coordinate) +{ + Q_UNUSED(coordinate); + _recalcWaypointLines(); +} + +void MissionEditor::_itemCommandChanged(MavlinkQmlSingleton::Qml_MAV_CMD command) { - Q_UNUSED(dirty); - _rebuildWaypointLines(); + Q_UNUSED(command);; + _recalcChildItems(); + _recalcWaypointLines(); } diff --git a/src/MissionEditor/MissionEditor.h b/src/MissionEditor/MissionEditor.h index eed09fc199d7db85054205e70cdbe194373274aa..491423ba8f44e630eff0188e5d13cf537cdd0e98 100644 --- a/src/MissionEditor/MissionEditor.h +++ b/src/MissionEditor/MissionEditor.h @@ -61,12 +61,19 @@ signals: private slots: void _newMissionItemsAvailable(); - void _missionListDirtyChanged(bool dirty); - + void _itemCoordinateChanged(const QGeoCoordinate& coordinate); + void _itemCommandChanged(MavlinkQmlSingleton::Qml_MAV_CMD command); + private: - void _reSequence(void); - void _rebuildWaypointLines(void); - + void _recalcSequence(void); + void _recalcWaypointLines(void); + void _recalcChildItems(void); + void _recalcAll(void); + void _initAllMissionItems(void); + void _deinitAllMissionItems(void); + void _initMissionItem(MissionItem* item); + void _deinitMissionItem(MissionItem* item); + private: QmlObjectListModel* _missionItems; QmlObjectListModel _waypointLines; diff --git a/src/MissionEditor/MissionEditor.qml b/src/MissionEditor/MissionEditor.qml index 9f79fc128b14019b5ed96bdb8a54939052804249..030fabd4e8f255e08e2e5deb9ecb97f580630d87 100644 --- a/src/MissionEditor/MissionEditor.qml +++ b/src/MissionEditor/MissionEditor.qml @@ -32,6 +32,7 @@ import QGroundControl.FlightMap 1.0 import QGroundControl.ScreenTools 1.0 import QGroundControl.Controls 1.0 import QGroundControl.Palette 1.0 +import QGroundControl.Mavlink 1.0 /// Mission Editor @@ -51,7 +52,7 @@ QGCView { property var _homePositionManager: QGroundControl.homePositionManager property string _homePositionName: _homePositionManager.homePositions.get(0).name - property var _homePositionCoordinate: _homePositionManager.homePositions.get(0).coordinate + property var homePositionCoordinate: _homePositionManager.homePositions.get(0).coordinate QGCPalette { id: _qgcPal; colorGroupEnabled: enabled } @@ -75,6 +76,22 @@ QGCView { } } + // Home position is mission item 0, so keep them in sync + onHomePositionCoordinateChanged: { + // Changing the coordinate will set the dirty bit, so we save and reset it + var dirtyBit = _missionItems.dirty + _missionItems.get(0).coordinate = homePositionCoordinate + _missionItems.dirty = dirtyBit + } + + Component.onCompleted: onHomePositionCoordinateChanged + + Connections { + target: controller + + onMissionItemsChanged: _missionItems.get(0).coordinate = homePositionCoordinate + } + QGCViewPanel { id: panel anchors.fill: parent @@ -91,8 +108,8 @@ QGCView { mapName: "MissionEditor" Component.onCompleted: { - latitude = _homePositionCoordinate.latitude - longitude = _homePositionCoordinate.longitude + latitude = homePositionCoordinate.latitude + longitude = homePositionCoordinate.longitude } MouseArea { @@ -104,7 +121,7 @@ QGCView { coordinate.longitude = coordinate.longitude.toFixed(_decimalPlaces) coordinate.altitude = coordinate.altitude.toFixed(_decimalPlaces) if (_showHomePositionManager) { - _homePositionCoordinate = coordinate + homePositionCoordinate = coordinate } else if (_addMissionItems) { var index = controller.addMissionItem(coordinate) setCurrentItem(index) @@ -183,7 +200,7 @@ QGCView { onClicked: { centerMapButton.hideDropDown() - editorMap.center = QtPositioning.coordinate(_homePositionCoordinate.latitude, _homePositionCoordinate.longitude) + editorMap.center = QtPositioning.coordinate(homePositionCoordinate.latitude, homePositionCoordinate.longitude) _showHomePositionManager = true } } @@ -212,8 +229,8 @@ QGCView { centerMapButton.hideDropDown() // Begin with only the home position in the region - var region = QtPositioning.rectangle(QtPositioning.coordinate(_homePositionCoordinate.latitude, _homePositionCoordinate.longitude), - QtPositioning.coordinate(_homePositionCoordinate.latitude, _homePositionCoordinate.longitude)) + var region = QtPositioning.rectangle(QtPositioning.coordinate(homePositionCoordinate.latitude, _homePositionCoordinate.longitude), + QtPositioning.coordinate(homePositionCoordinate.latitude, _homePositionCoordinate.longitude)) // Now expand the region to include all mission items for (var i=0; i<_missionItems.count; i++) { @@ -344,76 +361,48 @@ QGCView { } - MissionItemIndicator { - label: "H" - isCurrentItem: _showHomePositionManager - coordinate: _homePositionCoordinate - z: 2 - - onClicked: { - disableToggles() - if (_dropButtonsExclusiveGroup.current) { - _dropButtonsExclusiveGroup.current.checked = false - } - homePositionManagerButton.checked = true - _showHomePositionManager = true - } - } - // Add the mission items to the map MapItemView { model: controller.missionItems delegate: MissionItemIndicator { - label: object.sequenceNumber + id: itemIndicator + label: object.sequenceNumber == 0 ? "H" : object.sequenceNumber isCurrentItem: !_showHomePositionManager && object.isCurrentItem coordinate: object.coordinate z: 2 + visible: object.specifiesCoordinate onClicked: { - disableToggles() _showHomePositionManager = false - _addMissionItems = true - if (_dropButtonsExclusiveGroup.current) { - _dropButtonsExclusiveGroup.current.checked = false - } - addMissionItemsButton.checked = true setCurrentItem(object.sequenceNumber) } - } - } - - MapPolyline { - id: homePositionLine - line.width: 3 - line.color: _qgcPal.mapButtonHighlight - z: 1 - property var homePositionCoordinate: _homePositionCoordinate - - function update() { - while (homePositionLine.path.length != 0) { - homePositionLine.removeCoordinate(homePositionLine.path[0]) - } - if (_missionItems && _missionItems.count != 0) { - homePositionLine.addCoordinate(homePositionCoordinate) - homePositionLine.addCoordinate(_missionItems.get(0).coordinate) - } - } + Row { + anchors.top: parent.top + anchors.left: parent.right - onHomePositionCoordinateChanged: update() + Repeater { + model: object.childItems - Connections { - target: controller + delegate: + MissionItemIndexLabel { + label: object.sequenceNumber + isCurrentItem: !_showHomePositionManager && object.isCurrentItem + z: 2 - onWaypointLinesChanged: homePositionLine.update() - } + onClicked: { + _showHomePositionManager = false + setCurrentItem(object.sequenceNumber) + } - Component.onCompleted: homePositionLine.update() + } + } + } + } } - // Add lines between waypoints MapItemView { model: controller.waypointLines @@ -461,7 +450,7 @@ QGCView { // Mission Item Editor Item { anchors.fill: parent - visible: !_showHomePositionManager && controller.missionItems.count != 0 && !_showHelpPanel + visible: !_showHomePositionManager && controller.missionItems.count != 1 && ! _showHelpPanel ListView { id: missionItemSummaryList @@ -482,7 +471,7 @@ QGCView { onRemove: { var newCurrentItem = object.sequenceNumber - 1 controller.removeMissionItem(object.sequenceNumber) - if (_missionItems.count) { + if (_missionItems.count > 1) { newCurrentItem = Math.min(_missionItems.count - 1, newCurrentItem) setCurrentItem(newCurrentItem) } @@ -534,9 +523,9 @@ QGCView { if (currentIndex != -1) { var homePos = _homePositionManager.homePositions.get(currentIndex) _homePositionName = homePos.name - _homePositionCoordinate = homePos.coordinate - editorMap.latitude = _homePositionCoordinate.latitude - editorMap.longitude = _homePositionCoordinate.longitude + homePositionCoordinate = homePos.coordinate + editorMap.latitude = homePositionCoordinate.latitude + editorMap.longitude = homePositionCoordinate.longitude } } } @@ -595,7 +584,7 @@ QGCView { id: latitudeField anchors.right: parent.right width: _editFieldWidth - text: _homePositionCoordinate.latitude + text: homePositionCoordinate.latitude } } @@ -617,7 +606,7 @@ QGCView { id: longitudeField anchors.right: parent.right width: _editFieldWidth - text: _homePositionCoordinate.longitude + text: homePositionCoordinate.longitude } } @@ -639,7 +628,7 @@ QGCView { id: altitudeField anchors.right: parent.right width: _editFieldWidth - text: _homePositionCoordinate.altitude + text: homePositionCoordinate.altitude } } @@ -655,8 +644,8 @@ QGCView { text: "Add/Update" onClicked: { - _homePositionCoordinate = QtPositioning.coordinate(latitudeField.text, longitudeField.text, altitudeField.text) - _homePositionManager.updateHomePosition(nameField.text, _homePositionCoordinate) + homePositionCoordinate = QtPositioning.coordinate(latitudeField.text, longitudeField.text, altitudeField.text) + _homePositionManager.updateHomePosition(nameField.text, homePositionCoordinate) homePosCombo.currentIndex = homePosCombo.find(nameField.text) } } @@ -670,7 +659,7 @@ QGCView { homePosCombo.currentIndex = 0 var homePos = _homePositionManager.homePositions.get(0) _homePositionName = homePos.name - _homePositionCoordinate = homePos.coordinate + homePositionCoordinate = homePos.coordinate } } } @@ -680,7 +669,7 @@ QGCView { // Help Panel Item { anchors.fill: parent - visible: !_showHomePositionManager && (controller.missionItems.count == 0 || _showHelpPanel) + visible: !_showHomePositionManager && (controller.missionItems.count == 1 || _showHelpPanel) QGCLabel { id: helpTitle diff --git a/src/MissionItem.cc b/src/MissionItem.cc index febbfed487d509794362dac8d0132b3f9b657162..f3a02fa23df83d05d2cf22979a8930965f2f244e 100644 --- a/src/MissionItem.cc +++ b/src/MissionItem.cc @@ -84,6 +84,7 @@ MissionItem::MissionItem(QObject* parent, , _reachedTime(0) , _yawRadiansFact(NULL) ,_dirty(false) + , _homePositionSpecialCase(false) { _latitudeFact = new Fact(0, "Latitude:", FactMetaData::valueTypeDouble, this); _longitudeFact = new Fact(0, "Longitude:", FactMetaData::valueTypeDouble, this); @@ -219,6 +220,7 @@ const MissionItem& MissionItem::operator=(const MissionItem& other) _reachedTime = other._reachedTime; _altitudeRelativeToHomeFact = other._altitudeRelativeToHomeFact; _dirty = other._dirty; + _homePositionSpecialCase = other._homePositionSpecialCase; *_latitudeFact = *other._latitudeFact; *_longitudeFact = *other._longitudeFact; @@ -664,9 +666,11 @@ QmlObjectListModel* MissionItem::textFieldFacts(void) model->append(_latitudeFact); model->append(_longitudeFact); model->append(_altitudeFact); - model->append(_yawRadiansFact); - model->append(_param2Fact); - model->append(_param1Fact); + if (!_homePositionSpecialCase) { + model->append(_yawRadiansFact); + model->append(_param2Fact); + model->append(_param1Fact); + } break; case MAV_CMD_NAV_LOITER_UNLIM: model->append(_latitudeFact); @@ -736,7 +740,9 @@ QmlObjectListModel* MissionItem::checkboxFacts(void) switch ((MAV_CMD)_command) { case MAV_CMD_NAV_WAYPOINT: - model->append(_altitudeRelativeToHomeFact); + if (!_homePositionSpecialCase) { + model->append(_altitudeRelativeToHomeFact); + } break; case MAV_CMD_NAV_LOITER_UNLIM: model->append(_altitudeRelativeToHomeFact); diff --git a/src/MissionItem.h b/src/MissionItem.h index 514816f6195b3748d26e7c16f3357e85808f4bdc..09c57ade62aa1dec30b87fa42f664af39a48d450 100644 --- a/src/MissionItem.h +++ b/src/MissionItem.h @@ -36,6 +36,7 @@ #include "QmlObjectListModel.h" #include "Fact.h" #include "QGCLoggingCategory.h" +#include "QmlObjectListModel.h" Q_DECLARE_LOGGING_CATEGORY(MissionItemLog) @@ -76,6 +77,7 @@ public: Q_PROPERTY(QmlObjectListModel* textFieldFacts READ textFieldFacts NOTIFY commandChanged) Q_PROPERTY(QmlObjectListModel* checkboxFacts READ checkboxFacts NOTIFY commandChanged) Q_PROPERTY(MavlinkQmlSingleton::Qml_MAV_CMD command READ command WRITE setCommand NOTIFY commandChanged) + Q_PROPERTY(QmlObjectListModel* childItems READ childItems CONSTANT) // Property accesors @@ -111,6 +113,8 @@ public: bool dirty(void) { return _dirty; } void setDirty(bool dirty); + QmlObjectListModel* childItems(void) { return &_childItems; } + // C++ only methods /// Returns true if this item can be edited in the ui @@ -184,6 +188,8 @@ public: void save(QTextStream &saveStream); bool load(QTextStream &loadStream); + void setHomePositionSpecialCase(bool homePositionSpecialCase) { _homePositionSpecialCase = homePositionSpecialCase; } + signals: void sequenceNumberChanged(int sequenceNumber); void isCurrentItemChanged(bool isCurrentItem); @@ -265,6 +271,11 @@ private: bool _dirty; + bool _homePositionSpecialCase; ///< true: this item is being used as a ui home position indicator + + /// This is used to reference any subsequent mission items which do not specify a coordinate. + QmlObjectListModel _childItems; + static const int _cMavCmd2Name = 9; static const MavCmd2Name_t _rgMavCmd2Name[_cMavCmd2Name]; }; diff --git a/src/MissionManager/MissionManager.cc b/src/MissionManager/MissionManager.cc index 372bb724d00d4819c6717595c2db86e01eb9eaa5..4bb407b02e0f1751461379c9e93c1dbf7e553f45 100644 --- a/src/MissionManager/MissionManager.cc +++ b/src/MissionManager/MissionManager.cc @@ -53,13 +53,24 @@ MissionManager::~MissionManager() } -void MissionManager::writeMissionItems(const QmlObjectListModel& missionItems) +void MissionManager::writeMissionItems(const QmlObjectListModel& missionItems, bool skipFirstItem) { _retryCount = 0; _missionItems.clear(); - for (int i=0; i(missionItems[i]))); } + + if (skipFirstItem) { + for (int i=0; i<_missionItems.count(); i++) { + MissionItem* item = qobject_cast(_missionItems[i]); + + if (item->command() == MavlinkQmlSingleton::MAV_CMD_CONDITION_DELAY) { + item->setParam1((int)item->param1() - 1); + } + } + } qCDebug(MissionManagerLog) << "writeMissionItems count:" << _missionItems.count(); @@ -370,24 +381,24 @@ void MissionManager::_handleMissionAck(const mavlink_message_t& message) mavlink_msg_mission_ack_decode(&message, &missionAck); - qCDebug(MissionManagerLog) << "_handleMissionAck type:" << missionAck.type; + qCDebug(MissionManagerLog) << "_handleMissionAck type:" << _missionResultToString((MAV_MISSION_RESULT)missionAck.type); switch (savedRetryAck) { case AckNone: // State machine is idle. Vehicle is confused. - qCDebug(MissionManagerLog) << "_handleMissionAck vehicle sent ack while state machine is idle: error:" << missionAck.type; + qCDebug(MissionManagerLog) << "_handleMissionAck vehicle sent ack while state machine is idle: error:" << _missionResultToString((MAV_MISSION_RESULT)missionAck.type); _sendError(VehicleError, QString("Vehicle sent unexpected MISSION_ACK message, error: %1").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type))); break; case AckMissionCount: // MISSION_COUNT message expected - qCDebug(MissionManagerLog) << "_handleMissionAck vehicle sent ack when MISSION_COUNT expected: error:" << missionAck.type; + qCDebug(MissionManagerLog) << "_handleMissionAck vehicle sent ack when MISSION_COUNT expected: error:" << _missionResultToString((MAV_MISSION_RESULT)missionAck.type); if (!_retrySequence(AckMissionCount)) { _sendError(VehicleError, QString("Vehicle returned error: %1. Partial list of mission items may have been returned.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type))); } break; case AckMissionItem: // MISSION_ITEM expected - qCDebug(MissionManagerLog) << "_handleMissionAck vehicle sent ack when MISSION_ITEM expected: error:" << missionAck.type; + qCDebug(MissionManagerLog) << "_handleMissionAck vehicle sent ack when MISSION_ITEM expected: error:" << _missionResultToString((MAV_MISSION_RESULT)missionAck.type); if (!_retrySequence(AckMissionItem)) { _sendError(VehicleError, QString("Vehicle returned error: %1. Partial list of mission items may have been returned.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type))); } @@ -405,7 +416,7 @@ void MissionManager::_handleMissionAck(const mavlink_message_t& message) } } } else { - qCDebug(MissionManagerLog) << "_handleMissionAck ack error:" << missionAck.type; + qCDebug(MissionManagerLog) << "_handleMissionAck ack error:" << _missionResultToString((MAV_MISSION_RESULT)missionAck.type); if (!_retrySequence(AckMissionRequest)) { _sendError(VehicleError, QString("Vehicle returned error: %1. Vehicle only has partial list of mission items.").arg(_missionResultToString((MAV_MISSION_RESULT)missionAck.type))); } diff --git a/src/MissionManager/MissionManager.h b/src/MissionManager/MissionManager.h index 9b589a9f6a3dc9e3d86973d6b3479c58d65189d1..964d72ad8f5fed0d1ed375866fbdba93a56d48ce 100644 --- a/src/MissionManager/MissionManager.h +++ b/src/MissionManager/MissionManager.h @@ -62,7 +62,9 @@ public: void requestMissionItems(void); /// Writes the specified set of mission items to the vehicle - void writeMissionItems(const QmlObjectListModel& missionItems); + /// @oaram missionItems Items to send to vehicle + /// @param skipFirstItem true: Don't send first item + void writeMissionItems(const QmlObjectListModel& missionItems, bool skipFirstItem); /// Returns a copy of the current set of mission items. Caller is responsible for /// freeing returned object. diff --git a/src/MissionManager/MissionManagerTest.cc b/src/MissionManager/MissionManagerTest.cc index 6c758d795e5f0fbd33550dc53555163c43f9b12d..6be9a97de7b133b0c150eb9d393a24d2eb04cb68 100644 --- a/src/MissionManager/MissionManagerTest.cc +++ b/src/MissionManager/MissionManagerTest.cc @@ -158,7 +158,7 @@ void MissionManagerTest::_writeItems(MockLinkMissionItemHandler::FailureMode_t f } // Send the items to the vehicle - _missionManager->writeMissionItems(*list); + _missionManager->writeMissionItems(*list, false /* skipFirstItem */); // writeMissionItems should emit inProgressChanged signal before returning so no need to wait for it QVERIFY(_missionManager->inProgress()); diff --git a/src/QmlControls/MissionItemEditor.qml b/src/QmlControls/MissionItemEditor.qml index b8695fddc4ac00ad90591561f53431ade70f8f8c..723a4376243de1d5f78070646abc667795094f6d 100644 --- a/src/QmlControls/MissionItemEditor.qml +++ b/src/QmlControls/MissionItemEditor.qml @@ -53,7 +53,7 @@ Rectangle { id: label anchors.verticalCenter: commandPicker.verticalCenter isCurrentItem: missionItem.isCurrentItem - label: missionItem.sequenceNumber + label: missionItem.sequenceNumber == 0 ? "H" : missionItem.sequenceNumber } MouseArea { @@ -71,10 +71,26 @@ Rectangle { anchors.right: parent.right currentIndex: missionItem.commandByIndex model: missionItem.commandNames + visible: missionItem.sequenceNumber != 0 onActivated: missionItem.commandByIndex = index } + Rectangle { + anchors.fill: commandPicker + color: qgcPal.button + visible: missionItem.sequenceNumber == 0 + + QGCLabel { + id: homeLabel + anchors.leftMargin: ScreenTools.defaultFontPixelWidth + anchors.fill: parent + verticalAlignment: Text.AlignVCenter + text: "Home" + color: qgcPal.buttonText + } + } + Rectangle { anchors.topMargin: _margin anchors.top: commandPicker.bottom diff --git a/src/QmlControls/QmlObjectListModel.cc b/src/QmlControls/QmlObjectListModel.cc index 98421f0b15a33ed99ee45bc752a104f71af42e32..5ab27884440b5bdcf559c1df0ee4753a26edcb96 100644 --- a/src/QmlControls/QmlObjectListModel.cc +++ b/src/QmlControls/QmlObjectListModel.cc @@ -35,6 +35,7 @@ const int QmlObjectListModel::TextRole = Qt::UserRole + 1; QmlObjectListModel::QmlObjectListModel(QObject* parent) : QAbstractListModel(parent) , _dirty(false) + , _skipDirtyFirstItem(false) { } @@ -147,16 +148,22 @@ void QmlObjectListModel::clear(void) } } -void QmlObjectListModel::removeAt(int i) +QObject* QmlObjectListModel::removeAt(int i) { + QObject* removedObject = _objectList[i]; + // Look for a dirtyChanged signal on the object if (_objectList[i]->metaObject()->indexOfSignal(QMetaObject::normalizedSignature("dirtyChanged(bool)")) != -1) { - QObject::disconnect(_objectList[i], SIGNAL(dirtyChanged(bool)), this, SLOT(_childDirtyChanged(bool))); + if (!_skipDirtyFirstItem || i != 0) { + QObject::disconnect(_objectList[i], SIGNAL(dirtyChanged(bool)), this, SLOT(_childDirtyChanged(bool))); + } } removeRows(i, 1); setDirty(true); + + return removedObject; } void QmlObjectListModel::insert(int i, QObject* object) @@ -169,7 +176,9 @@ void QmlObjectListModel::insert(int i, QObject* object) // Look for a dirtyChanged signal on the object if (object->metaObject()->indexOfSignal(QMetaObject::normalizedSignature("dirtyChanged(bool)")) != -1) { - QObject::connect(object, SIGNAL(dirtyChanged(bool)), this, SLOT(_childDirtyChanged(bool))); + if (!_skipDirtyFirstItem || i != 0) { + QObject::connect(object, SIGNAL(dirtyChanged(bool)), this, SLOT(_childDirtyChanged(bool))); + } } _objectList.insert(i, object); diff --git a/src/QmlControls/QmlObjectListModel.h b/src/QmlControls/QmlObjectListModel.h index ea3cdd5c6086b81337ce0c7be545812ef2bda587..2e8d1bd8d8803f6a73894a6f2d41cc1e0cd58cf1 100644 --- a/src/QmlControls/QmlObjectListModel.h +++ b/src/QmlControls/QmlObjectListModel.h @@ -51,7 +51,7 @@ public: void append(QObject* object); void clear(void); - void removeAt(int i); + QObject* removeAt(int i); void insert(int i, QObject* object); QObject* operator[](int i); const QObject* operator[](int i) const; @@ -79,6 +79,7 @@ private: QList _objectList; bool _dirty; + bool _skipDirtyFirstItem; static const int ObjectRole; static const int TextRole; diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index d81d4e6925cd2848962085766bbe3e4789f3f00a..4003730071c15921b8570367e1313cc7f7bfdd8a 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -1960,15 +1960,15 @@ void UAS::enableHilFlightGear(bool enable, QString options, bool sensorHil, QObj } float noise_scaler = 0.05f; - xacc_var = noise_scaler * 1.2914f; - yacc_var = noise_scaler * 0.7048f; - zacc_var = noise_scaler * 1.9577f; - rollspeed_var = noise_scaler * 0.8126f; - pitchspeed_var = noise_scaler * 0.6145f; - yawspeed_var = noise_scaler * 0.5852f; - xmag_var = noise_scaler * 0.4786f; - ymag_var = noise_scaler * 0.4566f; - zmag_var = noise_scaler * 0.3333f; + xacc_var = noise_scaler * 0.2914f; + yacc_var = noise_scaler * 0.2914f; + zacc_var = noise_scaler * 0.9577f; + rollspeed_var = noise_scaler * 0.1f * 0.8126f; + pitchspeed_var = noise_scaler * 0.1f * 0.6145f; + yawspeed_var = noise_scaler * 0.1f * 0.5852f; + xmag_var = noise_scaler * 0.0786f; + ymag_var = noise_scaler * 0.0566f; + zmag_var = noise_scaler * 0.0333f; abs_pressure_var = noise_scaler * 1.1604f; diff_pressure_var = noise_scaler * 0.6604f; pressure_alt_var = noise_scaler * 1.1604f; @@ -2034,16 +2034,16 @@ void UAS::enableHilXPlane(bool enable) } simulation = new QGCXPlaneLink(this); - float noise_scaler = 0.05f; - xacc_var = noise_scaler * 1.2914f; - yacc_var = noise_scaler * 0.7048f; - zacc_var = noise_scaler * 1.9577f; - rollspeed_var = noise_scaler * 0.8126f; - pitchspeed_var = noise_scaler * 0.6145f; - yawspeed_var = noise_scaler * 0.5852f; - xmag_var = noise_scaler * 0.4786f; - ymag_var = noise_scaler * 0.4566f; - zmag_var = noise_scaler * 0.3333f; + float noise_scaler = 0.02f; + xacc_var = noise_scaler * 0.2914f; + yacc_var = noise_scaler * 0.2914f; + zacc_var = noise_scaler * 0.9577f; + rollspeed_var = noise_scaler * 0.15f * 0.8126f; + pitchspeed_var = noise_scaler * 0.15f * 0.6145f; + yawspeed_var = noise_scaler * 0.15f * 0.5852f; + xmag_var = noise_scaler * 0.0786f; + ymag_var = noise_scaler * 0.0566f; + zmag_var = noise_scaler * 0.0333f; abs_pressure_var = noise_scaler * 1.1604f; diff_pressure_var = noise_scaler * 0.6604f; pressure_alt_var = noise_scaler * 1.1604f;