Merge pull request #5882 from DonLakeFlyer/SeqNumFixes

Plan: Sequence number fixes

src/MissionManager/MissionController.cc

@@ -1084,6 +1084,9 @@ void MissionController::_updateBatteryInfo(int waypointIndex)
         _missionFlightStatus.hoverAmpsTotal = (_missionFlightStatus.hoverTime / 60.0) * _missionFlightStatus.hoverAmps;
         _missionFlightStatus.cruiseAmpsTotal = (_missionFlightStatus.cruiseTime / 60.0) * _missionFlightStatus.cruiseAmps;
         _missionFlightStatus.batteriesRequired = ceil((_missionFlightStatus.hoverAmpsTotal + _missionFlightStatus.cruiseAmpsTotal) / _missionFlightStatus.ampMinutesAvailable);
+        // FIXME: Battery change point code pretty much doesn't work. The reason is that is treats complex items as a black box. It needs to be able to look
+        // inside complex items in order to determine a swap point that is interior to a complex item. Current the swap point display in PlanToolbar is
+        // disabled to do this problem.
         if (waypointIndex != -1 && _missionFlightStatus.batteriesRequired == 2 && _missionFlightStatus.batteryChangePoint == -1) {
             _missionFlightStatus.batteryChangePoint = waypointIndex - 1;
         }

src/MissionManager/PlanManager.cc

@@ -604,7 +604,7 @@ void PlanManager::_handleMissionAck(const mavlink_message_t& message)
         // MISSION_REQUEST is expected, or MISSION_ACK to end sequence
         if (missionAck.type == MAV_MISSION_ACCEPTED) {
             if (_itemIndicesToWrite.count() == 0) {
-                qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionAck write sequence complete").arg(_planTypeString());
+                qCDebug(PlanManagerLog) << QStringLiteral("_handleMissionAck write sequence complete %1").arg(_planTypeString());
                 _finishTransaction(true);
             } else {
                 _sendError(MissingRequestsError, tr("Vehicle did not request all items during write sequence, missed count %1.").arg(_itemIndicesToWrite.count()));

src/MissionManager/StructureScanComplexItem.cc

@@ -127,8 +127,10 @@ void StructureScanComplexItem::_polygonCountChanged(int count)
 int StructureScanComplexItem::lastSequenceNumber(void) const
 {
     return _sequenceNumber +
-            ((_flightPolygon.count() + 1) * _layersFact.rawValue().toInt()) + // 1 waypoint for each polygon vertex + 1 to go back to first polygon vertex
-            1;  // Gimbal control command
+            (_layersFact.rawValue().toInt() *
+                ((_flightPolygon.count() + 1) + // 1 waypoint for each polygon vertex + 1 to go back to first polygon vertex for each layer
+                 2)) +                          // Camera trigger start/stop for each layer
+            1;                                  // Gimbal control command
 }
 
 void StructureScanComplexItem::setDirty(bool dirty)

src/MissionManager/StructureScanComplexItemTest.cc

@@ -122,9 +122,6 @@ void StructureScanComplexItemTest::_validateItem(StructureScanComplexItem* item)
     QCOMPARE(item->gimbalPitch()->cookedValue().toDouble(), 45.0);
     QCOMPARE(item->gimbalYaw()->cookedValue().toDouble(), 45.0);
     QCOMPARE(item->layers()->cookedValue().toInt(), 2);
-
-    int seqNum = item->sequenceNumber();
-    QCOMPARE(item->lastSequenceNumber(), seqNum + (5 /* 5 waypoints per layer */ * item->layers()->cookedValue().toInt()) + 1 /* gimbal command */);
 }
 
 void StructureScanComplexItemTest::_testSaveLoad(void)
@@ -152,3 +149,13 @@ void StructureScanComplexItemTest::_testGimbalAngleUpdate(void)
     QCOMPARE(_structureScanItem->gimbalPitch()->cookedValue().toDouble(), 0.0);
     QCOMPARE(_structureScanItem->gimbalYaw()->cookedValue().toDouble(), 90.0);
 }
+
+void StructureScanComplexItemTest::_testItemCount(void)
+{
+    QList<MissionItem*> items;
+
+    _initItem();
+    _structureScanItem->appendMissionItems(items, this);
+    QCOMPARE(items.count(), _structureScanItem->lastSequenceNumber());
+
+}

