Merge remote-tracking branch 'origin/thread/OP-1474_PID_Scaling_Banks' into thread/OP-1474_PID_Scaling_Banks

ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp

@@ -26,7 +26,9 @@
  */
 #include "modeluavoproxy.h"
 #include "extensionsystem/pluginmanager.h"
+#include "uavobjecthelper.h"
 
+#include <QProgressDialog>
 #include <math.h>
 
 ModelUavoProxy::ModelUavoProxy(QObject *parent, flightDataModel *model) : QObject(parent), myModel(model)
@@ -37,91 +39,126 @@ ModelUavoProxy::ModelUavoProxy(QObject *parent, flightDataModel *model) : QObjec
 
     objMngr = pm->getObject<UAVObjectManager>();
     Q_ASSERT(objMngr != NULL);
-
-    completionCountdown = 0;
-    successCountdown    = 0;
 }
 
 void ModelUavoProxy::sendPathPlan()
 {
     modelToObjects();
 
-    PathPlan *pathPlan = PathPlan::GetInstance(objMngr, 0);
+    PathPlan *pathPlan = PathPlan::GetInstance(objMngr);
-    connect(pathPlan, SIGNAL(transactionCompleted(UAVObject *, bool)),
-            this, SLOT(pathPlanElementSent(UAVObject *, bool)), Qt::UniqueConnection);
 
-    Waypoint *waypoint = Waypoint::GetInstance(objMngr, 0);
+    const int waypointCount = pathPlan->getWaypointCount();
-    connect(waypoint, SIGNAL(transactionCompleted(UAVObject *, bool)),
+    const int actionCount   = pathPlan->getPathActionCount();
-            this, SLOT(pathPlanElementSent(UAVObject *, bool)), Qt::UniqueConnection);
 
-    PathAction *action = PathAction::GetInstance(objMngr, 0);
+    QProgressDialog progress(tr("Sending the path plan to the board... "), "", 0, 1 + waypointCount + actionCount);
-    connect(action, SIGNAL(transactionCompleted(UAVObject *, bool)),
+    progress.setWindowModality(Qt::WindowModal);
-            this, SLOT(pathPlanElementSent(UAVObject *, bool)), Qt::UniqueConnection);
+    progress.setCancelButton(NULL);
+    progress.show();
 
-    // we will start 3 update all
+    UAVObjectUpdaterHelper updateHelper;
-    completionCountdown = 3;
-    successCountdown    = completionCountdown;
 
-    pathPlan->updated();
+    // send PathPlan
-    waypoint->updatedAll();
+    bool success = (updateHelper.doObjectAndWait(pathPlan) == UAVObjectUpdaterHelper::SUCCESS);
-    action->updatedAll();
+    progress.setValue(1);
-}
 
