Merge pull request #1972 from DonLakeFlyer/MissionEditor

More work on waypoint lines

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<MissionItem*>(_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<MissionItem*>(_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<MissionItem*>(_missionItems->get(0));
+    
     _waypointLines.clear();
+    
     for (int i=1; i<_missionItems->count(); i++) {
-        MissionItem* item1 = qobject_cast<MissionItem*>(_missionItems->get(i-1));
-        MissionItem* item2 = qobject_cast<MissionItem*>(_missionItems->get(i));
+        MissionItem* item = qobject_cast<MissionItem*>(_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<MissionItem*>(_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<MissionItem*>(_missionItems->get(0));
+    
+    currentParentItem->childItems()->clear();
+    
+    for (int i=0; i<_missionItems->count(); i++) {
+        MissionItem* item = qobject_cast<MissionItem*>(_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<MissionItem*>(_missionItems->get(0));
+    
+    currentParentItem->childItems()->clear();
+    
+    for (int i=1; i<_missionItems->count(); i++) {
+        MissionItem* item = qobject_cast<MissionItem*>(_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<MissionItem*>(_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<MissionItem*>(_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();
 }

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;

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);

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];
 };

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.count(); i++) {
+    
+    for (int i=skipFirstItem ? 1: 0; i<missionItems.count(); i++) {
         _missionItems.append(new MissionItem(*qobject_cast<const MissionItem*>(missionItems[i])));
     }
+    
+    if (skipFirstItem) {
+        for (int i=0; i<_missionItems.count(); i++) {
+            MissionItem* item = qobject_cast<MissionItem*>(_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)));
                 }

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.

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());

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

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);

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<QObject*> _objectList;
     
     bool _dirty;
+    bool _skipDirtyFirstItem;
         
     static const int ObjectRole;
     static const int TextRole;