Merge pull request #8536 from DonLakeFlyer/ConditionGate

Plan: CONDITION_GATE command support

src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc

@@ -23,6 +23,8 @@
 #include "RadioComponentController.h"
 #include "QGCCameraManager.h"
 #include "QGCFileDownload.h"
+#include "SettingsManager.h"
+#include "PlanViewSettings.h"
 
 #include <QDebug>
 
@@ -279,7 +281,7 @@ void PX4FirmwarePlugin::getParameterMetaDataVersionInfo(const QString& metaDataF
 
 QList<MAV_CMD> PX4FirmwarePlugin::supportedMissionCommands(void)
 {
-    return {
+    QList<MAV_CMD> cmds = {
         MAV_CMD_NAV_WAYPOINT,
         MAV_CMD_NAV_LOITER_UNLIM, MAV_CMD_NAV_LOITER_TIME, MAV_CMD_NAV_LOITER_TO_ALT,
         MAV_CMD_NAV_LAND, MAV_CMD_NAV_TAKEOFF, MAV_CMD_NAV_RETURN_TO_LAUNCH,
@@ -298,6 +300,12 @@ QList<MAV_CMD> PX4FirmwarePlugin::supportedMissionCommands(void)
         MAV_CMD_NAV_DELAY,
         MAV_CMD_CONDITION_YAW,
     };
+
+    if (qgcApp()->toolbox()->settingsManager()->planViewSettings()->useConditionGate()->rawValue().toBool()) {
+        cmds.append(MAV_CMD_CONDITION_GATE);
+    }
+
+    return cmds;
 }
 
 QString PX4FirmwarePlugin::missionCommandOverrides(MAV_TYPE vehicleType) const

src/MissionManager/CorridorScanComplexItem.cc

@@ -217,7 +217,7 @@ void CorridorScanComplexItem::_buildAndAppendMissionItems(QList<MissionItem*>& i
             firstOverallPoint = false;
 
             // Possibly add trigger start/stop to survey area entrance/exit
-            if (triggerCamera() && transectCoordInfo.coordType == TransectStyleComplexItem::CoordTypeSurveyEdge) {
+            if (triggerCamera() && (transectCoordInfo.coordType == TransectStyleComplexItem::CoordTypeSurveyEntry || transectCoordInfo.coordType == TransectStyleComplexItem::CoordTypeSurveyExit)) {
                 if (transectEntry) {
                     // Start of transect, always start triggering. We do this even if we are taking images everywhere.
                     // This allows a restart of the mission in mid-air without losing images from the entire mission.
@@ -370,9 +370,9 @@ void CorridorScanComplexItem::_rebuildTransectsPhase1(void)
                 TransectStyleComplexItem::CoordInfo_t coordInfo = { transectCoords[j], CoordTypeInterior };
                 transect.append(coordInfo);
             }
-            TransectStyleComplexItem::CoordInfo_t coordInfo = { transectCoords.first(), CoordTypeSurveyEdge };
+            TransectStyleComplexItem::CoordInfo_t coordInfo = { transectCoords.first(), CoordTypeSurveyEntry };
             transect.prepend(coordInfo);
-            coordInfo = { transectCoords.last(), CoordTypeSurveyEdge };
+            coordInfo = { transectCoords.last(), CoordTypeSurveyExit };
             transect.append(coordInfo);
 
             // Extend the transect ends for turnaround

src/MissionManager/MavCmdInfoCommon.json

@@ -858,6 +858,20 @@
                 "default":          25,
                 "units":            "m",
                 "decimalPlaces":    2
+            },
+            "param2": {
+                "label":            "Shutter",
+                "default":          0,
+                "units":            "msecs",
+                "decimalPlaces":    0
+            },
+            "param3": {
+                "label":            "Trigger",
+                "default":          25,
+                "enumStrings":      "No Trigger,Once Immediately",
+                "enumValues":       "0,1",
+                "default":          0,
+                "decimalPlaces":    0
             }
         },
         {
@@ -1063,6 +1077,22 @@
                 "enumValues":       "3,4"
             }
         },
+        {
+            "id":                   4501,
+            "rawName":              "MAV_CMD_CONDITION_GATE",
+            "friendlyName":         "Condition Gate",
+            "description":          "Delay mission state machine until gate has been reached.",
+            "specifiesCoordinate":  true,
+            "friendlyEdit":         true,
+            "category":             "Conditionals",
+            "param2": {
+                "label":            "Ignore Alt",
+                "enumStrings":      "False,True",
+                "enumValues":       "0,1",
+                "default":          1,
+                "decimalPlaces":    0
+            }
+        },
         { "id": 30001, "rawName": "MAV_CMD_PAYLOAD_PREPARE_DEPLOY", "friendlyName": "Payload prepare deploy" },
         { "id": 30002, "rawName": "MAV_CMD_PAYLOAD_CONTROL_DEPLOY", "friendlyName": "Payload control deploy" }
     ]

src/MissionManager/SurveyComplexItem.h

@@ -98,7 +98,6 @@ private:
     void _intersectLinesWithRect(const QList<QLineF>& lineList, const QRectF& boundRect, QList<QLineF>& resultLines);
     void _intersectLinesWithPolygon(const QList<QLineF>& lineList, const QPolygonF& polygon, QList<QLineF>& resultLines);
     void _adjustLineDirection(const QList<QLineF>& lineList, QList<QLineF>& resultLines);
-    int _appendWaypointToMission(QList<MissionItem*>& items, int seqNum, QGeoCoordinate& coord, CameraTriggerCode cameraTrigger, QObject* missionItemParent);
     bool _nextTransectCoord(const QList<QGeoCoordinate>& transectPoints, int pointIndex, QGeoCoordinate& coord);
     bool _appendMissionItemsWorker(QList<MissionItem*>& items, QObject* missionItemParent, int& seqNum, bool hasRefly, bool buildRefly);
     void _optimizeTransectsForShortestDistance(const QGeoCoordinate& distanceCoord, QList<QList<QGeoCoordinate>>& transects);
@@ -130,6 +129,11 @@ private:
     // return true if vertex a can see vertex b
     bool _VertexCanSeeOther(const QPolygonF& polygon, const QPointF* vertexA, const QPointF* vertexB);
     bool _VertexIsReflex(const QPolygonF& polygon, const QPointF* vertex);
+    void _appendWaypoint(QList<MissionItem*>& items, QObject* missionItemParent, int& seqNum, MAV_FRAME mavFrame, float holdTime, const QGeoCoordinate& coordinate);
+    void _appendSinglePhotoCapture(QList<MissionItem*>& items, QObject* missionItemParent, int& seqNum);
+    void _appendConditionGate(QList<MissionItem*>& items, QObject* missionItemParent, int& seqNum, MAV_FRAME mavFrame, const QGeoCoordinate& coordinate);
+    void _appendCameraTriggerDistance(QList<MissionItem*>& items, QObject* missionItemParent, int& seqNum, float triggerDistance);
+    void _appendCameraTriggerDistanceUpdatePoint(QList<MissionItem*>& items, QObject* missionItemParent, int& seqNum, MAV_FRAME mavFrame, const QGeoCoordinate& coordinate, bool useConditionGate, float triggerDistance);
 
     QMap<QString, FactMetaData*> _metaDataMap;
 

src/MissionManager/SurveyComplexItemTest.cc

@@ -9,11 +9,16 @@
 
 #include "SurveyComplexItemTest.h"
 #include "QGCApplication.h"
+#include "JsonHelper.h"
 
 SurveyComplexItemTest::SurveyComplexItemTest(void)
 {
-    _polyPoints << QGeoCoordinate(47.633550640000003, -122.08982199) << QGeoCoordinate(47.634129020000003, -122.08887249) <<
-                   QGeoCoordinate(47.633619320000001, -122.08811074) << QGeoCoordinate(47.633189139999999, -122.08900124);
+    // We use a 100m by 100m square test polygon
+    const double edgeDistance = 100;
+    _polyVertices.append(QGeoCoordinate(47.633550640000003, -122.08982199));
+    _polyVertices.append(_polyVertices[0].atDistanceAndAzimuth(edgeDistance, 90));
+    _polyVertices.append(_polyVertices[1].atDistanceAndAzimuth(edgeDistance, 180));
+    _polyVertices.append(_polyVertices[2].atDistanceAndAzimuth(edgeDistance, -90.0));
 }
 
 void SurveyComplexItemTest::init(void)
@@ -27,12 +32,26 @@ void SurveyComplexItemTest::init(void)
     _rgSurveySignals[surveyRefly90DegreesChangedIndex] =          SIGNAL(refly90DegreesChanged(bool));
     _rgSurveySignals[surveyDirtyChangedIndex] =                   SIGNAL(dirtyChanged(bool));
 
+    _planViewSettings = qgcApp()->toolbox()->settingsManager()->planViewSettings();
     _masterController = new PlanMasterController(this);
     _controllerVehicle = _masterController->controllerVehicle();
     _surveyItem = new SurveyComplexItem(_masterController, false /* flyView */, QString() /* kmlFile */, this /* parent */);
-    _surveyItem->turnAroundDistance()->setRawValue(0);  // Unit test written for no turnaround distance
-    _surveyItem->setDirty(false);
     _mapPolygon = _surveyItem->surveyAreaPolygon();
+    _mapPolygon->appendVertices(_polyVertices);
+
+    QVERIFY(_surveyItem->cameraCalc()->isManualCamera());
+
+    // Set grid spacing to match expected transect count
+    double polyWidthDistance = _polyVertices[0].distanceTo(_polyVertices[1]);
+    double polyHeightDistance = _polyVertices[0].distanceTo(_polyVertices[3]);
+    _surveyItem->cameraCalc()->adjustedFootprintSide()->setRawValue((polyWidthDistance * 0.5) - 1.0);
+    _surveyItem->cameraCalc()->adjustedFootprintFrontal()->setRawValue(polyHeightDistance * 0.25);
+
+    _surveyItem->gridAngle()->setRawValue(0);
+    int expectedTransectCount = _expectedTransectCount;
+    QCOMPARE(_surveyItem->_transectCount(), expectedTransectCount);
+
+    _surveyItem->setDirty(false);
 
     // It's important to check that the right signals are emitted at the right time since that drives ui change.
     // It's also important to check that things are not being over-signalled when they should not be, since that can lead
@@ -107,10 +126,7 @@ double SurveyComplexItemTest::_clampGridAngle180(double gridAngle)
 
 void SurveyComplexItemTest::_setPolygon(void)
 {
-    for (int i=0; i<_polyPoints.count(); i++) {
-        QGeoCoordinate& vertex = _polyPoints[i];
-        _mapPolygon->appendVertex(vertex);
-    }
+    _mapPolygon->appendVertices(_polyVertices);
 }
 
 void SurveyComplexItemTest::_testGridAngle(void)
@@ -184,3 +200,142 @@ void SurveyComplexItemTest::_testItemCount(void)
     QCOMPARE(items.count() - 1, _surveyItem->lastSequenceNumber());
     items.clear();
 }