-void ModelUavoProxy::pathPlanElementSent(UAVObject *obj, bool success)
+    if (success) {
-{
+        // send Waypoint instances
-    obj->disconnect(this);
+        qDebug() << "sending" << waypointCount << "waypoints";
-
+        for (int i = 0; i < waypointCount; ++i) {
-    completionCountdown--;
+            Waypoint *waypoint = Waypoint::GetInstance(objMngr, i);
-    successCountdown -= success ? 1 : 0;
+            success = (updateHelper.doObjectAndWait(waypoint) == UAVObjectUpdaterHelper::SUCCESS);
-
+            if (!success) {
-    if (completionCountdown == 0) {
+                break;
-        qDebug() << "ModelUavoProxy::pathPlanSent - completed" << (successCountdown == 0);
+            }
-        if (successCountdown == 0) {
+            progress.setValue(progress.value() + 1);
-            QMessageBox::information(NULL, tr("Path Plan Upload Successful"), tr("Path plan upload was successful."));
-        } else {
-            QMessageBox::critical(NULL, tr("Path Plan Upload Failed"), tr("Failed to upload the path plan !"));
         }
     }
+
+    if (success) {
+        // send PathAction instances
+        qDebug() << "sending" << actionCount << "path actions";
+        for (int i = 0; i < actionCount; ++i) {
+            PathAction *action = PathAction::GetInstance(objMngr, i);
+            success = (updateHelper.doObjectAndWait(action) == UAVObjectUpdaterHelper::SUCCESS);
+            if (!success) {
+                break;
+            }
+            progress.setValue(progress.value() + 1);
+        }
+    }
+
+    qDebug() << "ModelUavoProxy::pathPlanSent - completed" << success;
+    if (!success) {
+        QMessageBox::critical(NULL, tr("Sending Path Plan Failed!"), tr("Failed to send the path plan to the board."));
+    }
+
+    progress.close();
 }
 
 void ModelUavoProxy::receivePathPlan()
 {
-    PathPlan *pathPlan = PathPlan::GetInstance(objMngr, 0);
+    QProgressDialog progress(tr("Receiving the path plan from the board... "), "", 0, 0);
+    progress.setWindowModality(Qt::WindowModal);
+    progress.setCancelButton(NULL);
+    progress.show();
 
-    connect(pathPlan, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(pathPlanElementReceived(UAVObject *, bool)));
+    UAVObjectRequestHelper requestHelper;
 
-    Waypoint *waypoint = Waypoint::GetInstance(objMngr, 0);
+    PathPlan *pathPlan = PathPlan::GetInstance(objMngr);
-    connect(waypoint, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(pathPlanElementReceived(UAVObject *, bool)));
+    bool success = (requestHelper.doObjectAndWait(pathPlan) == UAVObjectUpdaterHelper::SUCCESS);
 
-    PathAction *action = PathAction::GetInstance(objMngr, 0);
+    const int waypointCount = pathPlan->getWaypointCount();
-    connect(action, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(pathPlanElementReceived(UAVObject *, bool)));
+    const int actionCount   = pathPlan->getPathActionCount();
 
-    // we will start 3 update requests
+    progress.setMaximum(1 + waypointCount + actionCount);
-    completionCountdown = 3;
+    progress.setValue(1);
-    successCountdown    = completionCountdown;
 
-    pathPlan->requestUpdate();
+    if (success && (waypointCount > objMngr->getNumInstances(Waypoint::OBJID))) {
-    waypoint->requestUpdateAll();
+        // allocate needed Waypoint instances
-    action->requestUpdateAll();
+        Waypoint *waypoint = new Waypoint;
-}
+        waypoint->initialize(waypointCount - 1, waypoint->getMetaObject());
-
+        success = objMngr->registerObject(waypoint);
-void ModelUavoProxy::pathPlanElementReceived(UAVObject *obj, bool success)
+    }
-{
+    if (success) {
-    obj->disconnect(this);
+        // request Waypoint instances
-
+        qDebug() << "requesting" << waypointCount << "waypoints";
-    completionCountdown--;
+        for (int i = 0; i < waypointCount; ++i) {
-    successCountdown -= success ? 1 : 0;
+            Waypoint *waypoint = Waypoint::GetInstance(objMngr, i);
-
+            success = (requestHelper.doObjectAndWait(waypoint) == UAVObjectRequestHelper::SUCCESS);
-    if (completionCountdown == 0) {
+            if (!success) {
-        qDebug() << "ModelUavoProxy::pathPlanReceived - completed" << (successCountdown == 0);
+                break;
-        if (successCountdown == 0) {
-            if (objectsToModel()) {
-                QMessageBox::information(NULL, tr("Path Plan Download Successful"), tr("Path plan download was successful."));
             }
-        } else {
+            progress.setValue(progress.value() + 1);
-            QMessageBox::critical(NULL, tr("Path Plan Download Failed"), tr("Failed to download the path plan !"));
         }
     }
+
+    if (success && (actionCount > objMngr->getNumInstances(PathAction::OBJID))) {
+        // allocate needed PathAction instances
+        PathAction *action = new PathAction;
+        action->initialize(actionCount - 1, action->getMetaObject());
+        success = objMngr->registerObject(action);
+    }
+    if (success) {
+        // request PathAction isntances
+        qDebug() << "requesting" << actionCount << "path actions";
+        for (int i = 0; i < actionCount; ++i) {
+            PathAction *action = PathAction::GetInstance(objMngr, i);
+            success = (requestHelper.doObjectAndWait(action) == UAVObjectRequestHelper::SUCCESS);
+            if (!success) {
+                break;
+            }
+            progress.setValue(progress.value() + 1);
+        }
+    }
+
+    qDebug() << "ModelUavoProxy::pathPlanReceived - completed" << success;
+    if (success) {
+        objectsToModel();
+    } else {
+        QMessageBox::critical(NULL, tr("Receiving Path Plan Failed!"), tr("Failed to receive the path plan from the board."));
+    }
+
+    progress.close();
 }
 
 // update waypoint and path actions UAV objects

ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.h

@@ -49,9 +49,6 @@ private:
     UAVObjectManager *objMngr;
     flightDataModel *myModel;
 
-    uint completionCountdown;
-    uint successCountdown;
-
     bool modelToObjects();
     bool objectsToModel();
 
@@ -67,10 +64,6 @@ private:
     void pathActionToModel(int i, PathAction::DataFields &data);
 
     quint8 computePathPlanCrc(int waypointCount, int actionCount);
-
-private slots:
-    void pathPlanElementSent(UAVObject *, bool success);
-    void pathPlanElementReceived(UAVObject *, bool success);
 };
 
 #endif // MODELUAVOPROXY_H

ground/openpilotgcs/src/plugins/uavobjectutil/uavobjecthelper.h

@@ -43,7 +43,10 @@ public:
     virtual ~AbstractUAVObjectHelper();
 
     enum Result { SUCCESS, FAIL, TIMEOUT };
-    Result doObjectAndWait(UAVObject *object, int timeout);
+
+    // default timeout = 3 x 250ms + 50ms safety margin = 800ms
+    // where 3 is the number of UAVTalk retries and 250ms is the UAVTalk timeout
+    Result doObjectAndWait(UAVObject *object, int timeout = 800);
 
 protected:
     virtual void doObjectAndWaitImpl() = 0;

ground/openpilotgcs/src/plugins/uavtalk/telemetry.cpp

@@ -48,10 +48,12 @@ Telemetry::Telemetry(UAVTalk *utalk, UAVObjectManager *objMngr) : objMngr(objMng
     }
 
     // Listen to new object creations
-    connect(objMngr, SIGNAL(newObject(UAVObject *)), this, SLOT(newObject(UAVObject *)));
+    // connection must be direct, if not, it is not possible to create and send (or request) an object in one go
-    connect(objMngr, SIGNAL(newInstance(UAVObject *)), this, SLOT(newInstance(UAVObject *)));
+    connect(objMngr, SIGNAL(newObject(UAVObject *)), this, SLOT(newObject(UAVObject *)), Qt::DirectConnection);
+    connect(objMngr, SIGNAL(newInstance(UAVObject *)), this, SLOT(newInstance(UAVObject *)), Qt::DirectConnection);
 
     // Listen to transaction completions
+    // these slots will be executed in the telemetry thread
     // TODO should send a status (SUCCESS, FAILED, TIMEOUT)
     connect(utalk, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(transactionCompleted(UAVObject *, bool)));
 
@@ -568,6 +570,10 @@ void Telemetry::newObject(UAVObject *obj)
 {
     QMutexLocker locker(mutex);
 
+#ifdef VERBOSE_TELEMETRY
+    qDebug() << "Telemetry - new object" << obj->toStringBrief();
+#endif
+
     registerObject(obj);
 }
 
@@ -575,6 +581,10 @@ void Telemetry::newInstance(UAVObject *obj)
 {
     QMutexLocker locker(mutex);
 
+#ifdef VERBOSE_TELEMETRY
+    qDebug() << "Telemetry - new object instance" << obj->toStringBrief();
+#endif
+
     registerObject(obj);
 }