src/MissionManager/StructureScanComplexItemTest.h

@@ -28,6 +28,7 @@ private slots:
     void _testDirty(void);
     void _testSaveLoad(void);
     void _testGimbalAngleUpdate(void);
+    void _testItemCount(void);
 
 private:
     void _initItem(void);

src/MissionManager/SurveyMissionItem.cc

@@ -638,6 +638,28 @@ void SurveyMissionItem::_adjustTransectsToEntryPointLocation(QList<QList<QGeoCoo
     qCDebug(SurveyMissionItemLog) << "Modified entry point" << transects.first().first();
 }
 
+int SurveyMissionItem::_calcMissionCommandCount(QList<QList<QGeoCoordinate>>& transectSegments)
+{
+    int missionCommandCount= 0;
+    for (int i=0; i<transectSegments.count(); i++) {
+        const QList<QGeoCoordinate>& transectSegment = transectSegments[i];
+
+        missionCommandCount += transectSegment.count();    // This accounts for all waypoints
+        if (_hoverAndCaptureEnabled()) {
+            // Internal camera trigger points are entry point, plus all points before exit point
+            missionCommandCount += transectSegment.count() - (_hasTurnaround() ? 2 : 0) - 1;
+        } else if (_triggerCamera() && !_imagesEverywhere()) {
+            // Camera on/off at entry/exit of each transect
+            missionCommandCount += 2;
+        }
+    }
+    if (transectSegments.count() && _triggerCamera() && _imagesEverywhere()) {
+         // Camera on/off for entire survey
+        missionCommandCount += 2;
+    }
+
+    return missionCommandCount;
+}
 void SurveyMissionItem::_generateGrid(void)
 {
     if (_ignoreRecalc) {
@@ -725,19 +747,8 @@ void SurveyMissionItem::_generateGrid(void)
     emit gridPointsChanged();
 
     // Determine command count for lastSequenceNumber
-
-    _missionCommandCount= 0;
-    for (int i=0; i<_transectSegments.count(); i++) {
-        const QList<QGeoCoordinate>& transectSegment = _transectSegments[i];
-
-        _missionCommandCount += transectSegment.count();    // This accounts for all waypoints
-        if (_hoverAndCaptureEnabled()) {
-            // Internal camera trigger points are entry point, plus all points before exit point
-            _missionCommandCount += transectSegment.count() - (_hasTurnaround() ? 2 : 0) - 1;
-        } else if (_triggerCamera()) {
-            _missionCommandCount += 2;                          // Camera on/off at entry/exit
-        }
-    }
+    _missionCommandCount = _calcMissionCommandCount(_transectSegments);
+    _missionCommandCount += _calcMissionCommandCount(_reflyTransectSegments);
     emit lastSequenceNumberChanged(lastSequenceNumber());
 
     // Set exit coordinate
@@ -1117,7 +1128,7 @@ bool SurveyMissionItem::_appendMissionItemsWorker(QList<MissionItem*>& items, QO
 {
     bool firstWaypointTrigger = false;
 
-    qCDebug(SurveyMissionItemLog) << "hasTurnaround:triggerCamera:hoverAndCapture:imagesEverywhere:hasRefly:buildRefly" << _hasTurnaround() << _triggerCamera() << _hoverAndCaptureEnabled() << _imagesEverywhere() << hasRefly << buildRefly;
+    qCDebug(SurveyMissionItemLog) << QStringLiteral("hasTurnaround(%1) triggerCamera(%2) hoverAndCapture(%3) imagesEverywhere(%4) hasRefly(%5) buildRefly(%6) ").arg(_hasTurnaround()).arg(_triggerCamera()).arg(_hoverAndCaptureEnabled()).arg(_imagesEverywhere()).arg(hasRefly).arg(buildRefly);
 
     QList<QList<QGeoCoordinate>>& transectSegments = buildRefly ? _reflyTransectSegments : _transectSegments;
 

src/MissionManager/SurveyMissionItem.h

@@ -222,6 +222,7 @@ private:
     void _adjustTransectsToEntryPointLocation(QList<QList<QGeoCoordinate>>& transects);
     bool _gridAngleIsNorthSouthTransects();
     double _clampGridAngle90(double gridAngle);
+    int _calcMissionCommandCount(QList<QList<QGeoCoordinate>>& transectSegments);
 
     int                             _sequenceNumber;
     bool                            _dirty;

src/MissionManager/SurveyMissionItemTest.cc

@@ -200,14 +200,17 @@ double SurveyMissionItemTest::_clampGridAngle180(double gridAngle)
     return gridAngle;
 }
 
-void SurveyMissionItemTest::_testGridAngle(void)
+void SurveyMissionItemTest::_setPolygon(void)
 {
-    QGCMapPolygon* mapPolygon = _surveyItem->mapPolygon();
-
     for (int i=0; i<_polyPoints.count(); i++) {
         QGeoCoordinate& vertex = _polyPoints[i];
-        mapPolygon->appendVertex(vertex);
+        _mapPolygon->appendVertex(vertex);
     }
+}
+
+void SurveyMissionItemTest::_testGridAngle(void)
+{
+    _setPolygon();
 
     for (double gridAngle=-360.0; gridAngle<=360.0; gridAngle++) {
         _surveyItem->gridAngle()->setRawValue(gridAngle);
@@ -223,12 +226,7 @@ void SurveyMissionItemTest::_testGridAngle(void)
 
 void SurveyMissionItemTest::_testEntryLocation(void)
 {
-    QGCMapPolygon* mapPolygon = _surveyItem->mapPolygon();
-
-    for (int i=0; i<_polyPoints.count(); i++) {
-        QGeoCoordinate& vertex = _polyPoints[i];
-        mapPolygon->appendVertex(vertex);
-    }
+    _setPolygon();
 
     for (double gridAngle=-360.0; gridAngle<=360.0; gridAngle++) {
         _surveyItem->gridAngle()->setRawValue(gridAngle);
@@ -251,3 +249,39 @@ void SurveyMissionItemTest::_testEntryLocation(void)
         rgSeenEntryCoords.clear();
     }
 }
+
+
+void SurveyMissionItemTest::_testItemCount(void)
+{
+    QList<MissionItem*> items;
+
+    _setPolygon();
+
+    _surveyItem->hoverAndCapture()->setRawValue(false);
+    _surveyItem->cameraTriggerInTurnaround()->setRawValue(false);
+    _surveyItem->setRefly90Degrees(false);
+    _surveyItem->appendMissionItems(items, this);
+    QCOMPARE(items.count(), _surveyItem->lastSequenceNumber());
+    items.clear();
+
+    _surveyItem->hoverAndCapture()->setRawValue(false);
+    _surveyItem->cameraTriggerInTurnaround()->setRawValue(true);
+    _surveyItem->setRefly90Degrees(false);
+    _surveyItem->appendMissionItems(items, this);
+    QCOMPARE(items.count(), _surveyItem->lastSequenceNumber());
+    items.clear();
+
+    _surveyItem->hoverAndCapture()->setRawValue(true);
+    _surveyItem->cameraTriggerInTurnaround()->setRawValue(false);
+    _surveyItem->setRefly90Degrees(false);
+    _surveyItem->appendMissionItems(items, this);
+    QCOMPARE(items.count(), _surveyItem->lastSequenceNumber());
+    items.clear();
+
+    _surveyItem->hoverAndCapture()->setRawValue(true);
+    _surveyItem->cameraTriggerInTurnaround()->setRawValue(false);
+    _surveyItem->setRefly90Degrees(true);
+    _surveyItem->appendMissionItems(items, this);
+    QCOMPARE(items.count(), _surveyItem->lastSequenceNumber());
+    items.clear();
+}

src/MissionManager/SurveyMissionItemTest.h

@@ -36,9 +36,11 @@ private slots:
     void _testCameraTrigger(void);
     void _testGridAngle(void);
     void _testEntryLocation(void);
+    void _testItemCount(void);
 
 private:
     double _clampGridAngle180(double gridAngle);
+    void _setPolygon(void);
 
     enum {
         gridPointsChangedIndex = 0,

src/PlanView/PlanToolBar.qml

@@ -283,13 +283,15 @@ Rectangle {
             }
 
             Item { width: 1; height: 1 }
-
+/*
+            FIXME: Swap point display is currently hidden since the code which calcs it doesn't work correctly
             QGCLabel { text: qsTr("Swap waypoint:"); font.pointSize: _dataFontSize; }
             QGCLabel {
                 text:                   _batteryChangePointText
                 font.pointSize:         _dataFontSize
                 Layout.minimumWidth:    _mediumValueWidth
             }
+*/
         }
     }