+
+QList<MAV_CMD> SurveyComplexItemTest::_createExpectedCommands(bool hasTurnaround, bool useConditionGate)
+{
+    static const QList<MAV_CMD> singleFullTransect = {
+        MAV_CMD_NAV_WAYPOINT,           // Turnaround
+        MAV_CMD_CONDITION_GATE,         // Survey area entry edge
+        MAV_CMD_DO_SET_CAM_TRIGG_DIST,
+        MAV_CMD_CONDITION_GATE,         // Survey area exit edge
+        MAV_CMD_DO_SET_CAM_TRIGG_DIST,
+        MAV_CMD_NAV_WAYPOINT,
+    };
+
+    QList<MAV_CMD> singleTransect = singleFullTransect;
+    QList<MAV_CMD> expectedCommands;
+
+    if (!useConditionGate) {
+        for (MAV_CMD& cmd : singleTransect) {
+            cmd = cmd == MAV_CMD_CONDITION_GATE ? MAV_CMD_NAV_WAYPOINT : cmd;
+        }
+    }
+
+    if (!hasTurnaround) {
+        singleTransect.takeFirst();
+        singleTransect.takeLast();
+    }
+    
+    for (int i=0; i<_expectedTransectCount; i++) {
+        expectedCommands.append(singleTransect);
+    }
+
+    return expectedCommands;
+}
+
+void SurveyComplexItemTest::_testItemGenerationWorker(bool imagesInTurnaround, bool hasTurnaround, bool useConditionGate, const QList<MAV_CMD>& expectedCommands)
+{
+    qDebug() << QStringLiteral("_testItemGenerationWorker imagesInTuraround:%1 turnaround:%2 gate:%3").arg(imagesInTurnaround).arg(hasTurnaround).arg(useConditionGate);
+
+    _surveyItem->turnAroundDistance()->setRawValue(hasTurnaround ? 50 : 0);
+    _surveyItem->cameraTriggerInTurnAround()->setRawValue(imagesInTurnaround);
+    _planViewSettings->useConditionGate()->setRawValue(useConditionGate);
+
+    QList<MissionItem*> items;
+    _surveyItem->appendMissionItems(items, this);
+#if 0
+    // Handy for debugging failures
+    for (const MissionItem* item : items) {
+        qDebug() << "Cmd" << item->command();
+    }
+#endif
+    QCOMPARE(items.count(), expectedCommands.count());
+    for (int i=0; i<expectedCommands.count(); i++) {
+        int actualCommand = items[i]->command();
+        int expectedCommand = expectedCommands[i];
+#if 0
+        // Handy for debugging failures
+        qDebug() << "Index" << i;
+#endif
+        QCOMPARE(actualCommand, expectedCommand);
+    }
+}
+
+void SurveyComplexItemTest::_testItemGeneration(void)
+{
+    // Test all the combinations of: cameraTriggerInTurnAround: false, hasTurnAround: *, useConditionGate: *
+
+    typedef struct {
+        bool        hasTurnaround;
+        bool        useConditionGate;
+    } TestCase_t;
+
+    static const TestCase_t rgTestCases[] = {
+        { false,    false },
+        { false,    true },
+        { true,     false },
+        { true,     true },
+    };
+
+    for (const TestCase_t& testCase : rgTestCases) {
+        _testItemGenerationWorker(false /* imagesInTurnaround */, testCase.hasTurnaround, testCase.useConditionGate, _createExpectedCommands(testCase.hasTurnaround, testCase.useConditionGate));
+    }
+
+    // Test cameraTriggerInTurnAround = true cases
+
+    QList<MAV_CMD> imagesInTurnaroundCommands = {
+        // Transect 1
+        MAV_CMD_CONDITION_GATE,         // First turaround
+        MAV_CMD_DO_SET_CAM_TRIGG_DIST,
+        MAV_CMD_CONDITION_GATE,         // Survey entry
+        MAV_CMD_DO_SET_CAM_TRIGG_DIST,  // Survey entry also has trigger start
+        MAV_CMD_NAV_WAYPOINT,           // Survey exit
+        MAV_CMD_NAV_WAYPOINT,           // Turnaround
+        // Transect 2
+        MAV_CMD_NAV_WAYPOINT,           // Turnaround
+        MAV_CMD_CONDITION_GATE,         // Survey entry
+        MAV_CMD_DO_SET_CAM_TRIGG_DIST,  // Survey entry also has trigger start
+        MAV_CMD_NAV_WAYPOINT,           // Survey exit
+        MAV_CMD_CONDITION_GATE,         // Final turnaround
+        MAV_CMD_DO_SET_CAM_TRIGG_DIST,
+    };
+
+    _testItemGenerationWorker(true /* imagesInTurnaround */, true /* hasTurnaround */, true /* useConditionGate */, imagesInTurnaroundCommands);
+
+    // Switch to non CONDITION_GATE usage
+    for (MAV_CMD& cmd : imagesInTurnaroundCommands) {
+        cmd = cmd == MAV_CMD_CONDITION_GATE ? MAV_CMD_NAV_WAYPOINT : cmd;
+    }
+    _testItemGenerationWorker(true /* imagesInTurnaround */, true /* hasTurnaround */, false /* useConditionGate */, imagesInTurnaroundCommands);
+}
+
+void SurveyComplexItemTest::_testHoverCaptureItemGeneration(void)
+{
+    static const QList<MAV_CMD> singleFullTransect = {
+        MAV_CMD_NAV_WAYPOINT,           // Turnaround
+        MAV_CMD_NAV_WAYPOINT,           // Survey area entry edge
+        MAV_CMD_IMAGE_START_CAPTURE,
+        MAV_CMD_NAV_WAYPOINT,           // Interior trigger
+        MAV_CMD_IMAGE_START_CAPTURE,
+        MAV_CMD_NAV_WAYPOINT,           // Interior trigger
+        MAV_CMD_IMAGE_START_CAPTURE,
+        MAV_CMD_NAV_WAYPOINT,           // Survey area exit edge
+        MAV_CMD_IMAGE_START_CAPTURE,
+        MAV_CMD_NAV_WAYPOINT,           // Turnaround
+    };
+
+    QList<MAV_CMD> expectedCommands;
+    for (int i=0; i<_expectedTransectCount; i++) {
+        expectedCommands.append(singleFullTransect);
+    }
+
+    // Set trigger distance to generates two interior capture points
+    double polyHeightDistance = _polyVertices[0].distanceTo(_polyVertices[3]);
+    double triggerDistance = (polyHeightDistance / 3.0) + 1.0;
+    _surveyItem->cameraCalc()->adjustedFootprintFrontal()->setRawValue(triggerDistance);
+
+    qDebug() << "_testHoverCaptureItemGeneration";
+    _surveyItem->hoverAndCapture()->setRawValue(true);
+    _testItemGenerationWorker(false /* imagesInTurnaround */, true /* hasTurnaround */, true /* useConditionGate */, expectedCommands);
+    _testItemGenerationWorker(false /* imagesInTurnaround */, true /* hasTurnaround */, false /* useConditionGate */, expectedCommands);
+}

