From 398a3ab0e6424376af9cf6e7132d11f98c6b707c Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Sun, 1 Dec 2013 19:38:23 +0100 Subject: [PATCH] OP-112 OP-1120 waypoint editor now sends and requests Waypoints and PathActions in a transacted and asynchronous manner. Board does not send Waypoints and PathActions periodically anymore. --- .../src/plugins/opmap/modeluavoproxy.cpp | 115 ++++++++++++++---- .../src/plugins/opmap/modeluavoproxy.h | 14 ++- .../src/plugins/opmap/opmapgadgetwidget.cpp | 6 +- shared/uavobjectdefinition/pathaction.xml | 34 +++--- shared/uavobjectdefinition/waypoint.xml | 8 +- 5 files changed, 128 insertions(+), 49 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp index 9dbc1fbfb..6937ee1b0 100644 --- a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp @@ -31,14 +31,83 @@ ModelUavoProxy::ModelUavoProxy(QObject *parent, flightDataModel *model) : QObject(parent), myModel(model) { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - Q_ASSERT(pm != NULL); + objManager = pm->getObject(); Q_ASSERT(objManager != NULL); - waypointObj = Waypoint::GetInstance(objManager); - Q_ASSERT(waypointObj != NULL); - pathactionObj = PathAction::GetInstance(objManager); - Q_ASSERT(pathactionObj != NULL); +} + +void ModelUavoProxy::sendFlightPlan() +{ + modelToObjects(); + + Waypoint *waypoint = Waypoint::GetInstance(objManager, 0); + connect(waypoint, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(flightPlanSent(UAVObject *, bool))); + + PathAction *action = PathAction::GetInstance(objManager, 0); + connect(action, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(flightPlanSent(UAVObject *, bool))); + + completionCount = 0; + completionSuccessCount = 0; + + waypoint->updatedAll(); + action->updatedAll(); +} + +void ModelUavoProxy::flightPlanSent(UAVObject *obj, bool success) +{ + obj->disconnect(this); + + completionCount++; + if (success) { + completionSuccessCount++; + } + + if (completionCount == 2) { + qDebug() << "ModelUavoProxy::flightPlanSent - success" << (completionSuccessCount == 2); + if (completionSuccessCount == 2) { + // TODO : popup? + } + else { + // TODO : popup? + } + } +} + +void ModelUavoProxy::receiveFlightPlan() +{ + Waypoint *waypoint = Waypoint::GetInstance(objManager, 0); + connect(waypoint, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(flightPlanReceived(UAVObject *, bool))); + + PathAction *action = PathAction::GetInstance(objManager, 0); + connect(action, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(flightPlanReceived(UAVObject *, bool))); + + completionCount = 0; + completionSuccessCount = 0; + + waypoint->requestUpdateAll(); + action->requestUpdateAll(); +} + +void ModelUavoProxy::flightPlanReceived(UAVObject *obj, bool success) +{ + obj->disconnect(this); + + completionCount++; + if (success) { + completionSuccessCount++; + } + + if (completionCount == 2) { + qDebug() << "ModelUavoProxy::flightPlanReceived - success" << (completionSuccessCount == 2); + if (completionSuccessCount == 2) { + objectsToModel(); + // TODO : popup? + } + else { + // TODO : popup? + } + } } void ModelUavoProxy::modelToObjects() @@ -80,10 +149,8 @@ void ModelUavoProxy::modelToObjects() action = createPathAction(actionCount, action); actionCount++; - // send action update to UAV + // update UAVObject action->setData(actionData); - action->updated(); - qDebug() << "ModelUAVProxy::modelToObjects - sent action instance :" << action->getInstID(); } else { action->deleteLater(); @@ -104,16 +171,14 @@ void ModelUavoProxy::modelToObjects() // connect waypoint to path action waypointData.Action = action->getInstID(); - // send waypoint update to UAV + // update UAVObject waypoint->setData(waypointData); - waypoint->updated(); - qDebug() << "ModelUAVProxy::modelToObjects - sent waypoint instance :" << waypoint->getInstID(); } } Waypoint *ModelUavoProxy::createWaypoint(int index, Waypoint *newWaypoint) { Waypoint *waypoint = NULL; - int count = objManager->getNumInstances(waypointObj->getObjID()); + int count = objManager->getNumInstances(Waypoint::OBJID); if (index < count) { // reuse object qDebug() << "ModelUAVProxy::createWaypoint - reused waypoint instance :" << index << "/" << count; @@ -138,7 +203,7 @@ Waypoint *ModelUavoProxy::createWaypoint(int index, Waypoint *newWaypoint) { PathAction *ModelUavoProxy::createPathAction(int index, PathAction *newAction) { PathAction *action = NULL; - int count = objManager->getNumInstances(pathactionObj->getObjID()); + int count = objManager->getNumInstances(PathAction::OBJID); if (index < count) { // reuse object qDebug() << "ModelUAVProxy::createPathAction - reused action instance :" << index << "/" << count; @@ -162,7 +227,7 @@ PathAction *ModelUavoProxy::createPathAction(int index, PathAction *newAction) { } PathAction *ModelUavoProxy::findPathAction(const PathAction::DataFields &actionData, int actionCount) { - int instancesCount = objManager->getNumInstances(pathactionObj->getObjID()); + int instancesCount = objManager->getNumInstances(PathAction::OBJID); int count = actionCount <= instancesCount ? actionCount : instancesCount; for (int i = 0; i < count; ++i) { PathAction *action = PathAction::GetInstance(objManager, i); @@ -189,21 +254,29 @@ PathAction *ModelUavoProxy::findPathAction(const PathAction::DataFields &actionD void ModelUavoProxy::objectsToModel() { - myModel->removeRows(0, myModel->rowCount()); + int instanceCount = objManager->getNumInstances(Waypoint::OBJID); + // TODO retain only reachable waypoints + + int rowCount = myModel->rowCount(); + if (instanceCount < rowCount) { + myModel->removeRows(instanceCount, rowCount - instanceCount); + } + else if (instanceCount > rowCount) { + myModel->insertRows(rowCount, instanceCount - rowCount); + } - int instanceCount = objManager->getNumInstances(waypointObj->getObjID()); for (int i = 0; i < instanceCount; ++i) { Waypoint *waypoint = Waypoint::GetInstance(objManager, i); Q_ASSERT(waypoint); if (!waypoint) { continue; } - myModel->insertRow(i); Waypoint::DataFields waypointData = waypoint->getData(); waypointToModel(i, waypointData); - PathAction *action = PathAction::GetInstance(objManager, waypointData.Action); + int actionId = waypointData.Action; + PathAction *action = PathAction::GetInstance(objManager, actionId); Q_ASSERT(action); if (!action) { continue; @@ -226,8 +299,8 @@ void ModelUavoProxy::modelToWaypoint(int i, Waypoint::DataFields &data) { index = myModel->index(i, flightDataModel::VELOCITY); velocity = myModel->data(index).toFloat(); - data.Position[Waypoint::POSITION_NORTH] = distance * cos(bearing / (180 * M_PI)); - data.Position[Waypoint::POSITION_EAST] = distance * sin(bearing / (180 * M_PI)); + data.Position[Waypoint::POSITION_NORTH] = distance * cos(bearing / 180 * M_PI); + data.Position[Waypoint::POSITION_EAST] = distance * sin(bearing / 180 * M_PI); data.Position[Waypoint::POSITION_DOWN] = -altitude; data.Velocity = velocity; } @@ -237,7 +310,7 @@ void ModelUavoProxy::waypointToModel(int i, Waypoint::DataFields &data) { double distance = sqrt(data.Position[Waypoint::POSITION_NORTH] * data.Position[Waypoint::POSITION_NORTH] + data.Position[Waypoint::POSITION_EAST] * data.Position[Waypoint::POSITION_EAST]); - double bearing = atan2(data.Position[Waypoint::POSITION_EAST], data.Position[Waypoint::POSITION_NORTH]) * (180 / M_PI); + double bearing = atan2(data.Position[Waypoint::POSITION_EAST], data.Position[Waypoint::POSITION_NORTH]) * 180 / M_PI; if (bearing != bearing) { bearing = 0; } diff --git a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.h b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.h index 38a88d1e8..1b685a739 100644 --- a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.h +++ b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.h @@ -38,15 +38,18 @@ class ModelUavoProxy : public QObject { public: explicit ModelUavoProxy(QObject *parent, flightDataModel *model); -public slots: void modelToObjects(); void objectsToModel(); +public slots: + void sendFlightPlan(); + void receiveFlightPlan(); + private: UAVObjectManager *objManager; - Waypoint *waypointObj; - PathAction *pathactionObj; flightDataModel *myModel; + uint completionCount; + uint completionSuccessCount; Waypoint *createWaypoint(int index, Waypoint *newWaypoint); void modelToWaypoint(int i, Waypoint::DataFields &data); @@ -56,6 +59,11 @@ private: PathAction *createPathAction(int index, PathAction *newAction); void modelToPathAction(int i, PathAction::DataFields &data); void pathActionToModel(int i, PathAction::DataFields &data); + +public slots: + void flightPlanSent(UAVObject *, bool success); + void flightPlanReceived(UAVObject *, bool success); + }; #endif // MODELUAVOPROXY_H diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp index 8f5f22d9d..c8b193a84 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp @@ -226,8 +226,10 @@ OPMapGadgetWidget::OPMapGadgetWidget(QWidget *parent) : QWidget(parent) table->setModel(model, selectionModel); waypoint_edit_dialog = new opmap_edit_waypoint_dialog(this, model, selectionModel); UAVProxy = new ModelUavoProxy(this, model); - connect(table, SIGNAL(sendPathPlanToUAV()), UAVProxy, SLOT(modelToObjects())); - connect(table, SIGNAL(receivePathPlanFromUAV()), UAVProxy, SLOT(objectsToModel())); + // sending and receiving is asynchronous + // TODO : buttons should be disabled while a send or receive is in progress + connect(table, SIGNAL(sendPathPlanToUAV()), UAVProxy, SLOT(sendFlightPlan())); + connect(table, SIGNAL(receivePathPlanFromUAV()), UAVProxy, SLOT(receiveFlightPlan())); #endif magicWayPoint = m_map->magicWPCreate(); magicWayPoint->setVisible(false); diff --git a/shared/uavobjectdefinition/pathaction.xml b/shared/uavobjectdefinition/pathaction.xml index 983863650..662a9fdd5 100644 --- a/shared/uavobjectdefinition/pathaction.xml +++ b/shared/uavobjectdefinition/pathaction.xml @@ -1,30 +1,24 @@ A waypoint command the pathplanner is to use at a certain waypoint - - - - - - - - + + + + + + + - + diff --git a/shared/uavobjectdefinition/waypoint.xml b/shared/uavobjectdefinition/waypoint.xml index 2a27f7fd5..912245163 100644 --- a/shared/uavobjectdefinition/waypoint.xml +++ b/shared/uavobjectdefinition/waypoint.xml @@ -1,12 +1,14 @@ A waypoint the aircraft can try and hit. Used by the @ref PathPlanner module + - - + + + - +