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