src/MissionManager/SurveyComplexItemTest.h

@@ -14,6 +14,7 @@
 #include "MultiSignalSpy.h"
 #include "SurveyComplexItem.h"
 #include "PlanMasterController.h"
+#include "PlanViewSettings.h"
 
 #include <QGeoCoordinate>
 
@@ -30,15 +31,18 @@ protected:
     void cleanup(void) final;
     
 private slots:
+    void _testHoverCaptureItemGeneration(void);
+
+private:
     void _testDirty(void);
     void _testGridAngle(void);
     void _testEntryLocation(void);
     void _testItemCount(void);
-
-private:
-
+    void _testItemGeneration(void);
     double _clampGridAngle180(double gridAngle);
     void _setPolygon(void);
+    QList<MAV_CMD> _createExpectedCommands(bool hasTurnaround, bool useConditionGate);
+    void _testItemGenerationWorker(bool imagesInTurnaround, bool hasTurnaround, bool useConditionGate, const QList<MAV_CMD>& expectedCommands);
 
     // SurveyComplexItem signals
 
@@ -69,5 +73,8 @@ private:
     MultiSignalSpy*         _multiSpy =             nullptr;
     SurveyComplexItem*      _surveyItem =           nullptr;
     QGCMapPolygon*          _mapPolygon =           nullptr;
