Merge pull request mavlink#4875 from DonLakeFlyer/FWLandBugFixes

Fixed Wing Landing Pattern Bug Fixes

src/FirmwarePlugin/PX4/MavCmdInfoCommon.json

@@ -8,6 +8,11 @@
             "id":           16,
             "comment":      "MAV_CMD_NAV_WAYPOINT",
             "paramRemove":  "2,3,4"
+        },
+        {
+            "id":                       21,
+            "comment":                  "MAV_CMD_NAV_LAND",
+            "paramRemove":              "1"
         }
     ]
 }

src/FirmwarePlugin/PX4/MavCmdInfoFixedWing.json

@@ -4,12 +4,5 @@
     "version": 1,
 
     "mavCmdInfo": [
-        {
-            "id":                       21,
-            "comment":                  "MAV_CMD_NAV_LAND",
-            "paramRemove":              "1,4",
-            "specifiesCoordinate":      false,
-            "specifiesAltitudeOnly":    true
-        }
     ]
 }

src/FlightDisplay/FlightDisplayViewMap.qml

@@ -248,10 +248,10 @@ FlightMap {
 
         onClicked: {
             switch (index) {
-            case 2:
+            case 1:
                 _flightMap.zoomLevel += 0.5
                 break
-            case 3:
+            case 2:
                 _flightMap.zoomLevel -= 0.5
                 break
             }

src/MissionManager/FixedWingLandingComplexItem.cc

@@ -12,6 +12,7 @@
 #include "MissionController.h"
 #include "QGCGeo.h"
 #include "QGroundControlQmlGlobal.h"
+#include "SimpleMissionItem.h"
 
 #include <QPolygonF>
 
@@ -169,6 +170,8 @@ bool FixedWingLandingComplexItem::load(const QJsonObject& complexObject, int seq
 
     setSequenceNumber(sequenceNumber);
 
+    _ignoreRecalcSignals = true;
+
     QGeoCoordinate coordinate;
     if (!JsonHelper::loadGeoCoordinate(complexObject[_jsonLoiterCoordinateKey], true /* altitudeRequired */, coordinate, errorString)) {
         return false;
@@ -188,7 +191,9 @@ bool FixedWingLandingComplexItem::load(const QJsonObject& complexObject, int seq
     _landingAltitudeRelative = complexObject[_jsonLandingAltitudeRelativeKey].toBool();
 
     _landingCoordSet = true;
-    _recalcFromHeadingAndDistanceChange();
+
+    _ignoreRecalcSignals = false;
+    _recalcFromCoordinateChange();
 
     return true;
 }
@@ -207,6 +212,8 @@ void FixedWingLandingComplexItem::appendMissionItems(QList<MissionItem*>& items,
 {
     int seqNum = _sequenceNumber;
 
+    // IMPORTANT NOTE: Any changes here must also be taken into account in scanForItem
+
     MissionItem* item = new MissionItem(seqNum++,                           // sequence number
                                         MAV_CMD_DO_LAND_START,              // MAV_CMD
                                         MAV_FRAME_MISSION,                  // MAV_FRAME
@@ -245,6 +252,82 @@ void FixedWingLandingComplexItem::appendMissionItems(QList<MissionItem*>& items,
     items.append(item);
 }
 
+bool FixedWingLandingComplexItem::scanForItem(QmlObjectListModel* visualItems, Vehicle* vehicle)
+{
+    qCDebug(FixedWingLandingComplexItemLog) << "FixedWingLandingComplexItem::scanForItem count" << visualItems->count();
+
+    if (visualItems->count() < 4) {
+        return false;
+    }
+
+    int lastItem = visualItems->count() - 1;
+
+    SimpleMissionItem* item = visualItems->value<SimpleMissionItem*>(lastItem--);
+    if (!item) {
+        return false;
+    }
+    MissionItem& missionItemLand = item->missionItem();
+    if (missionItemLand.command() != MAV_CMD_NAV_LAND ||
+            !(missionItemLand.frame() == MAV_FRAME_GLOBAL_RELATIVE_ALT || missionItemLand.frame() == MAV_FRAME_GLOBAL) ||
+            missionItemLand.param1() != 0 || missionItemLand.param2() != 0 || missionItemLand.param3() != 0 || missionItemLand.param4() == 1.0) {
+        return false;
+    }
+
+    item = visualItems->value<SimpleMissionItem*>(lastItem--);
+    if (!item) {
+        return false;
+    }
+    MissionItem& missionItemLoiter = item->missionItem();
+    if (missionItemLoiter.command() != MAV_CMD_NAV_LOITER_TO_ALT ||
+            !(missionItemLoiter.frame() == MAV_FRAME_GLOBAL_RELATIVE_ALT || missionItemLoiter.frame() == MAV_FRAME_GLOBAL) ||
+            missionItemLoiter.param1() != 1.0 || missionItemLoiter.param3() != 0 || missionItemLoiter.param4() != 1.0) {
+        return false;
+    }
+
+    item = visualItems->value<SimpleMissionItem*>(lastItem--);
+    if (!item) {
+        return false;
+    }
+    MissionItem& missionItemDoLandStart = item->missionItem();
+    if (missionItemDoLandStart.command() != MAV_CMD_DO_LAND_START ||
+            missionItemDoLandStart.param1() != 0 || missionItemDoLandStart.param2() != 0 || missionItemDoLandStart.param3() != 0 || missionItemDoLandStart.param4() != 0|| missionItemDoLandStart.param5() != 0|| missionItemDoLandStart.param6() != 0|| missionItemDoLandStart.param6() != 0) {
+        return false;
+    }
+
+    // We made it this far so we do have a Fixed Wing Landing Pattern item at the end of the mission
+
+    FixedWingLandingComplexItem* complexItem = new FixedWingLandingComplexItem(vehicle, visualItems);
+
+    complexItem->_ignoreRecalcSignals = true;
+
+    complexItem->_loiterAltitudeRelative = missionItemLoiter.frame() == MAV_FRAME_GLOBAL_RELATIVE_ALT;
+    complexItem->_loiterRadiusFact.setRawValue(qAbs(missionItemLoiter.param2()));
+    complexItem->_loiterClockwise = missionItemLoiter.param2() > 0;
+    complexItem->_loiterCoordinate.setLatitude(missionItemLoiter.param5());
+    complexItem->_loiterCoordinate.setLongitude(missionItemLoiter.param6());
+    complexItem->_loiterAltitudeFact.setRawValue(missionItemLoiter.param7());
+
+    complexItem->_landingAltitudeRelative = missionItemLand.frame() == MAV_FRAME_GLOBAL_RELATIVE_ALT;
+    complexItem->_landingCoordinate.setLatitude(missionItemLand.param5());
+    complexItem->_landingCoordinate.setLongitude(missionItemLand.param6());
+    complexItem->_landingAltitudeFact.setRawValue(missionItemLand.param7());
+
+    complexItem->_landingCoordSet = true;
+
+    complexItem->_ignoreRecalcSignals = false;
+    complexItem->_recalcFromCoordinateChange();
+    complexItem->setDirty(false);
+
+    lastItem = visualItems->count() - 1;
+    visualItems->removeAt(lastItem--)->deleteLater();
+    visualItems->removeAt(lastItem--)->deleteLater();
+    visualItems->removeAt(lastItem--)->deleteLater();
+
+    visualItems->append(complexItem);
+
+    return true;
+}
+
 double FixedWingLandingComplexItem::complexDistance(void) const
 {
     return _loiterCoordinate.distanceTo(_landingCoordinate);
@@ -437,3 +520,4 @@ void FixedWingLandingComplexItem::_setDirty(void)
 {
     setDirty(true);
 }
+

src/MissionManager/FixedWingLandingComplexItem.h

@@ -49,6 +49,9 @@ class FixedWingLandingComplexItem : public ComplexMissionItem
     void setLandingCoordinate       (const QGeoCoordinate& coordinate);
     void setLoiterCoordinate        (const QGeoCoordinate& coordinate);
 
+    /// Scans the loaded items for a landing pattern complex item
+    static bool scanForItem(QmlObjectListModel* visualItems, Vehicle* vehicle);
+
     // Overrides from ComplexMissionItem
 
     double              complexDistance     (void) const final;

src/MissionManager/MissionController.cc

@@ -1547,6 +1547,9 @@ QString MissionController::fileExtension(void) const
 
 void MissionController::_scanForAdditionalSettings(QmlObjectListModel* visualItems, Vehicle* vehicle)
 {
+    // First we look for a Fixed Wing Landing Pattern which is at the end
+    FixedWingLandingComplexItem::scanForItem(visualItems, vehicle);
+
     int scanIndex = 0;
     while (scanIndex < visualItems->count()) {
         VisualMissionItem* visualItem = visualItems->value<VisualMissionItem*>(scanIndex);

src/ui/MainWindow.cc

@@ -40,6 +40,7 @@
 #include "LogCompressor.h"
 #include "UAS.h"
 #include "QGCImageProvider.h"
+#include "QGCCorePlugin.h"
 
 #ifndef __mobile__
 #include "Linecharts.h"
@@ -175,6 +176,9 @@ MainWindow::MainWindow()
     _ui.menuWidgets->addAction(qmlTestAction);
 #endif
 
+    connect(qgcApp()->toolbox()->corePlugin(), &QGCCorePlugin::showAdvancedUIChanged, this, &MainWindow::_showAdvancedUIChanged);
+    _showAdvancedUIChanged(qgcApp()->toolbox()->corePlugin()->showAdvancedUI());
+
     // Status Bar
     setStatusBar(new QStatusBar(this));
     statusBar()->setSizeGripEnabled(true);
@@ -558,3 +562,13 @@ QObject* MainWindow::rootQmlObject(void)
 {
     return _mainQmlWidgetHolder->getRootObject();
 }
+
+void MainWindow::_showAdvancedUIChanged(bool advanced)
+{
+    if (advanced) {
+        menuBar()->addMenu(_ui.menuFile);
+        menuBar()->addMenu(_ui.menuWidgets);
+    } else {
+        menuBar()->clear();
+    }
+}

src/ui/MainWindow.h

@@ -158,6 +158,7 @@ private slots:
     void _closeWindow(void) { close(); }
     void _vehicleAdded(Vehicle* vehicle);
     void _showDockWidgetAction(bool show);
+    void _showAdvancedUIChanged(bool advanced);
 
 #ifdef UNITTEST_BUILD
     void _showQmlTestWidget(void);

src/ui/MainWindow.ui

@@ -54,7 +54,7 @@
      <height>22</height>
     </rect>
    </property>
-   <widget class="QMenu" name="menuMGround">
+   <widget class="QMenu" name="menuFile">
     <property name="title">
      <string>File</string>
     </property>
@@ -68,7 +68,7 @@
      <string>Widgets</string>
     </property>
    </widget>
-   <addaction name="menuMGround"/>
+   <addaction name="menuFile"/>
    <addaction name="menuWidgets"/>
   </widget>
   <widget class="QStatusBar" name="statusBar"/>
@@ -98,7 +98,7 @@
     <bool>true</bool>
    </property>
    <property name="text">
-    <string>Show Status Bar</string>
+    <string>Replay Flight Data</string>
    </property>
   </action>
  </widget>