Merge pull request #8947 from DonLakeFlyer/PlanMissionState

Plan: Fix recalc bugs associated with vtol state, speed, vehicle/gimbal yaw

src/MissionManager/MissionController.h

@@ -45,27 +45,29 @@ public:
     ~MissionController();
 
     typedef struct {
-        double  maxTelemetryDistance;
-        double  totalDistance;
-        double  totalTime;
-        double  hoverDistance;
-        double  hoverTime;
-        double  cruiseDistance;
-        double  cruiseTime;
-        double  cruiseSpeed;
-        double  hoverSpeed;
-        double  vehicleSpeed;           ///< Either cruise or hover speed based on vehicle type and vtol state
-        double  vehicleYaw;
-        double  gimbalYaw;              ///< NaN signals yaw was never changed
-        double  gimbalPitch;            ///< NaN signals pitch was never changed
-        int     mAhBattery;             ///< 0 for not available
-        double  hoverAmps;              ///< Amp consumption during hover
-        double  cruiseAmps;             ///< Amp consumption during cruise
-        double  ampMinutesAvailable;    ///< Amp minutes available from single battery
-        double  hoverAmpsTotal;         ///< Total hover amps used
-        double  cruiseAmpsTotal;        ///< Total cruise amps used
-        int     batteryChangePoint;     ///< -1 for not supported, 0 for not needed
-        int     batteriesRequired;      ///< -1 for not supported
+        double                      maxTelemetryDistance;
+        double                      totalDistance;
+        double                      totalTime;
+        double                      hoverDistance;
+        double                      hoverTime;
+        double                      cruiseDistance;
+        double                      cruiseTime;
+        int                         mAhBattery;             ///< 0 for not available
+        double                      hoverAmps;              ///< Amp consumption during hover
+        double                      cruiseAmps;             ///< Amp consumption during cruise
+        double                      ampMinutesAvailable;    ///< Amp minutes available from single battery
+        double                      hoverAmpsTotal;         ///< Total hover amps used
+        double                      cruiseAmpsTotal;        ///< Total cruise amps used
+        int                         batteryChangePoint;     ///< -1 for not supported, 0 for not needed
+        int                         batteriesRequired;      ///< -1 for not supported
+        double                      vehicleYaw;
+        double                      gimbalYaw;              ///< NaN signals yaw was never changed
+        double                      gimbalPitch;            ///< NaN signals pitch was never changed
+        // The following values are the state prior to executing this item
+        QGCMAVLink::VehicleClass_t  vtolMode;               ///< Either VehicleClassFixedWing, VehicleClassMultiRotor, VehicleClassGeneric (mode unknown)
+        double                      cruiseSpeed;
+        double                      hoverSpeed;
+        double                      vehicleSpeed;           ///< Either cruise or hover speed based on vehicle type and vtol state
     } MissionFlightStatus_t;
 
     Q_PROPERTY(QmlObjectListModel*  visualItems                     READ visualItems                    NOTIFY visualItemsChanged)

src/MissionManager/MissionControllerTest.cc

@@ -167,17 +167,68 @@ void MissionControllerTest::_testGimbalRecalc(void)
         QVERIFY(qIsNaN(visualItem->missionGimbalYaw()));
     }
 
-#if 0
-    // FIXME: No longer works due to signal compression
-    // Specify gimbal yaw on settings item should generate yaw on all items
-    MissionSettingsItem* settingsItem = _missionController->visualItems()->value<MissionSettingsItem*>(0);
-    settingsItem->cameraSection()->setSpecifyGimbal(true);
-    settingsItem->cameraSection()->gimbalYaw()->setRawValue(0.0);
+    // Specify gimbal yaw on settings item should generate yaw on all subsequent items
+    const int yawIndex = 2;
+    SimpleMissionItem* item = _missionController->visualItems()->value<SimpleMissionItem*>(yawIndex);
+    item->cameraSection()->setSpecifyGimbal(true);
+    item->cameraSection()->gimbalYaw()->setRawValue(0.0);
+    QTest::qWait(100); // Recalcs in MissionController are queued to remove dups. Allow return to main message loop.
     for (int i=1; i<_missionController->visualItems()->count(); i++) {
+        //qDebug() << i;
         VisualMissionItem* visualItem = _missionController->visualItems()->value<VisualMissionItem*>(i);
-        QCOMPARE(visualItem->missionGimbalYaw(), 0.0);
+        if (i >= yawIndex) {
+            QCOMPARE(visualItem->missionGimbalYaw(), 0.0);
+        } else {
+            QVERIFY(qIsNaN(visualItem->missionGimbalYaw()));
+        }
+    }
+}
+
+void MissionControllerTest::_testVehicleYawRecalc(void)
+{
+    _initForFirmwareType(MAV_AUTOPILOT_PX4);
+
+    double wpDistance   = 1000;
+    double wpAngleInc   = 45;
+    double wpAngle      = 0;
+
+    int cMissionItems = 4;
+    QGeoCoordinate currentCoord(0, 0);
+    _missionController->insertSimpleMissionItem(currentCoord, 1);
+    for (int i=2; i<=cMissionItems; i++) {
+        wpAngle += wpAngleInc;
+        currentCoord = currentCoord.atDistanceAndAzimuth(wpDistance, wpAngle);
+        _missionController->insertSimpleMissionItem(currentCoord, i);
+    }
+
+    QTest::qWait(100); // Recalcs in MissionController are queued to remove dups. Allow return to main message loop.
+
+    // No specific vehicle yaw set yet. Vehicle yaw should track flight path.
+    double expectedVehicleYaw = wpAngleInc;
+    for (int i=2; i<cMissionItems; i++) {
+        //qDebug() << i;
+        VisualMissionItem* visualItem = _missionController->visualItems()->value<VisualMissionItem*>(i);
+        QCOMPARE(visualItem->missionVehicleYaw(), expectedVehicleYaw);
+        if (i <= cMissionItems - 1) {
+            expectedVehicleYaw += wpAngleInc;
+        }
+    }
+
+    SimpleMissionItem* simpleItem = _missionController->visualItems()->value<SimpleMissionItem*>(3);
+    simpleItem->missionItem().setParam4(66);
+
+    QTest::qWait(100); // Recalcs in MissionController are queued to remove dups. Allow return to main message loop.
+
+    // All item should track vehicle path except for the one changed
+    expectedVehicleYaw = wpAngleInc;
+    for (int i=2; i<cMissionItems; i++) {
+        //qDebug() << i;
+        VisualMissionItem* visualItem = _missionController->visualItems()->value<VisualMissionItem*>(i);
+        QCOMPARE(visualItem->missionVehicleYaw(), i == 3 ? 66.0 : expectedVehicleYaw);
+        if (i <= cMissionItems - 1) {
+            expectedVehicleYaw += wpAngleInc;
+        }
     }
-#endif
 }
 
 void MissionControllerTest::_testLoadJsonSectionAvailable(void)

src/MissionManager/MissionControllerTest.h

@@ -32,11 +32,12 @@ public:
 private slots:
     void cleanup(void);
 
-    void _testGimbalRecalc              (void);
     void _testLoadJsonSectionAvailable  (void);
     void _testEmptyVehicleAPM           (void);
     void _testEmptyVehiclePX4           (void);
     void _testGlobalAltMode             (void);
+    void _testGimbalRecalc              (void);
+    void _testVehicleYawRecalc          (void);
 
 private:
 #if 0