-    QList<QGeoCoordinate>   _polyPoints;
+    PlanViewSettings*       _planViewSettings =     nullptr;
+    QList<QGeoCoordinate>   _polyVertices;
+
+    static const int _expectedTransectCount = 2;
 };

src/MissionManager/TransectStyleComplexItem.h

@@ -74,6 +74,9 @@ public:
     bool    hoverAndCaptureEnabled  (void) const { return hoverAndCapture()->rawValue().toBool(); }
     bool    triggerCamera           (void) const { return triggerDistance() != 0; }
 
+    // Used internally only by unit tests
+    int _transectCount(void) { return _transects.count(); }
+
     // Overrides from ComplexMissionItem
 
     int                 lastSequenceNumber  (void) const final;
@@ -159,8 +162,9 @@ protected:
         CoordTypeInterior,              ///< Interior waypoint for flight path only
         CoordTypeInteriorHoverTrigger,  ///< Interior waypoint for hover and capture trigger
         CoordTypeInteriorTerrainAdded,  ///< Interior waypoint added for terrain
-        CoordTypeSurveyEdge,            ///< Waypoint at edge of survey polygon
-        CoordTypeTurnaround             ///< Waypoint outside of survey polygon for turnaround
+        CoordTypeSurveyEntry,           ///< Waypoint at entry edge of survey polygon
+        CoordTypeSurveyExit,            ///< Waypoint at exit edge of survey polygon
+        CoordTypeTurnaround,            ///< First turnaround waypoint
     };
 
     typedef struct {

src/Settings/PlanView.SettingsGroup.json

@@ -16,5 +16,11 @@
     "shortDescription": "Show/Hide the mission item status display",
     "type":             "bool",
     "defaultValue":     false
+},
+{
+    "name":                 "useConditionGate",
+    "shortDescription":     "Use MAV_CMD_CONDITION_GATE for pattern generation",
+    "type":                 "bool",
+    "defaultValue":         false
 }
 ]

