diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc index 0eb07475bc519fd7251697d35f63d4ac8a89b1cd..b9aeb72f80a88099651325fe7eae9cb9d7ed78b0 100644 --- a/src/comm/MAVLinkSimulationLink.cc +++ b/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; } diff --git a/src/comm/MAVLinkSimulationWaypointPlanner.cc b/src/comm/MAVLinkSimulationWaypointPlanner.cc index 0819e1f4fa9bd2029b6c5437424d927c82c86658..448c50697c12e36db53e761d01a09a6919922d05 100644 --- a/src/comm/MAVLinkSimulationWaypointPlanner.cc +++ b/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; } diff --git a/src/uas/UASWaypointManager.cc b/src/uas/UASWaypointManager.cc index b47abb86e8ed32a67fb0da8554ca912abeb4ed96..9c4aa167e87925246c8218d7e695839a2350447c 100644 --- a/src/uas/UASWaypointManager.cc +++ b/src/uas/UASWaypointManager.cc @@ -487,11 +487,119 @@ void UASWaypointManager::clearWaypointList() } } +const QVector UASWaypointManager::getGlobalFrameWaypointList() +{ + // TODO Keep this global frame list up to date + // with complete waypoint list + // instead of filtering on each request + QVector 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); diff --git a/src/uas/UASWaypointManager.h b/src/uas/UASWaypointManager.h index 5e62476144ed26d03853b284d59e78caf028da7a..0157d4ae6076177cd9b97d62e8e7cd2c2c622227 100644 --- a/src/uas/UASWaypointManager.h +++ b/src/uas/UASWaypointManager.h @@ -86,7 +86,13 @@ public: /** @name Waypoint list operations */ /*@{*/ const QVector &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 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 diff --git a/src/ui/MapWidget.cc b/src/ui/MapWidget.cc index 76affefe06fcacd8b154d7fb48b62ae00377e573..04b7961bcd356bca51a317806c3c84fbcff6fb6c 100644 --- a/src/ui/MapWidget.cc +++ b/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 (mc->layer("Waypoints")->get_Geometry(wpindex)); + //point2Find->setCoordinate(coordinate); + if (updateView) if (isVisible()) mc->updateRequest(waypoint->boundingBox().toRect()); } - - //point2Find = dynamic_cast (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 wps = mav->getWaypointManager()->getWaypointList(); + QVector 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); }