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

   1 /**
   2  ******************************************************************************
   3  *
   4  * @file       pathcompiler.h
   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 <pathcompiler.h>
  28 #include <extensionsystem/pluginmanager.h>
  29 #include <utils/coordinateconversions.h>
  30 #include <uavobjectmanager.h>
  31 #include <waypoint.h>
  32 #include <homelocation.h>
  33
  34 #include <QDebug>
  35
  36 PathCompiler::PathCompiler(QObject *parent) :
  37     QObject(parent)
  38 {
  39     HomeLocation *homeLocation = NULL;
  40
  41     /* To catch new waypoint UAVOs */
  42     connect(getObjectManager(), SIGNAL(newInstance(UAVObject *)), this, SLOT(doNewInstance(UAVObject *)));
  43
  44     /* Connect the object updates */
  45     int numWaypoints = getObjectManager()->getNumInstances(Waypoint::OBJID);
  46     for (int i = 0; i < numWaypoints; i++) {
  47         Waypoint *waypoint = Waypoint::GetInstance(getObjectManager(), i);
  48         Q_ASSERT(waypoint);
  49         if (waypoint) {
  50             connect(waypoint, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doUpdateFromUAV(UAVObject *)));
  51         }
  52     }
  53
  54     homeLocation = HomeLocation::GetInstance(getObjectManager());
  55     Q_ASSERT(homeLocation);
  56     if (homeLocation) {
  57         connect(homeLocation, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doUpdateFromUAV(UAVObject *)));
  58     }
  59 }
  60
  61 /**
  62  * Helper method to get the uavobjectamanger
  63  */
  64 UAVObjectManager *PathCompiler::getObjectManager()
  65 {
  66     ExtensionSystem::PluginManager *pm = NULL;
  67     UAVObjectManager *objMngr = NULL;
  68
  69     pm = ExtensionSystem::PluginManager::instance();
  70     Q_ASSERT(pm);
  71     if (pm) {
  72         objMngr = pm->getObject<UAVObjectManager>();
  73     }
  74     Q_ASSERT(objMngr);
  75
  76     return objMngr;
  77 }
  78
  79 /**
  80  * This method opens a dialog (if filename is null) and saves the path
  81  * @param filename The file to save the path to
  82  * @returns -1 for failure, 0 for success
  83  */
  84 int PathCompiler::savePath(QString filename)
  85 {
  86     Q_UNUSED(filename);
  87     return -1;
  88 }
  89
  90 /**
  91  * This method opens a dialog (if filename is null) and loads the path
  92  * @param filename The file to load from
  93  * @returns -1 for failure, 0 for success
  94  */
  95 int PathCompiler::loadPath(QString filename)
  96 {
  97     Q_UNUSED(filename);
  98     return -1;
  99 }
 100
 101 /**
 102  * Called whenever a new object instance is created so we can check
 103  * if it's a waypoint and if so connect to it
 104  * @param [in] obj The point to the object being created
 105  */
 106 void PathCompiler::doNewInstance(UAVObject *obj)
 107 {
 108     Q_ASSERT(obj);
 109     if (!obj) {
 110         return;
 111     }
 112
 113     if (obj->getObjID() == Waypoint::OBJID) {
 114         connect(obj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doUpdateFromUAV(UAVObject *)));
 115     }
 116 }
 117
 118 /**
 119  * add a waypoint
 120  * @param waypoint the new waypoint to add
 121  * @param position which position to insert it to, defaults to end
 122  */
 123 void PathCompiler::doAddWaypoint(waypoint newWaypointInternal, int /*position*/)
 124 {
 125     /* TODO: If a waypoint is inserted not at the end shift them all by one and */
 126     /* add the data there */
 127
 128     UAVObjectManager *objManager     = getObjectManager();
 129
 130     // Format the data from the map into a UAVO
 131     Waypoint::DataFields newWaypoint = InternalToUavo(newWaypointInternal);
 132
 133     // Search for any waypoints set to stop, because if they exist we
 134     // should add the waypoint immediately after that one
 135     int numWaypoints = objManager->getNumInstances(Waypoint::OBJID);
 136     int i;
 137
 138     for (i = 0; i < numWaypoints; i++) {
 139         Waypoint *waypoint = Waypoint::GetInstance(objManager, i);
 140         Q_ASSERT(waypoint);
 141         if (waypoint == NULL) {
 142             return;
 143         }
 144
 145         Waypoint::DataFields waypointData = waypoint->getData();
 146         if (waypointData.Action == Waypoint::ACTION_STOP) {
 147             waypointData.Action = Waypoint::ACTION_PATHTONEXT;
 148             waypoint->setData(waypointData);
 149             waypoint->updated();
 150             break;
 151         }
 152     }
 153
 154     if (i >= numWaypoints - 1) {
 155         // We reached end of list so new waypoint needs to be registered
 156
 157         Waypoint *waypoint = new Waypoint();
 158         Q_ASSERT(waypoint);
 159         if (waypoint) {
 160             // Register a new waypoint instance
 161             quint32 newInstId = objManager->getNumInstances(waypoint->getObjID());
 162             waypoint->initialize(newInstId, waypoint->getMetaObject());
 163             objManager->registerObject(waypoint);
 164
 165             // Set the data in the new object
 166             waypoint->setData(newWaypoint);
 167             waypoint->updated();
 168         }
 169     } else {
 170         Waypoint *waypoint = Waypoint::GetInstance(objManager, i + 1);
 171         Q_ASSERT(waypoint);
 172         if (waypoint) {
 173             waypoint->setData(newWaypoint);
 174             waypoint->updated();
 175         }
 176     }
 177 }
 178
 179 /**
 180  * Update waypoint
 181  */
 182 void PathCompiler::doUpdateWaypoints(PathCompiler::waypoint changedWaypoint, int position)
 183 {
 184     int numWaypoints = getObjectManager()->getNumInstances(Waypoint::OBJID);
 185
 186     Q_ASSERT(position < numWaypoints);
 187     if (position >= numWaypoints) {
 188         return;
 189     }
 190
 191     Waypoint *waypointInst = Waypoint::GetInstance(getObjectManager(), position);
 192     Q_ASSERT(waypointInst);
 193
 194     // Mirror over the updated position.  We don't just use the changedWaypoint
 195     // because things like action might need to be preserved
 196     Waypoint::DataFields changedWaypointUAVO = InternalToUavo(changedWaypoint);
 197     Waypoint::DataFields oldWaypointUAVO     = waypointInst->getData();
 198     oldWaypointUAVO.Position[0] = changedWaypointUAVO.Position[0];
 199     oldWaypointUAVO.Position[1] = changedWaypointUAVO.Position[1];
 200     // Don't take the altitude from the map for now
 201
 202     waypointInst->setData(oldWaypointUAVO);
 203     waypointInst->updated();
 204 }
 205
 206 /**
 207  * Delete a waypoint
 208  * @param index which waypoint to delete
 209  */
 210 void PathCompiler::doDelWaypoint(int index)
 211 {
 212     // This method is awkward because there is no support
 213     // on the FC for actually deleting a waypoint.  We need
 214     // to shift them all by one and set the new "last" waypoint
 215     // to a stop action
 216
 217     UAVObjectManager *objManager = getObjectManager();
 218     Waypoint *waypoint = Waypoint::GetInstance(objManager);
 219
 220     Q_ASSERT(waypoint);
 221     if (!waypoint) {
 222         return;
 223     }
 224
 225     int numWaypoints = objManager->getNumInstances(waypoint->getObjID());
 226     for (int i = index; i < numWaypoints - 1; i++) {
 227         Waypoint *waypointDest = Waypoint::GetInstance(objManager, i);
 228         Q_ASSERT(waypointDest);
 229
 230         Waypoint *waypointSrc  = Waypoint::GetInstance(objManager, i + 1);
 231         Q_ASSERT(waypointSrc);
 232
 233         if (!waypointDest || !waypointSrc) {
 234             return;
 235         }
 236
 237         // Copy the data down an index
 238         Waypoint::DataFields waypoint = waypointSrc->getData();
 239         waypointDest->setData(waypoint);
 240         waypointDest->updated();
 241     }
 242
 243     // Set the second to last waypoint to stop (and last for safety)
 244     // the functional equivalent to deleting
 245     for (int i = numWaypoints - 2; i < numWaypoints; i++) {
 246         waypoint = Waypoint::GetInstance(objManager, i);
 247         Q_ASSERT(waypoint);
 248         if (waypoint) {
 249             Waypoint::DataFields waypointData = waypoint->getData();
 250             waypointData.Action = Waypoint::ACTION_STOP;
 251             waypoint->setData(waypointData);
 252             waypoint->updated();
 253         }
 254     }
 255 }
 256
 257 /**
 258  * Delete all the waypoints
 259  */
 260 void PathCompiler::doDelAllWaypoints()
 261 {
 262     Waypoint *waypointObj = Waypoint::GetInstance(getObjectManager(), 0);
 263
 264     Q_ASSERT(waypointObj);
 265     if (waypointObj == NULL) {
 266         return;
 267     }
 268
 269     int numWaypoints = getObjectManager()->getNumInstances(waypointObj->getObjID());
 270     for (int i = 0; i < numWaypoints; i++) {
 271         Waypoint *waypoint = Waypoint::GetInstance(getObjectManager(), i);
 272         Q_ASSERT(waypoint);
 273         if (waypoint) {
 274             Waypoint::DataFields waypointData = waypoint->getData();
 275             waypointData.Action = Waypoint::ACTION_STOP;
 276             waypoint->setData(waypointData);
 277         }
 278     }
 279
 280     waypointObj->updated();
 281 }
 282
 283 /**
 284  * When the UAV waypoints change trigger the pathcompiler to
 285  * get the latest version and then update the visualization
 286  */
 287 void PathCompiler::doUpdateFromUAV(UAVObject *obj)
 288 {
 289     UAVObjectManager *objManager = getObjectManager();
 290
 291     if (!objManager) {
 292         return;
 293     }
 294
 295     Waypoint *waypointObj = Waypoint::GetInstance(getObjectManager());
 296     Q_ASSERT(waypointObj);
 297     if (waypointObj == NULL) {
 298         return;
 299     }
 300
 301     /* Get all the waypoints from the UAVO and create a representation for the visualization */
 302     QList <waypoint> waypoints;
 303     waypoints.clear();
 304     int numWaypoints = objManager->getNumInstances(waypointObj->getObjID());
 305     bool stopped     = false;
 306     for (int i = 0; i < numWaypoints && !stopped; i++) {
 307         Waypoint *waypoint = Waypoint::GetInstance(objManager, i);
 308         Q_ASSERT(waypoint);
 309         if (waypoint == NULL) {
 310             return;
 311         }
 312
 313         Waypoint::DataFields waypointData = waypoint->getData();
 314         waypoints.append(UavoToInternal(waypointData));
 315         stopped = waypointData.Action == Waypoint::ACTION_STOP;
 316     }
 317
 318     if (previousWaypoints != waypoints) {
 319         /* Because the waypoints have to update periodically (or we miss new ones on the FC */
 320         /* side - needs telem fix) we want to filter updates to map that are simply periodic */
 321         previousWaypoints = waypoints;
 322
 323         /* Inform visualization */
 324         emit visualizationChanged(waypoints);
 325     }
 326 }
 327
 328 /**
 329  * Convert a UAVO waypoint to the local structure
 330  * @param uavo The UAVO data representation
 331  * @return The waypoint structure for visualization
 332  */
 333 struct PathCompiler::waypoint PathCompiler::UavoToInternal(Waypoint::DataFields uavo)
 334 {
 335     double homeLLA[3];
 336     double LLA[3];
 337     double NED[3];
 338     waypoint internalWaypoint;
 339
 340     HomeLocation *homeLocation = HomeLocation::GetInstance(getObjectManager());
 341
 342     Q_ASSERT(homeLocation);
 343     if (homeLocation == NULL) {
 344         return internalWaypoint;
 345     }
 346     HomeLocation::DataFields homeLocationData = homeLocation->getData();
 347     homeLLA[0] = homeLocationData.Latitude / 10e6;
 348     homeLLA[1] = homeLocationData.Longitude / 10e6;
 349     homeLLA[2] = homeLocationData.Altitude;
 350
 351     NED[0]     = uavo.Position[Waypoint::POSITION_NORTH];
 352     NED[1]     = uavo.Position[Waypoint::POSITION_EAST];
 353     NED[2]     = uavo.Position[Waypoint::POSITION_DOWN];
 354     Utils::CoordinateConversions().GetLLA(homeLLA, NED, LLA);
 355
 356     internalWaypoint.latitude  = LLA[0];
 357     internalWaypoint.longitude = LLA[1];
 358     internalWaypoint.altitude  = LLA[2];
 359     return internalWaypoint;
 360 }
 361
 362 /**
 363  * Convert a UAVO waypoint to the local structure
 364  * @param internal The internal structure type
 365  * @returns The waypoint UAVO data structure
 366  */
 367 Waypoint::DataFields PathCompiler::InternalToUavo(waypoint internal)
 368 {
 369     Waypoint::DataFields uavo;
 370
 371     double homeLLA[3];
 372     double LLA[3];
 373     double NED[3];
 374
 375     HomeLocation *homeLocation = HomeLocation::GetInstance(getObjectManager());
 376
 377     Q_ASSERT(homeLocation);
 378     if (homeLocation == NULL) {
 379         return uavo;
 380     }
 381     HomeLocation::DataFields homeLocationData = homeLocation->getData();
 382     homeLLA[0] = homeLocationData.Latitude / 10e6;
 383     homeLLA[1] = homeLocationData.Longitude / 10e6;
 384     homeLLA[2] = homeLocationData.Altitude;
 385
 386     // TODO: Give the point a concept of altitude
 387     LLA[0]     = internal.latitude;
 388     LLA[1]     = internal.longitude;
 389     LLA[2]     = internal.altitude;
 390
 391     Utils::CoordinateConversions().GetNED(homeLLA, LLA, NED);
 392
 393     uavo.Position[Waypoint::POSITION_NORTH] = NED[0];
 394     uavo.Position[Waypoint::POSITION_EAST]  = NED[1];
 395     uavo.Position[Waypoint::POSITION_DOWN]  = NED[2];
 396
 397     uavo.Action = Waypoint::ACTION_PATHTONEXT;
 398
 399     uavo.Velocity[Waypoint::VELOCITY_NORTH] = 5;
 400     uavo.Velocity[Waypoint::VELOCITY_EAST]  = 0;
 401     uavo.Velocity[Waypoint::VELOCITY_DOWN]  = 0;
 402
 403     return uavo;
 404 }