From 8c7792b6e5e5572269df70a6e8834d954969c04e Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Thu, 9 Jan 2014 00:09:51 +0100 Subject: [PATCH] OP-1122 OP-1120 Added a new FligthPlanInfo uavobject - FligthPlanInfo contains the number of waypoints and action pathes - FligthPlanInfo contains a CRC of all waypoints and action pathes - The CRC is not yet implemented - These informations will be used to check the consistency of a flight plan --- flight/modules/PathPlanner/pathplanner.c | 2 + .../boards/revolution/firmware/UAVObjects.inc | 1 + .../src/plugins/opmap/modeluavoproxy.cpp | 127 ++++++++++++------ .../src/plugins/opmap/modeluavoproxy.h | 12 +- .../src/plugins/uavobjects/uavobjects.pro | 2 + shared/uavobjectdefinition/flightplaninfo.xml | 14 ++ 6 files changed, 111 insertions(+), 47 deletions(-) create mode 100644 shared/uavobjectdefinition/flightplaninfo.xml diff --git a/flight/modules/PathPlanner/pathplanner.c b/flight/modules/PathPlanner/pathplanner.c index 837e78608..ab2716d99 100644 --- a/flight/modules/PathPlanner/pathplanner.c +++ b/flight/modules/PathPlanner/pathplanner.c @@ -31,6 +31,7 @@ #include "openpilot.h" +#include "flightplaninfo.h" #include "flightstatus.h" #include "airspeedstate.h" #include "pathaction.h" @@ -99,6 +100,7 @@ int32_t PathPlannerInitialize() { taskHandle = NULL; + FlightPlanInfoInitialize(); PathActionInitialize(); PathStatusInitialize(); PathDesiredInitialize(); diff --git a/flight/targets/boards/revolution/firmware/UAVObjects.inc b/flight/targets/boards/revolution/firmware/UAVObjects.inc index 7a63dc9f2..153525147 100644 --- a/flight/targets/boards/revolution/firmware/UAVObjects.inc +++ b/flight/targets/boards/revolution/firmware/UAVObjects.inc @@ -43,6 +43,7 @@ UAVOBJSRCFILENAMES += debuglogentry UAVOBJSRCFILENAMES += flightbatterysettings UAVOBJSRCFILENAMES += firmwareiapobj UAVOBJSRCFILENAMES += flightbatterystate +UAVOBJSRCFILENAMES += flightplaninfo UAVOBJSRCFILENAMES += flightplancontrol UAVOBJSRCFILENAMES += flightplansettings UAVOBJSRCFILENAMES += flightplanstatus diff --git a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp index 5af2a707f..6d19d2248 100644 --- a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.cpp @@ -26,6 +26,7 @@ */ #include "modeluavoproxy.h" #include "extensionsystem/pluginmanager.h" + #include ModelUavoProxy::ModelUavoProxy(QObject *parent, flightDataModel *model) : QObject(parent), myModel(model) @@ -33,28 +34,32 @@ ModelUavoProxy::ModelUavoProxy(QObject *parent, flightDataModel *model) : QObjec ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); Q_ASSERT(pm != NULL); - objManager = pm->getObject(); - Q_ASSERT(objManager != NULL); + objMngr = pm->getObject(); + Q_ASSERT(objMngr != NULL); } void ModelUavoProxy::sendFlightPlan() { modelToObjects(); - Waypoint *waypoint = Waypoint::GetInstance(objManager, 0); - connect(waypoint, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(flightPlanSent(UAVObject *, bool))); + FlightPlanInfo *flightPlan = FlightPlanInfo::GetInstance(objMngr, 0); + connect(flightPlan, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(flightPlanElementSent(UAVObject *, bool))); - PathAction *action = PathAction::GetInstance(objManager, 0); - connect(action, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(flightPlanSent(UAVObject *, bool))); + Waypoint *waypoint = Waypoint::GetInstance(objMngr, 0); + connect(waypoint, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(flightPlanElementSent(UAVObject *, bool))); + + PathAction *action = PathAction::GetInstance(objMngr, 0); + connect(action, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(flightPlanElementSent(UAVObject *, bool))); completionCount = 0; completionSuccessCount = 0; + flightPlan->updated(); waypoint->updatedAll(); action->updatedAll(); } -void ModelUavoProxy::flightPlanSent(UAVObject *obj, bool success) +void ModelUavoProxy::flightPlanElementSent(UAVObject *obj, bool success) { obj->disconnect(this); @@ -63,9 +68,9 @@ void ModelUavoProxy::flightPlanSent(UAVObject *obj, bool success) completionSuccessCount++; } - if (completionCount == 2) { - qDebug() << "ModelUavoProxy::flightPlanSent - success" << (completionSuccessCount == 2); - if (completionSuccessCount == 2) { + if (completionCount == 3) { + qDebug() << "ModelUavoProxy::flightPlanSent - completed" << (completionSuccessCount == 3); + if (completionSuccessCount == 3) { QMessageBox::information(NULL, tr("Flight Plan Upload Successful"), tr("Flight plan upload was successful.")); } else { @@ -76,20 +81,24 @@ void ModelUavoProxy::flightPlanSent(UAVObject *obj, bool success) void ModelUavoProxy::receiveFlightPlan() { - Waypoint *waypoint = Waypoint::GetInstance(objManager, 0); - connect(waypoint, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(flightPlanReceived(UAVObject *, bool))); + FlightPlanInfo *flightPlan = FlightPlanInfo::GetInstance(objMngr, 0); + connect(flightPlan, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(flightPlanElementReceived(UAVObject *, bool))); - PathAction *action = PathAction::GetInstance(objManager, 0); - connect(action, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(flightPlanReceived(UAVObject *, bool))); + Waypoint *waypoint = Waypoint::GetInstance(objMngr, 0); + connect(waypoint, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(flightPlanElementReceived(UAVObject *, bool))); + + PathAction *action = PathAction::GetInstance(objMngr, 0); + connect(action, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(flightPlanElementReceived(UAVObject *, bool))); completionCount = 0; completionSuccessCount = 0; + flightPlan->requestUpdate(); waypoint->requestUpdateAll(); action->requestUpdateAll(); } -void ModelUavoProxy::flightPlanReceived(UAVObject *obj, bool success) +void ModelUavoProxy::flightPlanElementReceived(UAVObject *obj, bool success) { obj->disconnect(this); @@ -98,9 +107,9 @@ void ModelUavoProxy::flightPlanReceived(UAVObject *obj, bool success) completionSuccessCount++; } - if (completionCount == 2) { - qDebug() << "ModelUavoProxy::flightPlanReceived - success" << (completionSuccessCount == 2); - if (completionSuccessCount == 2) { + if (completionCount == 3) { + qDebug() << "ModelUavoProxy::flightPlanReceived - completed" << (completionSuccessCount == 3); + if (completionSuccessCount == 3) { objectsToModel(); QMessageBox::information(NULL, tr("Flight Plan Download Successful"), tr("Flight plan download was successful.")); } @@ -110,23 +119,26 @@ void ModelUavoProxy::flightPlanReceived(UAVObject *obj, bool success) } } +// update waypoint and path actions UAV objects +// +// waypoints are unique and each waypoint has an entry in the UAV waypoint list +// +// a path action can be referenced by multiple waypoints +// waypoints reference path action by their index in the UAV path action list +// the compression of path actions happens here. +// (compression consists in keeping only one instance of similar path actions) +// +// the UAV waypoint list and path action list are probably not empty, so we try to reuse existing instances void ModelUavoProxy::modelToObjects() { qDebug() << "ModelUAVProxy::modelToObjects"; - // update waypoint and path actions UAV objects - // waypoints are unique and each waypoint has an entry in the UAV waypoint list - // a path action can be referenced by multiple waypoints - // waypoints reference path action by their index in the UAV path action list - // the factoring of path actions (to remove duplicates) happens here... - - // the UAV waypoint list and path action list are probably not empty - // so we try to reuse existing instances // track number of path actions int actionCount = 0; // iterate over waypoints - for (int i = 0; i < myModel->rowCount(); ++i) { + int waypointCount = myModel->rowCount(); + for (int i = 0; i < waypointCount; ++i) { // Path Actions @@ -174,15 +186,38 @@ void ModelUavoProxy::modelToObjects() // update UAVObject waypoint->setData(waypointData); } + // Put "safe" values in unused waypoint objects + if (waypointCount < objMngr->getNumInstances(Waypoint::OBJID)) { + for (int i = waypointCount; i < objMngr->getNumInstances(Waypoint::OBJID); ++i) { + // TODO + } + } + // Put "safe" values in unused path action objects + if (actionCount < objMngr->getNumInstances(PathAction::OBJID)) { + for (int i = actionCount; i < objMngr->getNumInstances(PathAction::OBJID); ++i) { + // TODO + } + } + + // Update FlightPlanInfo + FlightPlanInfo *flightPlan = FlightPlanInfo::GetInstance(objMngr); + FlightPlanInfo::DataFields flightPlanData = flightPlan->getData(); + + flightPlanData.WaypointCount = waypointCount; + flightPlanData.PathActionCount = actionCount; + // TODO + flightPlanData.Crc = 0; + + flightPlan->setData(flightPlanData); } Waypoint *ModelUavoProxy::createWaypoint(int index, Waypoint *newWaypoint) { Waypoint *waypoint = NULL; - int count = objManager->getNumInstances(Waypoint::OBJID); + int count = objMngr->getNumInstances(Waypoint::OBJID); if (index < count) { // reuse object qDebug() << "ModelUAVProxy::createWaypoint - reused waypoint instance :" << index << "/" << count; - waypoint = Waypoint::GetInstance(objManager, index); + waypoint = Waypoint::GetInstance(objMngr, index); if (newWaypoint) { newWaypoint->deleteLater(); } @@ -192,7 +227,7 @@ Waypoint *ModelUavoProxy::createWaypoint(int index, Waypoint *newWaypoint) { // TODO is there a limit to the number of wp? waypoint = newWaypoint ? newWaypoint : new Waypoint; waypoint->initialize(index, waypoint->getMetaObject()); - objManager->registerObject(waypoint); + objMngr->registerObject(waypoint); } else { // we can only create the "next" object @@ -203,11 +238,11 @@ Waypoint *ModelUavoProxy::createWaypoint(int index, Waypoint *newWaypoint) { PathAction *ModelUavoProxy::createPathAction(int index, PathAction *newAction) { PathAction *action = NULL; - int count = objManager->getNumInstances(PathAction::OBJID); + int count = objMngr->getNumInstances(PathAction::OBJID); if (index < count) { // reuse object qDebug() << "ModelUAVProxy::createPathAction - reused action instance :" << index << "/" << count; - action = PathAction::GetInstance(objManager, index); + action = PathAction::GetInstance(objMngr, index); if (newAction) { newAction->deleteLater(); } @@ -217,7 +252,7 @@ PathAction *ModelUavoProxy::createPathAction(int index, PathAction *newAction) { // TODO is there a limit to the number of path actions? action = newAction ? newAction : new PathAction; action->initialize(index, action->getMetaObject()); - objManager->registerObject(action); + objMngr->registerObject(action); } else { // we can only create the "next" object @@ -227,10 +262,10 @@ PathAction *ModelUavoProxy::createPathAction(int index, PathAction *newAction) { } PathAction *ModelUavoProxy::findPathAction(const PathAction::DataFields &actionData, int actionCount) { - int instancesCount = objManager->getNumInstances(PathAction::OBJID); + int instancesCount = objMngr->getNumInstances(PathAction::OBJID); int count = actionCount <= instancesCount ? actionCount : instancesCount; for (int i = 0; i < count; ++i) { - PathAction *action = PathAction::GetInstance(objManager, i); + PathAction *action = PathAction::GetInstance(objMngr, i); Q_ASSERT(action); if (!action) { continue; @@ -258,18 +293,24 @@ void ModelUavoProxy::objectsToModel() // the list of objects can end with "garbage" instances due to previous flightpath // they need to be ignored - int instanceCount = objManager->getNumInstances(Waypoint::OBJID); + FlightPlanInfo *flightPlan = FlightPlanInfo::GetInstance(objMngr); + FlightPlanInfo::DataFields flightPlanData = flightPlan->getData(); + + int waypointCount = flightPlanData.WaypointCount; + + // TODO consistency checks + // objMngr->getNumInstances(Waypoint::OBJID); int rowCount = myModel->rowCount(); - if (instanceCount < rowCount) { - myModel->removeRows(instanceCount, rowCount - instanceCount); + if (waypointCount < rowCount) { + myModel->removeRows(waypointCount, rowCount - waypointCount); } - else if (instanceCount > rowCount) { - myModel->insertRows(rowCount, instanceCount - rowCount); + else if (waypointCount > rowCount) { + myModel->insertRows(rowCount, waypointCount - rowCount); } - for (int i = 0; i < instanceCount; ++i) { - Waypoint *waypoint = Waypoint::GetInstance(objManager, i); + for (int i = 0; i < waypointCount; ++i) { + Waypoint *waypoint = Waypoint::GetInstance(objMngr, i); Q_ASSERT(waypoint); if (!waypoint) { continue; @@ -278,7 +319,7 @@ void ModelUavoProxy::objectsToModel() Waypoint::DataFields waypointData = waypoint->getData(); waypointToModel(i, waypointData); - PathAction *action = PathAction::GetInstance(objManager, waypoint->getAction()); + PathAction *action = PathAction::GetInstance(objMngr, waypoint->getAction()); Q_ASSERT(action); if (!action) { continue; diff --git a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.h b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.h index 9e3a2cd2e..5d1d5b942 100644 --- a/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.h +++ b/ground/openpilotgcs/src/plugins/opmap/modeluavoproxy.h @@ -27,11 +27,14 @@ #ifndef MODELUAVOPROXY_H #define MODELUAVOPROXY_H -#include #include "flightdatamodel.h" + +#include "flightplaninfo.h" #include "pathaction.h" #include "waypoint.h" +#include + class ModelUavoProxy : public QObject { Q_OBJECT @@ -43,8 +46,9 @@ public slots: void receiveFlightPlan(); private: - UAVObjectManager *objManager; + UAVObjectManager *objMngr; flightDataModel *myModel; + uint completionCount; uint completionSuccessCount; @@ -63,8 +67,8 @@ private: void pathActionToModel(int i, PathAction::DataFields &data); private slots: - void flightPlanSent(UAVObject *, bool success); - void flightPlanReceived(UAVObject *, bool success); + void flightPlanElementSent(UAVObject *, bool success); + void flightPlanElementReceived(UAVObject *, bool success); }; diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index f65bffd4d..04be9ca02 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -92,6 +92,7 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ $$UAVOBJECT_SYNTHETICS/i2cstats.h \ $$UAVOBJECT_SYNTHETICS/flightbatterysettings.h \ $$UAVOBJECT_SYNTHETICS/taskinfo.h \ + $$UAVOBJECT_SYNTHETICS/flightplaninfo.h \ $$UAVOBJECT_SYNTHETICS/flightplanstatus.h \ $$UAVOBJECT_SYNTHETICS/flightplansettings.h \ $$UAVOBJECT_SYNTHETICS/flightplancontrol.h \ @@ -184,6 +185,7 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ $$UAVOBJECT_SYNTHETICS/i2cstats.cpp \ $$UAVOBJECT_SYNTHETICS/flightbatterysettings.cpp \ $$UAVOBJECT_SYNTHETICS/taskinfo.cpp \ + $$UAVOBJECT_SYNTHETICS/flightplaninfo.cpp \ $$UAVOBJECT_SYNTHETICS/flightplanstatus.cpp \ $$UAVOBJECT_SYNTHETICS/flightplansettings.cpp \ $$UAVOBJECT_SYNTHETICS/flightplancontrol.cpp \ diff --git a/shared/uavobjectdefinition/flightplaninfo.xml b/shared/uavobjectdefinition/flightplaninfo.xml new file mode 100644 index 000000000..05727e50f --- /dev/null +++ b/shared/uavobjectdefinition/flightplaninfo.xml @@ -0,0 +1,14 @@ + + + Flight plan informations + + + + + + + + + + +