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

   1 /**
   2  ******************************************************************************
   3  *
   4  * @file       modeluavproxy.cpp
   5  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
   6  * @addtogroup GCSPlugins GCS Plugins
   7  * @{
   8  * @addtogroup OPMapPlugin OpenPilot Map Plugin
   9  * @{
  10  * @brief The OpenPilot Map plugin
  11  *****************************************************************************/
  12 /*
  13  * This program is free software; you can redistribute it and/or modify
  14  * it under the terms of the GNU General Public License as published by
  15  * the Free Software Foundation; either version 3 of the License, or
  16  * (at your option) any later version.
  17  *
  18  * This program is distributed in the hope that it will be useful, but
  19  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  20  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  21  * for more details.
  22  *
  23  * You should have received a copy of the GNU General Public License along
  24  * with this program; if not, write to the Free Software Foundation, Inc.,
  25  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  26  */
  27 #include "modeluavoproxy.h"
  28 #include "extensionsystem/pluginmanager.h"
  29 #include "uavobjecthelper.h"
  30 #include "uavobjectmanager.h"
  31
  32 #include <QProgressDialog>
  33 #include <math.h>
  34
  35 ModelUavoProxy::ModelUavoProxy(QObject *parent, flightDataModel *model) : QObject(parent), myModel(model)
  36 {
  37     ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
  38
  39     Q_ASSERT(pm != NULL);
  40
  41     objMngr = pm->getObject<UAVObjectManager>();
  42     Q_ASSERT(objMngr != NULL);
  43 }
  44
  45 void ModelUavoProxy::sendPathPlan()
  46 {
  47     modelToObjects();
  48
  49     PathPlan *pathPlan      = PathPlan::GetInstance(objMngr);
  50
  51     const int waypointCount = pathPlan->getWaypointCount();
  52     const int actionCount   = pathPlan->getPathActionCount();
  53
  54     QProgressDialog progress(tr("Sending the path plan to the board... "), "", 0, 1 + waypointCount + actionCount);
  55     progress.setWindowModality(Qt::WindowModal);
  56     progress.setCancelButton(NULL);
  57     progress.show();
  58
  59     UAVObjectUpdaterHelper updateHelper;
  60
  61     // send PathPlan
  62     bool success = (updateHelper.doObjectAndWait(pathPlan) == UAVObjectUpdaterHelper::SUCCESS);
  63     progress.setValue(1);
  64
  65     if (success) {
  66         // send Waypoint instances
  67         qDebug() << "sending" << waypointCount << "waypoints";
  68         for (int i = 0; i < waypointCount; ++i) {
  69             Waypoint *waypoint = Waypoint::GetInstance(objMngr, i);
  70             success = (updateHelper.doObjectAndWait(waypoint) == UAVObjectUpdaterHelper::SUCCESS);
  71             if (!success) {
  72                 break;
  73             }
  74             progress.setValue(progress.value() + 1);
  75         }
  76     }
  77
  78     if (success) {
  79         // send PathAction instances
  80         qDebug() << "sending" << actionCount << "path actions";
  81         for (int i = 0; i < actionCount; ++i) {
  82             PathAction *action = PathAction::GetInstance(objMngr, i);
  83             success = (updateHelper.doObjectAndWait(action) == UAVObjectUpdaterHelper::SUCCESS);
  84             if (!success) {
  85                 break;
  86             }
  87             progress.setValue(progress.value() + 1);
  88         }
  89     }
  90
  91     qDebug() << "ModelUavoProxy::pathPlanSent - completed" << success;
  92     if (!success) {
  93         QMessageBox::critical(NULL, tr("Sending Path Plan Failed!"), tr("Failed to send the path plan to the board."));
  94     }
  95
  96     progress.close();
  97 }
  98
  99 void ModelUavoProxy::receivePathPlan()
 100 {
 101     QProgressDialog progress(tr("Receiving the path plan from the board... "), "", 0, 0);
 102
 103     progress.setWindowModality(Qt::WindowModal);
 104     progress.setCancelButton(NULL);
 105     progress.show();
 106
 107     UAVObjectRequestHelper requestHelper;
 108
 109     PathPlan *pathPlan = PathPlan::GetInstance(objMngr);
 110     bool success = (requestHelper.doObjectAndWait(pathPlan) == UAVObjectUpdaterHelper::SUCCESS);
 111
 112     const int waypointCount = pathPlan->getWaypointCount();
 113     const int actionCount   = pathPlan->getPathActionCount();
 114
 115     progress.setMaximum(1 + waypointCount + actionCount);
 116     progress.setValue(1);
 117
 118     if (success && (waypointCount > objMngr->getNumInstances(Waypoint::OBJID))) {
 119         // allocate needed Waypoint instances
 120         Waypoint *waypoint = new Waypoint;
 121         waypoint->initialize(waypointCount - 1, waypoint->getMetaObject());
 122         success = objMngr->registerObject(waypoint);
 123     }
 124     if (success) {
 125         // request Waypoint instances
 126         qDebug() << "requesting" << waypointCount << "waypoints";
 127         for (int i = 0; i < waypointCount; ++i) {
 128             Waypoint *waypoint = Waypoint::GetInstance(objMngr, i);
 129             success = (requestHelper.doObjectAndWait(waypoint) == UAVObjectRequestHelper::SUCCESS);
 130             if (!success) {
 131                 break;
 132             }
 133             progress.setValue(progress.value() + 1);
 134         }
 135     }
 136
 137     if (success && (actionCount > objMngr->getNumInstances(PathAction::OBJID))) {
 138         // allocate needed PathAction instances
 139         PathAction *action = new PathAction;
 140         action->initialize(actionCount - 1, action->getMetaObject());
 141         success = objMngr->registerObject(action);
 142     }
 143     if (success) {
 144         // request PathAction isntances
 145         qDebug() << "requesting" << actionCount << "path actions";
 146         for (int i = 0; i < actionCount; ++i) {
 147             PathAction *action = PathAction::GetInstance(objMngr, i);
 148             success = (requestHelper.doObjectAndWait(action) == UAVObjectRequestHelper::SUCCESS);
 149             if (!success) {
 150                 break;
 151             }
 152             progress.setValue(progress.value() + 1);
 153         }
 154     }
 155
 156     qDebug() << "ModelUavoProxy::pathPlanReceived - completed" << success;
 157     if (success) {
 158         objectsToModel();
 159     } else {
 160         QMessageBox::critical(NULL, tr("Receiving Path Plan Failed!"), tr("Failed to receive the path plan from the board."));
 161     }
 162
 163     progress.close();
 164 }
 165
 166 // update waypoint and path actions UAV objects
 167 //
 168 // waypoints are unique and each waypoint has an entry in the UAV waypoint list
 169 //
 170 // a path action can be referenced by multiple waypoints
 171 // waypoints reference path action by their index in the UAV path action list
 172 // the compression of path actions happens here.
 173 // (compression consists in keeping only one instance of similar path actions)
 174 //
 175 // the UAV waypoint list and path action list are probably not empty, so we try to reuse existing instances
 176 bool ModelUavoProxy::modelToObjects()
 177 {
 178     qDebug() << "ModelUAVProxy::modelToObjects";
 179
 180     // track number of path actions
 181     int actionCount   = 0;
 182
 183     // iterate over waypoints
 184     int waypointCount = myModel->rowCount();
 185     for (int i = 0; i < waypointCount; ++i) {
 186         // Path Actions
 187
 188         // create action to use as a search criteria
 189         // this object does not need to be deleted as it will either be added to the managed list or deleted later
 190         PathAction *action = new PathAction;
 191
 192         // get model data
 193         PathAction::DataFields actionData = action->getData();
 194         modelToPathAction(i, actionData);
 195
 196         // see if that path action has already been added in this run
 197         PathAction *foundAction = findPathAction(actionData, actionCount);
 198         // TODO this test needs a consistency check as it is unsafe.
 199         // if the find method is buggy and returns false positives then the flight plan sent to the uav is broken!
 200         // the find method should do a "binary" compare instead of a field by field compare
 201         // if a field is added everywhere and not in the compare method then you can start having false positives
 202         if (!foundAction) {
 203             // create or reuse an action instance
 204             action = createPathAction(actionCount, action);
 205             actionCount++;
 206
 207             // update UAVObject
 208             action->setData(actionData);
 209         } else {
 210             action->deleteLater();
 211             action = foundAction;
 212             qDebug() << "ModelUAVProxy::modelToObjects - found action instance :" << action->getInstID();
 213         }
 214
 215         // Waypoints
 216
 217         // create or reuse a waypoint instance
 218         Waypoint *waypoint = createWaypoint(i, NULL);
 219         Q_ASSERT(waypoint);
 220
 221         // get model data
 222         Waypoint::DataFields waypointData = waypoint->getData();
 223         modelToWaypoint(i, waypointData);
 224
 225         // connect waypoint to path action
 226         waypointData.Action = action->getInstID();
 227
 228         // update UAVObject
 229         waypoint->setData(waypointData);
 230     }
 231
 232     // Put "safe" values in unused waypoint and path action objects
 233     if (waypointCount < objMngr->getNumInstances(Waypoint::OBJID)) {
 234         for (int i = waypointCount; i < objMngr->getNumInstances(Waypoint::OBJID); ++i) {
 235             // TODO
 236         }
 237     }
 238     if (actionCount < objMngr->getNumInstances(PathAction::OBJID)) {
 239         for (int i = actionCount; i < objMngr->getNumInstances(PathAction::OBJID); ++i) {
 240             // TODO
 241         }
 242     }
 243
 244     // Update PathPlan
 245     PathPlan *pathPlan = PathPlan::GetInstance(objMngr);
 246     PathPlan::DataFields pathPlanData = pathPlan->getData();
 247
 248     pathPlanData.WaypointCount   = waypointCount;
 249     pathPlanData.PathActionCount = actionCount;
 250     pathPlanData.Crc = computePathPlanCrc(waypointCount, actionCount);
 251
 252     pathPlan->setData(pathPlanData);
 253
 254     return true;
 255 }
 256
 257 Waypoint *ModelUavoProxy::createWaypoint(int index, Waypoint *newWaypoint)
 258 {
 259     Waypoint *waypoint = NULL;
 260     int count = objMngr->getNumInstances(Waypoint::OBJID);
 261
 262     if (index < count) {
 263         // reuse object
 264         qDebug() << "ModelUAVProxy::createWaypoint - reused waypoint instance :" << index << "/" << count;
 265         waypoint = Waypoint::GetInstance(objMngr, index);
 266         if (newWaypoint) {
 267             newWaypoint->deleteLater();
 268         }
 269     } else if (index < count + 1) {
 270         // create "next" object
 271         qDebug() << "ModelUAVProxy::createWaypoint - created waypoint instance :" << index;
 272         // TODO is there a limit to the number of wp?
 273         waypoint = newWaypoint ? newWaypoint : new Waypoint;
 274         waypoint->initialize(index, waypoint->getMetaObject());
 275         objMngr->registerObject(waypoint);
 276     } else {
 277         // we can only create the "next" object
 278         // TODO fail in a clean way :(
 279     }
 280     return waypoint;
 281 }
 282
 283 PathAction *ModelUavoProxy::createPathAction(int index, PathAction *newAction)
 284 {
 285     PathAction *action = NULL;
 286     int count = objMngr->getNumInstances(PathAction::OBJID);
 287
 288     if (index < count) {
 289         // reuse object
 290         qDebug() << "ModelUAVProxy::createPathAction - reused action instance :" << index << "/" << count;
 291         action = PathAction::GetInstance(objMngr, index);
 292         if (newAction) {
 293             newAction->deleteLater();
 294         }
 295     } else if (index < count + 1) {
 296         // create "next" object
 297         qDebug() << "ModelUAVProxy::createPathAction - created action instance :" << index;
 298         // TODO is there a limit to the number of path actions?
 299         action = newAction ? newAction : new PathAction;
 300         action->initialize(index, action->getMetaObject());
 301         objMngr->registerObject(action);
 302     } else {
 303         // we can only create the "next" object
 304         // TODO fail in a clean way :(
 305     }
 306     return action;
 307 }
 308
 309 PathAction *ModelUavoProxy::findPathAction(const PathAction::DataFields &actionData, int actionCount)
 310 {
 311     int instancesCount = objMngr->getNumInstances(PathAction::OBJID);
 312     int count = actionCount <= instancesCount ? actionCount : instancesCount;
 313
 314     for (int i = 0; i < count; ++i) {
 315         PathAction *action = PathAction::GetInstance(objMngr, i);
 316         Q_ASSERT(action);
 317         if (!action) {
 318             continue;
 319         }
 320         PathAction::DataFields fields = action->getData();
 321         if (fields.Command == actionData.Command
 322             && fields.ConditionParameters[0] == actionData.ConditionParameters[0]
 323             && fields.ConditionParameters[1] == actionData.ConditionParameters[1]
 324             && fields.ConditionParameters[2] == actionData.ConditionParameters[2]
 325             && fields.EndCondition == actionData.EndCondition
 326             && fields.ErrorDestination == actionData.ErrorDestination
 327             && fields.JumpDestination == actionData.JumpDestination && fields.Mode == actionData.Mode
 328             && fields.ModeParameters[0] == actionData.ModeParameters[0]
 329             && fields.ModeParameters[1] == actionData.ModeParameters[1]
 330             && fields.ModeParameters[2] == actionData.ModeParameters[2]) {
 331             return action;
 332         }
 333     }
 334     return NULL;
 335 }
 336
 337 bool ModelUavoProxy::objectsToModel()
 338 {
 339     // build model from uav objects
 340     // the list of objects can end with "garbage" instances due to previous flightpath
 341     // they need to be ignored
 342
 343     PathPlan *pathPlan = PathPlan::GetInstance(objMngr);
 344     PathPlan::DataFields pathPlanData = pathPlan->getData();
 345
 346     int waypointCount  = pathPlanData.WaypointCount;
 347     int actionCount    = pathPlanData.PathActionCount;
 348
 349     // consistency check
 350     if (waypointCount > objMngr->getNumInstances(Waypoint::OBJID)) {
 351         QMessageBox::critical(NULL, tr("Path Plan Download Failed"), tr("Path plan way point count error !"));
 352         return false;
 353     }
 354     if (actionCount > objMngr->getNumInstances(PathAction::OBJID)) {
 355         QMessageBox::critical(NULL, tr("Path Plan Download Failed"), tr("Path plan path action count error !"));
 356         return false;
 357     }
 358     if (pathPlanData.Crc != computePathPlanCrc(waypointCount, actionCount)) {
 359         QMessageBox::critical(NULL, tr("Path Plan Upload Failed"), tr("Path plan CRC error !"));
 360         return false;
 361     }
 362
 363     int rowCount = myModel->rowCount();
 364     if (waypointCount < rowCount) {
 365         myModel->removeRows(waypointCount, rowCount - waypointCount);
 366     } else if (waypointCount > rowCount) {
 367         myModel->insertRows(rowCount, waypointCount - rowCount);
 368     }
 369
 370     for (int i = 0; i < waypointCount; ++i) {
 371         Waypoint *waypoint = Waypoint::GetInstance(objMngr, i);
 372         Q_ASSERT(waypoint);
 373         if (!waypoint) {
 374             continue;
 375         }
 376
 377         Waypoint::DataFields waypointData = waypoint->getData();
 378         waypointToModel(i, waypointData);
 379
 380         PathAction *action = PathAction::GetInstance(objMngr, waypoint->getAction());
 381         Q_ASSERT(action);
 382         if (!action) {
 383             continue;
 384         }
 385
 386         PathAction::DataFields actionData = action->getData();
 387         pathActionToModel(i, actionData);
 388     }
 389     return true;
 390 }
 391
 392 void ModelUavoProxy::modelToWaypoint(int i, Waypoint::DataFields &data)
 393 {
 394     double distance, bearing, altitude, velocity;
 395
 396     QModelIndex index = myModel->index(i, flightDataModel::DISRELATIVE);
 397
 398     distance = myModel->data(index).toDouble();
 399     index    = myModel->index(i, flightDataModel::BEARELATIVE);
 400     bearing  = myModel->data(index).toDouble();
 401     index    = myModel->index(i, flightDataModel::ALTITUDERELATIVE);
 402     altitude = myModel->data(index).toFloat();
 403     index    = myModel->index(i, flightDataModel::VELOCITY);
 404     velocity = myModel->data(index).toFloat();
 405
 406     data.Position[Waypoint::POSITION_NORTH] = distance * cos(bearing / 180 * M_PI);
 407     data.Position[Waypoint::POSITION_EAST]  = distance * sin(bearing / 180 * M_PI);
 408     data.Position[Waypoint::POSITION_DOWN]  = -altitude;
 409     data.Velocity = velocity;
 410 }
 411
 412 void ModelUavoProxy::waypointToModel(int i, Waypoint::DataFields &data)
 413 {
 414     double distance = sqrt(data.Position[Waypoint::POSITION_NORTH] * data.Position[Waypoint::POSITION_NORTH] +
 415                            data.Position[Waypoint::POSITION_EAST] * data.Position[Waypoint::POSITION_EAST]);
 416
 417     double bearing  = atan2(data.Position[Waypoint::POSITION_EAST], data.Position[Waypoint::POSITION_NORTH]) * 180 / M_PI;
 418
 419     if (bearing != bearing) {
 420         bearing = 0;
 421     }
 422
 423     double altitude   = -data.Position[Waypoint::POSITION_DOWN];
 424
 425     QModelIndex index = myModel->index(i, flightDataModel::VELOCITY);
 426     myModel->setData(index, data.Velocity);
 427     index = myModel->index(i, flightDataModel::DISRELATIVE);
 428     myModel->setData(index, distance);
 429     index = myModel->index(i, flightDataModel::BEARELATIVE);
 430     myModel->setData(index, bearing);
 431     index = myModel->index(i, flightDataModel::ALTITUDERELATIVE);
 432     myModel->setData(index, altitude);
 433 }
 434
 435 void ModelUavoProxy::modelToPathAction(int i, PathAction::DataFields &data)
 436 {
 437     QModelIndex index = myModel->index(i, flightDataModel::MODE);
 438
 439     data.Mode    = myModel->data(index).toInt();
 440     index        = myModel->index(i, flightDataModel::MODE_PARAMS0);
 441     data.ModeParameters[0] = myModel->data(index).toFloat();
 442     index        = myModel->index(i, flightDataModel::MODE_PARAMS1);
 443     data.ModeParameters[1] = myModel->data(index).toFloat();
 444     index        = myModel->index(i, flightDataModel::MODE_PARAMS2);
 445     data.ModeParameters[2] = myModel->data(index).toFloat();
 446     index        = myModel->index(i, flightDataModel::MODE_PARAMS3);
 447     data.ModeParameters[3] = myModel->data(index).toFloat();
 448     index        = myModel->index(i, flightDataModel::CONDITION);
 449     data.EndCondition = myModel->data(index).toInt();
 450     index        = myModel->index(i, flightDataModel::CONDITION_PARAMS0);
 451     data.ConditionParameters[0] = myModel->data(index).toFloat();
 452     index        = myModel->index(i, flightDataModel::CONDITION_PARAMS1);
 453     data.ConditionParameters[1] = myModel->data(index).toFloat();
 454     index        = myModel->index(i, flightDataModel::CONDITION_PARAMS2);
 455     data.ConditionParameters[2] = myModel->data(index).toFloat();
 456     index        = myModel->index(i, flightDataModel::CONDITION_PARAMS3);
 457     data.ConditionParameters[3] = myModel->data(index).toFloat();
 458     index        = myModel->index(i, flightDataModel::COMMAND);
 459     data.Command = myModel->data(index).toInt();
 460     index        = myModel->index(i, flightDataModel::JUMPDESTINATION);
 461     data.JumpDestination = myModel->data(index).toInt() - 1;
 462     index        = myModel->index(i, flightDataModel::ERRORDESTINATION);
 463     data.ErrorDestination = myModel->data(index).toInt() - 1;
 464 }
 465
 466 void ModelUavoProxy::pathActionToModel(int i, PathAction::DataFields &data)
 467 {
 468     QModelIndex index = myModel->index(i, flightDataModel::ISRELATIVE);
 469
 470     myModel->setData(index, true);
 471
 472     index = myModel->index(i, flightDataModel::COMMAND);
 473     myModel->setData(index, data.Command);
 474
 475     index = myModel->index(i, flightDataModel::CONDITION_PARAMS0);
 476     myModel->setData(index, data.ConditionParameters[0]);
 477     index = myModel->index(i, flightDataModel::CONDITION_PARAMS1);
 478     myModel->setData(index, data.ConditionParameters[1]);
 479     index = myModel->index(i, flightDataModel::CONDITION_PARAMS2);
 480     myModel->setData(index, data.ConditionParameters[2]);
 481     index = myModel->index(i, flightDataModel::CONDITION_PARAMS3);
 482     myModel->setData(index, data.ConditionParameters[3]);
 483
 484     index = myModel->index(i, flightDataModel::CONDITION);
 485     myModel->setData(index, data.EndCondition);
 486
 487     index = myModel->index(i, flightDataModel::ERRORDESTINATION);
 488     myModel->setData(index, data.ErrorDestination + 1);
 489
 490     index = myModel->index(i, flightDataModel::JUMPDESTINATION);
 491     myModel->setData(index, data.JumpDestination + 1);
 492
 493     index = myModel->index(i, flightDataModel::MODE);
 494     myModel->setData(index, data.Mode);
 495
 496     index = myModel->index(i, flightDataModel::MODE_PARAMS0);
 497     myModel->setData(index, data.ModeParameters[0]);
 498     index = myModel->index(i, flightDataModel::MODE_PARAMS1);
 499     myModel->setData(index, data.ModeParameters[1]);
 500     index = myModel->index(i, flightDataModel::MODE_PARAMS2);
 501     myModel->setData(index, data.ModeParameters[2]);
 502     index = myModel->index(i, flightDataModel::MODE_PARAMS3);
 503     myModel->setData(index, data.ModeParameters[3]);
 504 }
 505
 506 quint8 ModelUavoProxy::computePathPlanCrc(int waypointCount, int actionCount)
 507 {
 508     quint8 crc = 0;
 509
 510     for (int i = 0; i < waypointCount; ++i) {
 511         Waypoint *waypoint = Waypoint::GetInstance(objMngr, i);
 512         crc = waypoint->updateCRC(crc);
 513     }
 514     for (int i = 0; i < actionCount; ++i) {
 515         PathAction *action = PathAction::GetInstance(objMngr, i);
 516         crc = action->updateCRC(crc);
 517     }
 518     return crc;
 519 }