diff --git a/.gitignore b/.gitignore index 03e3788..3d8652c 100644 --- a/.gitignore +++ b/.gitignore @@ -44,3 +44,4 @@ user_config.pri *.cproject *.sln *.suo +*thirdParty* diff --git a/src/Waypoint.cc b/src/Waypoint.cc index fe36d14..0d1f876 100644 --- a/src/Waypoint.cc +++ b/src/Waypoint.cc @@ -57,6 +57,11 @@ Waypoint::~Waypoint() } +bool Waypoint::isNavigationType() +{ + return (action < MAV_CMD_NAV_LAST); +} + void Waypoint::save(QTextStream &saveStream) { QString position("%1\t%2\t%3"); diff --git a/src/Waypoint.h b/src/Waypoint.h index e9607ab..0990edd 100644 --- a/src/Waypoint.h +++ b/src/Waypoint.h @@ -71,6 +71,8 @@ public: MAV_FRAME getFrame() const { return frame; } MAV_CMD getAction() const { return action; } const QString& getName() const { return name; } + /** @brief Returns true if x, y, z contain reasonable navigation data */ + bool isNavigationType(); void save(QTextStream &saveStream); bool load(QTextStream &loadStream); diff --git a/src/uas/UASWaypointManager.cc b/src/uas/UASWaypointManager.cc index 17680bd..b245a1e 100644 --- a/src/uas/UASWaypointManager.cc +++ b/src/uas/UASWaypointManager.cc @@ -506,6 +506,22 @@ const QVector UASWaypointManager::getGlobalFrameWaypointList() return wps; } +const QVector UASWaypointManager::getGlobalFrameAndNavTypeWaypointList() +{ + // 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 && wp->isNavigationType()) + { + wps.append(wp); + } + } + return wps; +} + int UASWaypointManager::getIndexOf(Waypoint* wp) { return waypoints.indexOf(wp); @@ -531,6 +547,26 @@ int UASWaypointManager::getGlobalFrameIndexOf(Waypoint* wp) return -1; } +int UASWaypointManager::getGlobalFrameAndNavTypeIndexOf(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 && p->isNavigationType()) + { + if (p == wp) + { + return i; + } + i++; + } + } + + return -1; +} + int UASWaypointManager::getGlobalFrameCount() { // Search through all waypoints, @@ -547,6 +583,22 @@ int UASWaypointManager::getGlobalFrameCount() return i; } +int UASWaypointManager::getGlobalFrameAndNavTypeCount() +{ + // Search through all waypoints, + // counting only those in global frame + int i = 0; + foreach (Waypoint* p, waypoints) + { + if (p->getFrame() == MAV_FRAME_GLOBAL && p->isNavigationType()) + { + i++; + } + } + + return i; +} + int UASWaypointManager::getLocalFrameCount() { // Search through all waypoints, diff --git a/src/uas/UASWaypointManager.h b/src/uas/UASWaypointManager.h index 0157d4a..773470e 100644 --- a/src/uas/UASWaypointManager.h +++ b/src/uas/UASWaypointManager.h @@ -87,11 +87,14 @@ public: /*@{*/ const QVector &getWaypointList(void) { return waypoints; } ///< Returns a const reference to the waypoint list. const QVector getGlobalFrameWaypointList(); ///< Returns a global waypoint list + const QVector getGlobalFrameAndNavTypeWaypointList(); ///< Returns a global waypoint list containing only waypoints suitable for navigation. Actions and other mission items are filtered out. 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 getGlobalFrameAndNavTypeIndexOf(Waypoint* wp); ///< Get the index of a waypoint in the list, counting only global AND navigation mode 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 getGlobalFrameAndNavTypeCount(); ///< Get the count of global waypoints in navigation mode in the list int getLocalFrameCount(); ///< Get the count of local waypoints in the list /*@}*/ diff --git a/src/ui/CommConfigurationWindow.cc b/src/ui/CommConfigurationWindow.cc index 2a87d59..c09319b 100644 --- a/src/ui/CommConfigurationWindow.cc +++ b/src/ui/CommConfigurationWindow.cc @@ -111,9 +111,10 @@ CommConfigurationWindow::CommConfigurationWindow(LinkInterface* link, ProtocolIn if(serial != 0) { QWidget* conf = new SerialConfigurationWindow(serial, this); - QBoxLayout* layout = new QBoxLayout(QBoxLayout::LeftToRight, ui.linkGroupBox); - layout->addWidget(conf); - ui.linkGroupBox->setLayout(layout); + //QBoxLayout* layout = new QBoxLayout(QBoxLayout::LeftToRight, ui.linkGroupBox); + //layout->addWidget(conf); + ui.linkScrollArea->setWidget(conf); + //ui.linkScrollArea->setLayout(layout); ui.linkGroupBox->setTitle(tr("Serial Link")); ui.linkType->setCurrentIndex(0); //ui.linkGroupBox->setTitle(link->getName()); @@ -123,9 +124,10 @@ CommConfigurationWindow::CommConfigurationWindow(LinkInterface* link, ProtocolIn if (udp != 0) { QWidget* conf = new QGCUDPLinkConfiguration(udp, this); - QBoxLayout* layout = new QBoxLayout(QBoxLayout::LeftToRight, ui.linkGroupBox); - layout->addWidget(conf); - ui.linkGroupBox->setLayout(layout); + //QBoxLayout* layout = new QBoxLayout(QBoxLayout::LeftToRight, ui.linkGroupBox); + //layout->addWidget(conf); + //ui.linkGroupBox->setLayout(layout); + ui.linkScrollArea->setWidget(conf); ui.linkGroupBox->setTitle(tr("UDP Link")); ui.linkType->setCurrentIndex(1); } @@ -162,9 +164,10 @@ CommConfigurationWindow::CommConfigurationWindow(LinkInterface* link, ProtocolIn if (mavlink != 0) { QWidget* conf = new MAVLinkSettingsWidget(mavlink, this); - QBoxLayout* layout = new QBoxLayout(QBoxLayout::LeftToRight, ui.protocolGroupBox); - layout->addWidget(conf); - ui.protocolGroupBox->setLayout(layout); + //QBoxLayout* layout = new QBoxLayout(QBoxLayout::LeftToRight, ui.protocolGroupBox); + //layout->addWidget(conf); + //ui.protocolGroupBox->setLayout(layout); + ui.protocolScrollArea->setWidget(conf); ui.protocolGroupBox->setTitle(protocol->getName()+" (Global Settings)"); } else diff --git a/src/ui/CommSettings.ui b/src/ui/CommSettings.ui index 71a4c30..3eeaea7 100644 --- a/src/ui/CommSettings.ui +++ b/src/ui/CommSettings.ui @@ -7,16 +7,19 @@ 0 0 413 - 404 + 484 Form - + 6 + + 6 + @@ -28,7 +31,7 @@ 5 - 10 + 5 @@ -73,6 +76,31 @@ Link + + + 0 + + + 0 + + + + + true + + + + + 0 + 0 + 393 + 154 + + + + + + @@ -80,6 +108,31 @@ Protocol + + + 0 + + + 0 + + + + + true + + + + + 0 + 0 + 393 + 154 + + + + + + @@ -97,6 +150,9 @@ + + 12 + QLayout::SetDefaultConstraint diff --git a/src/ui/MapWidget.cc b/src/ui/MapWidget.cc index a803587..d1f2c18 100644 --- a/src/ui/MapWidget.cc +++ b/src/ui/MapWidget.cc @@ -492,14 +492,14 @@ void MapWidget::updateWaypoint(int uas, Waypoint* wp, bool updateView) if (uas == this->mav->getUASID()) { // Only accept waypoints in global coordinate frame - if (wp->getFrame() == MAV_FRAME_GLOBAL) + if (wp->getFrame() == MAV_FRAME_GLOBAL && wp->isNavigationType()) { // We're good, this is a global waypoint // Get the index of this waypoint - // note the call to getGlobalFrameIndexOf() + // note the call to getGlobalFrameAndNavTypeIndexOf() // as we're only handling global waypoints - int wpindex = UASManager::instance()->getUASForId(uas)->getWaypointManager()->getGlobalFrameIndexOf(wp); + int wpindex = UASManager::instance()->getUASForId(uas)->getWaypointManager()->getGlobalFrameAndNavTypeIndexOf(wp); // If not found, return (this should never happen, but helps safety) if (wpindex == -1) return; @@ -558,7 +558,7 @@ void MapWidget::updateWaypoint(int uas, Waypoint* wp, bool updateView) // waypoint list. This implies that the coordinate frame of this // waypoint was changed and the list containing only global // waypoints was shortened. Thus update the whole list - if (waypointPath->points().count() > UASManager::instance()->getUASForId(uas)->getWaypointManager()->getGlobalFrameCount()) + if (waypointPath->points().count() > UASManager::instance()->getUASForId(uas)->getWaypointManager()->getGlobalFrameAndNavTypeCount()) { updateWaypointList(uas); } @@ -659,7 +659,7 @@ void MapWidget::captureGeometryDrag(Geometry* geom, QPointF coordinate) // Update waypoint data storage if (mav) { - QVector wps = mav->getWaypointManager()->getGlobalFrameWaypointList(); + QVector wps = mav->getWaypointManager()->getGlobalFrameAndNavTypeWaypointList(); if (wps.size() > index) { @@ -738,7 +738,7 @@ void MapWidget::updateWaypointList(int uas) } // Trim internal list to number of global waypoints in the waypoint manager list - int overSize = waypointPath->points().count() - uasInstance->getWaypointManager()->getGlobalFrameCount(); + int overSize = waypointPath->points().count() - uasInstance->getWaypointManager()->getGlobalFrameAndNavTypeCount(); if (overSize > 0) { // Remove n waypoints at the end of the list diff --git a/src/ui/SerialSettings.ui b/src/ui/SerialSettings.ui index 4910784..6056720 100644 --- a/src/ui/SerialSettings.ui +++ b/src/ui/SerialSettings.ui @@ -6,14 +6,14 @@ 0 0 - 449 - 250 + 304 + 283 Form - + 6 @@ -284,13 +284,6 @@ - - - - Data bits - - - @@ -373,12 +366,19 @@ - 431 - 47 + 0 + 0 + + + + Data bits + + + diff --git a/src/ui/map3D/QGCGoogleEarthView.cc b/src/ui/map3D/QGCGoogleEarthView.cc index 2b26892..6a69fc7 100644 --- a/src/ui/map3D/QGCGoogleEarthView.cc +++ b/src/ui/map3D/QGCGoogleEarthView.cc @@ -234,14 +234,14 @@ void QGCGoogleEarthView::setActiveUAS(UASInterface* uas) void QGCGoogleEarthView::updateWaypoint(int uas, Waypoint* wp) { // Only accept waypoints in global coordinate frame - if (wp->getFrame() == MAV_FRAME_GLOBAL) + if (wp->getFrame() == MAV_FRAME_GLOBAL && wp->isNavigationType()) { // We're good, this is a global waypoint // Get the index of this waypoint - // note the call to getGlobalFrameIndexOf() + // note the call to getGlobalFrameAndNavTypeIndexOf() // as we're only handling global waypoints - int wpindex = UASManager::instance()->getUASForId(uas)->getWaypointManager()->getGlobalFrameIndexOf(wp); + int wpindex = UASManager::instance()->getUASForId(uas)->getWaypointManager()->getGlobalFrameAndNavTypeIndexOf(wp); // If not found, return (this should never happen, but helps safety) if (wpindex == -1) { @@ -267,7 +267,7 @@ void QGCGoogleEarthView::updateWaypointList(int uas) if (uasInstance) { // Get all waypoints, including non-global waypoints - QVector wpList = uasInstance->getWaypointManager()->getGlobalFrameWaypointList(); + QVector wpList = uasInstance->getWaypointManager()->getGlobalFrameAndNavTypeWaypointList(); // Trim internal list to number of global waypoints in the waypoint manager list javaScript(QString("updateWaypointListLength(%1,%2);").arg(uas).arg(wpList.count())); @@ -741,7 +741,7 @@ void QGCGoogleEarthView::updateState() // Update waypoint or symbol if (mav) { - QVector wps = mav->getWaypointManager()->getGlobalFrameWaypointList(); + QVector wps = mav->getWaypointManager()->getGlobalFrameAndNavTypeWaypointList(); bool ok; int index = idText.toInt(&ok);