Merge pull request #6285 from arwagoner/pr-rally-points-fix

rally point upload and download functionality

src/MissionManager/RallyPointController.cc

@@ -300,10 +300,8 @@ bool RallyPointController::showPlanFromManagerVehicle (void)
             qCDebug(RallyPointControllerLog) << "showPlanFromManagerVehicle: syncInProgress wait for signal";
             return true;
         } else {
-            // Fake a _loadComplete with the current items
+            qCDebug(RallyPointControllerLog) << "showPlanFromManagerVehicle: sync complete";
-            qCDebug(RallyPointControllerLog) << "showPlanFromManagerVehicle: sync complete simulate signal";
             _itemsRequested = true;
-            _managerLoadComplete(_rallyPointManager->points());
             return false;
         }
     }

src/MissionManager/RallyPointManager.cc

@@ -8,6 +8,7 @@
  ****************************************************************************/
 
 #include "RallyPointManager.h"
+#include "ParameterManager.h"
 #include "Vehicle.h"
 
 QGC_LOGGING_CATEGORY(RallyPointManagerLog, "RallyPointManagerLog")
@@ -15,10 +16,16 @@ QGC_LOGGING_CATEGORY(RallyPointManagerLog, "RallyPointManagerLog")
 RallyPointManager::RallyPointManager(Vehicle* vehicle)
     : QObject(vehicle)
     , _vehicle(vehicle)
+    , _planManager              (vehicle, MAV_MISSION_TYPE_RALLY)
 {
-
+    connect(&_planManager, &PlanManager::inProgressChanged,         this, &RallyPointManager::inProgressChanged);
+    connect(&_planManager, &PlanManager::error,                     this, &RallyPointManager::error);
+    connect(&_planManager, &PlanManager::removeAllComplete,         this, &RallyPointManager::removeAllComplete);
+    connect(&_planManager, &PlanManager::sendComplete,              this, &RallyPointManager::sendComplete);
+    connect(&_planManager, &PlanManager::newMissionItemsAvailable,  this, &RallyPointManager::_planManagerLoadComplete);
 }
 
+
 RallyPointManager::~RallyPointManager()
 {
 
@@ -33,24 +40,65 @@ void RallyPointManager::_sendError(ErrorCode_t errorCode, const QString& errorMs
 
 void RallyPointManager::loadFromVehicle(void)
 {
-    // No support in generic vehicle
+     _planManager.loadFromVehicle();
-    emit loadComplete(QList<QGeoCoordinate>());
 }
 
 void RallyPointManager::sendToVehicle(const QList<QGeoCoordinate>& rgPoints)
 {
-    // No support in generic vehicle
+    QList<MissionItem*> rallyItems;
-    Q_UNUSED(rgPoints);
+
-    emit sendComplete(false /* error */);
+    for (int i=0; i<rgPoints.count(); i++) {
+
+        MissionItem* item = new MissionItem(0,
+                                            MAV_CMD_NAV_RALLY_POINT,
+                                            MAV_FRAME_GLOBAL,
+                                            0, 0, 0, 0,                 // param 1-4 unused
+                                            rgPoints[i].latitude(),
+                                            rgPoints[i].longitude(),
+                                            rgPoints[i].altitude(),
+                                            false,                      // autocontinue
+                                            false,                      // isCurrentItem
+                                            this);                      // parent
+        rallyItems.append(item);
+    }
+
+    // Plan manager takes control of MissionItems, so no need to delete
+    _planManager.writeMissionItems(rallyItems);
 }
 
 void RallyPointManager::removeAll(void)
 {
-    // No support in generic vehicle
+    _planManager.removeAll();
-    emit removeAllComplete(false /* error */);
 }
 
 bool RallyPointManager::supported(void) const
 {
     return (_vehicle->capabilityBits() & MAV_PROTOCOL_CAPABILITY_MISSION_RALLY) && (_vehicle->maxProtoVersion() >= 200);
 }
+
+void RallyPointManager::_planManagerLoadComplete(bool removeAllRequested)
+{
+    _rgPoints.clear();
+
+    Q_UNUSED(removeAllRequested);
+
+    const QList<MissionItem*>& rallyItems = _planManager.missionItems();
+
+    for (int i=0; i<rallyItems.count(); i++) {
+        MissionItem* item = rallyItems[i];
+
+        MAV_CMD command = item->command();
+
+        if (command == MAV_CMD_NAV_RALLY_POINT) {
+            _rgPoints.append(QGeoCoordinate(item->param5(), item->param6(), item->param7()));
+
+
+        } else {
+            qCDebug(RallyPointManagerLog) << "RallyPointManager load: Unsupported command %1" << item->command();
+            break;
+        }
+    }
+
+
+    emit loadComplete(_rgPoints);
+}

src/MissionManager/RallyPointManager.h

@@ -14,8 +14,10 @@
 #include <QGeoCoordinate>
 
 #include "QGCLoggingCategory.h"
+#include "PlanManager.h"
 
 class Vehicle;
+class PlanManager;
 
 Q_DECLARE_LOGGING_CATEGORY(RallyPointManagerLog)
 
@@ -66,10 +68,14 @@ signals:
     void removeAllComplete  (bool error);
     void sendComplete       (bool error);
 
+private slots:
+    void _planManagerLoadComplete   (bool removeAllRequested);
+
 protected:
     void _sendError(ErrorCode_t errorCode, const QString& errorMsg);
 
     Vehicle*                _vehicle;
+    PlanManager             _planManager;
     QList<QGeoCoordinate>   _rgPoints;
 };