src/Settings/PlanViewSettings.cc

@@ -20,3 +20,4 @@ DECLARE_SETTINGGROUP(PlanView, "PlanView")
 DECLARE_SETTINGSFACT(PlanViewSettings, displayPresetsTabFirst)
 DECLARE_SETTINGSFACT(PlanViewSettings, aboveTerrainWarning)
 DECLARE_SETTINGSFACT(PlanViewSettings, showMissionItemStatus)
+DECLARE_SETTINGSFACT(PlanViewSettings, useConditionGate)

src/Settings/PlanViewSettings.h

@@ -23,4 +23,5 @@ public:
     DEFINE_SETTINGFACT(displayPresetsTabFirst)
     DEFINE_SETTINGFACT(aboveTerrainWarning)
     DEFINE_SETTINGFACT(showMissionItemStatus)
+    DEFINE_SETTINGFACT(useConditionGate)
 };

src/ui/preferences/GeneralSettings.qml

@@ -621,6 +621,11 @@ Rectangle {
                                     fact:                   QGroundControl.settingsManager.appSettings.defaultMissionItemAltitude
                                 }
                             }
+
+                            FactCheckBox {
+                                text:   qsTr("Use MAV_CMD_CONDITION_GATE for pattern generation")
+                                fact:   QGroundControl.settingsManager.planViewSettings.useConditionGate
+                            }
                         }
                     }