Made map widget aware of non-global frame waypoints

src/comm/MAVLinkSimulationLink.cc

@@ -957,9 +957,9 @@ bool MAVLinkSimulationLink::connect()
 
     start(LowPriority);
     MAVLinkSimulationMAV* mav1 = new MAVLinkSimulationMAV(this, 1, 47.376, 8.548);
-    MAVLinkSimulationMAV* mav2 = new MAVLinkSimulationMAV(this, 2);
+    //MAVLinkSimulationMAV* mav2 = new MAVLinkSimulationMAV(this, 2);
     Q_UNUSED(mav1);
-    Q_UNUSED(mav2);
+    //Q_UNUSED(mav2);
     //    timer->start(rate);
     return true;
 }

src/comm/MAVLinkSimulationWaypointPlanner.cc

@@ -205,6 +205,7 @@ MAVLinkSimulationWaypointPlanner::MAVLinkSimulationWaypointPlanner(MAVLinkSimula
     silent(false)
 {
     connect(parent, SIGNAL(messageReceived(mavlink_message_t)), this, SLOT(handleMessage(mavlink_message_t)));
+    qDebug() << "PLANNER FOR SYSTEM" << systemid << "INITIALIZED";
 }
 
 
@@ -706,6 +707,10 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
                                         }
                                 }
                         }
+                        else
+                        {
+                            qDebug() << "SYSTEM / COMPONENT ID MISMATCH: target sys:" << wpc.target_system << "this system:" << systemid << "target comp:" << wpc.target_component << "this comp:" << compid;
+                        }
                         break;
                 }
 

src/uas/UASWaypointManager.cc

@@ -487,11 +487,119 @@ void UASWaypointManager::clearWaypointList()
     }
 }
 
+const QVector<Waypoint *> UASWaypointManager::getGlobalFrameWaypointList()
+{
+    // TODO Keep this global frame list up to date
+    // with complete waypoint list
+    // instead of filtering on each request
+    QVector<Waypoint*> wps;
+    foreach (Waypoint* wp, waypoints)
+    {
+        if (wp->getFrame() == MAV_FRAME_GLOBAL)
+        {
+            wps.append(wp);
+        }
+    }
+    return wps;
+}
+
 int UASWaypointManager::getIndexOf(Waypoint* wp)
 {
     return waypoints.indexOf(wp);
 }
 
+int UASWaypointManager::getGlobalFrameIndexOf(Waypoint* wp)
+{
+    // Search through all waypoints,
+    // counting only those in global frame
+    int i = 0;
+    foreach (Waypoint* p, waypoints)
+    {
+        if (p->getFrame() == MAV_FRAME_GLOBAL)
+        {
+            if (p == wp)
+            {
+                return i;
+            }
+            i++;
+        }
+    }
+
+    return -1;
+}
+
+int UASWaypointManager::getGlobalFrameCount()
+{
+    // Search through all waypoints,
+    // counting only those in global frame
+    int i = 0;
+    foreach (Waypoint* p, waypoints)
+    {
+        if (p->getFrame() == MAV_FRAME_GLOBAL)
+        {
+            i++;
+        }
+    }
+
+    return i;
+}
+
+int UASWaypointManager::getLocalFrameCount()
+{
+    // Search through all waypoints,
+    // counting only those in global frame
+    int i = 0;
+    foreach (Waypoint* p, waypoints)
+    {
+        if (p->getFrame() == MAV_FRAME_GLOBAL)
+        {
+            i++;
+        }
+    }
+
+    return i;
+}
+
+int UASWaypointManager::getLocalFrameIndexOf(Waypoint* wp)
+{
+    // Search through all waypoints,
+    // counting only those in local frame
+    int i = 0;
+    foreach (Waypoint* p, waypoints)
+    {
+        if (p->getFrame() == MAV_FRAME_LOCAL)
+        {
+            if (p == wp)
+            {
+                return i;
+            }
+            i++;
+        }
+    }
+
+    return -1;
+}
+
+int UASWaypointManager::getMissionFrameIndexOf(Waypoint* wp)
+{
+    // Search through all waypoints,
+    // counting only those in mission frame
+    int i = 0;
+    foreach (Waypoint* p, waypoints)
+    {
+        if (p->getFrame() == MAV_FRAME_MISSION)
+        {
+            if (p == wp)
+            {
+                return i;
+            }
+            i++;
+        }
+    }
+
+    return -1;
+}
+
 void UASWaypointManager::readWaypoints()
 {
     emit readGlobalWPFromUAS(true);

src/uas/UASWaypointManager.h

@@ -86,7 +86,13 @@ public:
     /** @name Waypoint list operations */
     /*@{*/
     const QVector<Waypoint *> &getWaypointList(void) { return waypoints; }  ///< Returns a const reference to the waypoint list.
-    int getIndexOf(Waypoint* wp);                               ///< Get the index of a waypoint in the list
+    const QVector<Waypoint *> getGlobalFrameWaypointList();  ///< Returns a global waypoint list
+    int getIndexOf(Waypoint* wp);                   ///< Get the index of a waypoint in the list
+    int getGlobalFrameIndexOf(Waypoint* wp);    ///< Get the index of a waypoint in the list, counting only global waypoints
+    int getLocalFrameIndexOf(Waypoint* wp);     ///< Get the index of a waypoint in the list, counting only local waypoints
+    int getMissionFrameIndexOf(Waypoint* wp);   ///< Get the index of a waypoint in the list, counting only mission waypoints
+    int getGlobalFrameCount(); ///< Get the count of global waypoints in the list
+    int getLocalFrameCount();   ///< Get the count of local waypoints in the list
     /*@}*/
 
     UAS& getUAS() { return this->uas; }                         ///< Returns the owning UAS

src/ui/MapWidget.cc

@@ -445,93 +445,81 @@ void MapWidget::updateWaypoint(int uas, Waypoint* wp)
     updateWaypoint(uas, wp, true);
 }
 
+/**
+ * This function is called if a a single waypoint is updated and
+ * also if the whole list changes.
+ */
 void MapWidget::updateWaypoint(int uas, Waypoint* wp, bool updateView)
 {
-    qDebug() << "UPDATING WP" << wp->getId() << wp <<  __FILE__ << __LINE__;
+    // Make sure this is the right UAS
     if (uas == this->mav->getUASID())
     {
         // TODO The map widget is not yet aware of non-global, non-navigation WPs
-        if (wp->getFrame() == MAV_FRAME_GLOBAL && wp->getAction() == MAV_ACTION_NAVIGATE)
+        if (wp->getFrame() == MAV_FRAME_GLOBAL)
         {
             // We're good, this is a global waypoint
-            // This is where the code below should be executed in
-        }
-        else
-        {
-            // We're screwed, this is not a global waypoint
-            qDebug() << "WARNING: The map widget is not prepared yet for non-navigation WPs" << __FILE__ << __LINE__;
-        }
 
-        int wpindex = UASManager::instance()->getUASForId(uas)->getWaypointManager()->getIndexOf(wp);
-        if (wpindex == -1) return;
-        // Create waypoint name
-        //QString str = QString("%1").arg(wpindex);
-        // Check if wp exists yet
-        if (!(wpIcons.count() > wpindex))
-        {
-            QPointF coordinate;
-            coordinate.setX(wp->getX());
-            coordinate.setY(wp->getY());
-            createWaypointGraphAtMap(wpindex, coordinate);
-        }
-        else
-        {
-            // Waypoint exists, update it
-            if(!waypointIsDrag)
+            int wpindex = UASManager::instance()->getUASForId(uas)->getWaypointManager()->getGlobalFrameIndexOf(wp);
+            if (wpindex == -1) return;
+            // Check if wp exists yet
+            if (!(wpIcons.count() > wpindex))
             {
                 QPointF coordinate;
                 coordinate.setX(wp->getX());
                 coordinate.setY(wp->getY());
-
-                Point* waypoint;
-                waypoint = wps.at(wpindex);//wpIndex[str];
-                if (waypoint)
+                createWaypointGraphAtMap(wpindex, coordinate);
+            }
+            else
+            {
+                // Waypoint exists, update it
+                if(!waypointIsDrag)
                 {
-                    // First set waypoint coordinate
-                    waypoint->setCoordinate(coordinate);
-                    // Now update icon position
-                    //mc->layer("Waypoints")->removeGeometry(wpIcons.at(wpindex));
-                    wpIcons.at(wpindex)->setCoordinate(coordinate);
-                    wpIcons.at(wpindex)->setPen(mavPens.value(uas));
-                    //mc->layer("Waypoints")->addGeometry(wpIcons.at(wpindex));
-                    // Then waypoint line coordinate
-                    Point* linesegment = NULL;
-                    if (waypointPath->points().size() > wpindex)
-                    {
-                        linesegment = waypointPath->points().at(wpindex);
-                    }
-                    else
-                    {
-                        waypointPath->addPoint(waypoint);
-                    }
+                    QPointF coordinate;
+                    coordinate.setX(wp->getX());
+                    coordinate.setY(wp->getY());
 
-                    if (linesegment)
+                    Point* waypoint;
+                    waypoint = wps.at(wpindex);//wpIndex[str];
+                    if (waypoint)
                     {
-                        linesegment->setCoordinate(coordinate);
+                        // First set waypoint coordinate
+                        waypoint->setCoordinate(coordinate);
+                        // Now update icon position
+                        //mc->layer("Waypoints")->removeGeometry(wpIcons.at(wpindex));
+                        wpIcons.at(wpindex)->setCoordinate(coordinate);
+                        wpIcons.at(wpindex)->setPen(mavPens.value(uas));
+                        //mc->layer("Waypoints")->addGeometry(wpIcons.at(wpindex));
+                        // Then waypoint line coordinate
+                        Point* linesegment = NULL;
+                        if (waypointPath->points().size() > wpindex)
+                        {
+                            linesegment = waypointPath->points().at(wpindex);
+                        }
+                        else
+                        {
+                            waypointPath->addPoint(waypoint);
+                        }
+
+                        if (linesegment)
+                        {
+                            linesegment->setCoordinate(coordinate);
+                        }
+
+                        //point2Find = dynamic_cast <Point*> (mc->layer("Waypoints")->get_Geometry(wpindex));
+                        //point2Find->setCoordinate(coordinate);
+                        if (updateView) if (isVisible()) mc->updateRequest(waypoint->boundingBox().toRect());
                     }
-
-                    //point2Find = dynamic_cast <Point*> (mc->layer("Waypoints")->get_Geometry(wpindex));
-                    //point2Find->setCoordinate(coordinate);
-                    if (updateView) if (isVisible()) mc->updateRequest(waypoint->boundingBox().toRect());
                 }
             }
         }
-        // Set unvisible
-        if (wp->getFrame() != MAV_FRAME_GLOBAL || !(wp->getAction() == MAV_ACTION_NAVIGATE
-                                                 || wp->getAction() == MAV_ACTION_LOITER
-                                                 || wp->getAction() == MAV_ACTION_LOITER_MAX_TIME
-                                                 || wp->getAction() == MAV_ACTION_LOITER_MAX_TURNS
-                                                 || wp->getAction() == MAV_ACTION_TAKEOFF
-                                                 || wp->getAction() == MAV_ACTION_LAND
-                                                 || wp->getAction() == MAV_ACTION_RETURN))
-        {
-            wpIcons.at(wpindex)->setVisible(false);
-            waypointPath->points().at(wpindex)->setVisible(false);
-        }
         else
         {
-            wpIcons.at(wpindex)->setVisible(true);
-            waypointPath->points().at(wpindex)->setVisible(true);
+            // We're screwed, this is not a global waypoint
+            // Delete from list if the list is now too long
+            if (waypointPath->points().count() > UASManager::instance()->getUASForId(uas)->getWaypointManager()->getGlobalFrameCount())
+            {
+                updateWaypointList(uas);
+            }
         }
     }
 }
@@ -625,13 +613,14 @@ void MapWidget::captureGeometryDrag(Geometry* geom, QPointF coordinate)
         // Update waypoint data storage
         if (mav)
         {
-            QVector<Waypoint*> wps = mav->getWaypointManager()->getWaypointList();
+            QVector<Waypoint*> wps = mav->getWaypointManager()->getGlobalFrameWaypointList();
 
             if (wps.size() > index)
             {
-                wps.at(index)->setX(coordinate.x());
-                wps.at(index)->setY(coordinate.y());
-                mav->getWaypointManager()->notifyOfChange(wps.at(index));
+                Waypoint* wp = wps.at(index);
+                wp->setX(coordinate.x());
+                wp->setY(coordinate.y());
+                mav->getWaypointManager()->notifyOfChange(wp);
             }
         }
 
@@ -692,17 +681,10 @@ void MapWidget::updateWaypointList(int uas)
             return;
         }
 
-        // Load all existing waypoints into map view
-        foreach (Waypoint* wp, wpList)
-        {
-            // Block updates, since we update everything in the next step
-            updateWaypoint(mav->getUASID(), wp, false);
-        }
-
-        // Delete now unused wps
-        if (waypointPath->points().count() > wpList.count())
+        // Delete unused waypoints first
+        int overSize = waypointPath->points().count() - uasInstance->getWaypointManager()->getGlobalFrameCount();
+        if (overSize > 0)
         {
-            int overSize = waypointPath->points().count() - wpList.count();
             for (int i = 0; i < overSize; ++i)
             {
                 wps.removeLast();
@@ -712,6 +694,13 @@ void MapWidget::updateWaypointList(int uas)
             }
         }
 
+        // Load all existing waypoints into map view
+        foreach (Waypoint* wp, wpList)
+        {
+            // Block updates, since we update everything in the next step
+            updateWaypoint(mav->getUASID(), wp, false);
+        }
+
         // Update view
         if (isVisible()) mc->updateRequest(updateRect);
     }