From 08040ab5cf2235d53287bc3b7963fce467572e69 Mon Sep 17 00:00:00 2001 From: PT_Dreamer Date: Sun, 3 Jun 2012 19:19:00 +0100 Subject: [PATCH] GCS/MapLib - massive cleaning of the plugin --- .../opmapcontrol/src/mapwidget/homeitem.h | 2 +- .../src/mapwidget/mapgraphicitem.cpp | 1 + .../src/mapwidget/opmapwidget.cpp | 58 +- .../opmapcontrol/src/mapwidget/opmapwidget.h | 3 + .../src/mapwidget/waypointitem.cpp | 58 +- .../opmapcontrol/src/mapwidget/waypointitem.h | 1 + .../src/plugins/opmap/opmapgadget.cpp | 35 +- .../src/plugins/opmap/opmapgadget.h | 7 +- .../opmap/opmapgadgetconfiguration.cpp | 19 +- .../plugins/opmap/opmapgadgetconfiguration.h | 5 +- .../src/plugins/opmap/opmapgadgetwidget.cpp | 587 ++++-------------- .../src/plugins/opmap/opmapgadgetwidget.h | 32 +- .../src/plugins/opmap/pathplanmanager.cpp | 27 +- .../src/plugins/opmap/pathplanmanager.h | 6 +- 14 files changed, 299 insertions(+), 542 deletions(-) diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/homeitem.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/homeitem.h index eb16b6017..f8003335b 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/homeitem.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/homeitem.h @@ -53,7 +53,7 @@ namespace mapcontrol int SafeArea()const{return safearea;} void SetSafeArea(int const& value){safearea=value;} bool safe; - void SetCoord(internals::PointLatLng const& value){coord=value;} + void SetCoord(internals::PointLatLng const& value){emit homePositionChanged(value);coord=value;} internals::PointLatLng Coord()const{return coord;} void SetAltitude(int const& value){altitude=value;} int Altitude()const{return altitude;} diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapgraphicitem.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapgraphicitem.cpp index 2e098bc73..8620886ee 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapgraphicitem.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/mapgraphicitem.cpp @@ -276,6 +276,7 @@ namespace mapcontrol { if(event->modifiers()&(Qt::ShiftModifier|Qt::ControlModifier)) this->setCursor(Qt::CrossCursor); + QGraphicsItem::keyPressEvent(event); } void MapGraphicItem::keyReleaseEvent(QKeyEvent *event) { diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.cpp index 7dd2ef600..f09859395 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.cpp @@ -220,6 +220,13 @@ namespace mapcontrol emit WPCreated(position,item); return item; } + WayPointItem* OPMapWidget::magicWPCreate() + { + WayPointItem* item=new WayPointItem(map,true); + item->SetShowNumber(false); + item->setParentItem(map); + return item; + } void OPMapWidget::WPCreate(WayPointItem* item) { ConnectWP(item); @@ -294,13 +301,46 @@ namespace mapcontrol emit WPDeleted(item->Number(),item); delete item; } - void OPMapWidget::WPDeleteAll() + void OPMapWidget::WPSetVisibleAll(bool value) { foreach(QGraphicsItem* i,map->childItems()) { WayPointItem* w=qgraphicsitem_cast(i); if(w) - delete w; + { + if(w->Number()!=-1) + w->setVisible(value); + } + } + } + void OPMapWidget::WPDeleteAll() + { + int x=0; + foreach(QGraphicsItem* i,map->childItems()) + { + WayPointItem* w=qgraphicsitem_cast(i); + if(w) + { + if(w->Number()!=-1) + { + emit WPDeleted(w->Number(),w); + delete w; + } + } + } + } + bool OPMapWidget::WPPresent() + { + foreach(QGraphicsItem* i,map->childItems()) + { + WayPointItem* w=qgraphicsitem_cast(i); + if(w) + { + if(w->Number()!=-1) + { + return true; + } + } } } void OPMapWidget::deleteAllOverlays() @@ -309,12 +349,12 @@ namespace mapcontrol { WayPointLine* w=qgraphicsitem_cast(i); if(w) - delete w; + w->deleteLater(); else { WayPointCircle* ww=qgraphicsitem_cast(i); if(ww) - delete ww; + ww->deleteLater(); } } } @@ -336,11 +376,11 @@ namespace mapcontrol void OPMapWidget::ConnectWP(WayPointItem *item) { - connect(item,SIGNAL(WPNumberChanged(int,int,WayPointItem*)),this,SIGNAL(WPNumberChanged(int,int,WayPointItem*))); - connect(item,SIGNAL(WPValuesChanged(WayPointItem*)),this,SIGNAL(WPValuesChanged(WayPointItem*))); - connect(this,SIGNAL(WPInserted(int,WayPointItem*)),item,SLOT(WPInserted(int,WayPointItem*))); - connect(this,SIGNAL(WPNumberChanged(int,int,WayPointItem*)),item,SLOT(WPRenumbered(int,int,WayPointItem*))); - connect(this,SIGNAL(WPDeleted(int,WayPointItem*)),item,SLOT(WPDeleted(int,WayPointItem*))); + connect(item,SIGNAL(WPNumberChanged(int,int,WayPointItem*)),this,SIGNAL(WPNumberChanged(int,int,WayPointItem*)),Qt::DirectConnection); + connect(item,SIGNAL(WPValuesChanged(WayPointItem*)),this,SIGNAL(WPValuesChanged(WayPointItem*)),Qt::DirectConnection); + connect(this,SIGNAL(WPInserted(int,WayPointItem*)),item,SLOT(WPInserted(int,WayPointItem*)),Qt::DirectConnection); + connect(this,SIGNAL(WPNumberChanged(int,int,WayPointItem*)),item,SLOT(WPRenumbered(int,int,WayPointItem*)),Qt::DirectConnection); + connect(this,SIGNAL(WPDeleted(int,WayPointItem*)),item,SLOT(WPDeleted(int,WayPointItem*)),Qt::DirectConnection); } void OPMapWidget::diagRefresh() { diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.h index 67d4e08fb..4da888bb1 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.h @@ -366,6 +366,9 @@ namespace mapcontrol WayPointCircle *WPCircleCreate(WayPointItem *center, WayPointItem *radius,bool clockwise); WayPointCircle *WPCircleCreate(HomeItem *center, WayPointItem *radius,bool clockwise); void deleteAllOverlays(); + void WPSetVisibleAll(bool value); + WayPointItem *magicWPCreate(); + bool WPPresent(); private: internals::Core *core; MapGraphicItem *map; diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointitem.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointitem.cpp index 08c202e9f..9f6f87f6d 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointitem.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointitem.cpp @@ -53,10 +53,49 @@ WayPointItem::WayPointItem(const internals::PointLatLng &coord,int const& altitu if(myHome) map->Projection()->offSetFromLatLngs(myHome->Coord(),coord,relativeCoord.distance,relativeCoord.bearing); - qDebug()<<"RELATIVE DISTANCE SET ON CTOR1"<setFlag(QGraphicsItem::ItemIsMovable,true); + this->setFlag(QGraphicsItem::ItemIgnoresTransformations,true); + this->setFlag(QGraphicsItem::ItemIsSelectable,true); + SetShowNumber(shownumber); + RefreshToolTip(); + RefreshPos(); + myHome=NULL; + QList list=map->childItems(); + foreach(QGraphicsItem * obj,list) + { + HomeItem* h=qgraphicsitem_cast (obj); + if(h) + myHome=h; + } + + if(myHome) + { + coord=map->Projection()->translate(myHome->Coord(),relativeCoord.distance,relativeCoord.bearing); + connect(myHome,SIGNAL(homePositionChanged(internals::PointLatLng)),this,SLOT(onHomePositionChanged(internals::PointLatLng))); + } + connect(this,SIGNAL(waypointdoubleclick(WayPointItem*)),map,SIGNAL(wpdoubleclicked(WayPointItem*))); +} WayPointItem::WayPointItem(const internals::PointLatLng &coord,int const& altitude, const QString &description, MapGraphicItem *map,wptype type):coord(coord),reached(false),description(description),shownumber(true),isDragging(false),altitude(altitude),map(map),myType(type) { text=0; @@ -81,7 +120,6 @@ WayPointItem::WayPointItem(const internals::PointLatLng &coord,int const& altitu if(myHome) { map->Projection()->offSetFromLatLngs(myHome->Coord(),coord,relativeCoord.distance,relativeCoord.bearing); - qDebug()<<"RELATIVE DISTANCE SET ON CTOR2"< list=map->childItems(); foreach(QGraphicsItem * obj,list) @@ -198,7 +235,6 @@ WayPointItem::WayPointItem(const internals::PointLatLng &coord,int const& altitu if(myHome) { map->Projection()->offSetFromLatLngs(myHome->Coord(),coord,relativeCoord.distance,relativeCoord.bearing); - qDebug()<<"RELATIVE DISTANCE SET ON MOUSEMOVEEVENT"<setText(coord_str+"\n"+relativeCoord_str); @@ -253,7 +289,6 @@ WayPointItem::WayPointItem(const internals::PointLatLng &coord,int const& altitu numberI->setText(QString::number(number)); numberIBG->setRect(numberI->boundingRect().adjusted(-2,0,1,0)); this->update(); - qDebug()<<"emit"<onumber) SetNumber(--number); + int n=number; + if(number>onumber) SetNumber(--n); } void WayPointItem::WPInserted(const int &onumber, WayPointItem *waypoint) { + if(Number()==-1) + return; + if(waypoint!=this) { if(onumber<=number) SetNumber(++number); @@ -358,7 +397,10 @@ WayPointItem::WayPointItem(const internals::PointLatLng &coord,int const& altitu type_str="Absolute"; QString coord_str = " " + QString::number(coord.Lat(), 'f', 6) + " " + QString::number(coord.Lng(), 'f', 6); QString relativeCoord_str = " Distance:" + QString::number(relativeCoord.distance) + " Bearing:" + QString::number(relativeCoord.bearing*180/M_PI); - setToolTip(QString("WayPoint Number:%1\nDescription:%2\nCoordinate:%4\nFrom Home:%5\nAltitude:%6\nType:%7\n%8").arg(QString::number(WayPointItem::number)).arg(description).arg(coord_str).arg(relativeCoord_str).arg(QString::number(altitude)).arg(type_str).arg(myCustomString)); + if(Number()!=-1) + setToolTip(QString("WayPoint Number:%1\nDescription:%2\nCoordinate:%4\nFrom Home:%5\nAltitude:%6\nType:%7\n%8").arg(QString::number(Number())).arg(description).arg(coord_str).arg(relativeCoord_str).arg(QString::number(altitude)).arg(type_str).arg(myCustomString)); + else + setToolTip(QString("Magic WayPoint\nCoordinate:%1\nFrom Home:%2\nAltitude:%3\nType:%4\n%5").arg(coord_str).arg(relativeCoord_str).arg(QString::number(altitude)).arg(type_str).arg(myCustomString)); } int WayPointItem::snumber=0; diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointitem.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointitem.h index 3139980cf..e0bd594b5 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointitem.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointitem.h @@ -64,6 +64,7 @@ public: * @return */ WayPointItem(internals::PointLatLng const& coord,int const& altitude,MapGraphicItem* map,wptype type=absolute); + WayPointItem(MapGraphicItem* map,bool magicwaypoint); /** * @brief Constructer * diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapgadget.cpp b/ground/openpilotgcs/src/plugins/opmap/opmapgadget.cpp index be2a4e476..388fd57bb 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmapgadget.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/opmapgadget.cpp @@ -26,31 +26,42 @@ */ #include "opmapgadget.h" #include "opmapgadgetwidget.h" -#include "opmapgadgetconfiguration.h" OPMapGadget::OPMapGadget(QString classId, OPMapGadgetWidget *widget, QWidget *parent) : IUAVGadget(classId, parent), - m_widget(widget) + m_widget(widget),m_config(NULL) { + connect(m_widget,SIGNAL(defaultLocationAndZoomChanged(double,double,double)),this,SLOT(saveConfiguration(double,double,double))); } OPMapGadget::~OPMapGadget() { delete m_widget; } +void OPMapGadget::saveConfiguration(double lng,double lat,double zoom) +{ + if(m_config) + { + m_config->setLatitude(lat); + m_config->setLongitude(lng); + m_config->setZoom(zoom); + m_config->saveConfig(); + } +} void OPMapGadget::loadConfiguration(IUAVGadgetConfiguration *config) { - OPMapGadgetConfiguration *m = qobject_cast(config); + m_config = qobject_cast(config); - m_widget->setMapProvider(m->mapProvider()); - m_widget->setZoom(m->zoom()); - m_widget->setPosition(QPointF(m->longitude(), m->latitude())); - m_widget->setUseOpenGL(m->useOpenGL()); - m_widget->setShowTileGridLines(m->showTileGridLines()); - m_widget->setAccessMode(m->accessMode()); - m_widget->setUseMemoryCache(m->useMemoryCache()); - m_widget->setCacheLocation(m->cacheLocation()); - m_widget->SetUavPic(m->uavSymbol()); + m_widget->setMapProvider(m_config->mapProvider()); + m_widget->setZoom(m_config->zoom()); + m_widget->setPosition(QPointF(m_config->longitude(), m_config->latitude())); + m_widget->setHomePosition(QPointF(m_config->longitude(), m_config->latitude())); + m_widget->setUseOpenGL(m_config->useOpenGL()); + m_widget->setShowTileGridLines(m_config->showTileGridLines()); + m_widget->setAccessMode(m_config->accessMode()); + m_widget->setUseMemoryCache(m_config->useMemoryCache()); + m_widget->setCacheLocation(m_config->cacheLocation()); + m_widget->SetUavPic(m_config->uavSymbol()); } diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapgadget.h b/ground/openpilotgcs/src/plugins/opmap/opmapgadget.h index 030caf986..87866c43f 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmapgadget.h +++ b/ground/openpilotgcs/src/plugins/opmap/opmapgadget.h @@ -30,6 +30,7 @@ #include #include "opmapgadgetwidget.h" +#include "opmapgadgetconfiguration.h" class IUAVGadget; //class QList; @@ -47,10 +48,12 @@ public: ~OPMapGadget(); QWidget *widget() { return m_widget; } - void loadConfiguration(IUAVGadgetConfiguration* config); - + void loadConfiguration(IUAVGadgetConfiguration* m_config); private: OPMapGadgetWidget *m_widget; + OPMapGadgetConfiguration *m_config; +private slots: + void saveConfiguration(double lng, double lat, double zoom); }; diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetconfiguration.cpp b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetconfiguration.cpp index 67d1d1f98..7e77b0031 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetconfiguration.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetconfiguration.cpp @@ -41,7 +41,8 @@ OPMapGadgetConfiguration::OPMapGadgetConfiguration(QString classId, QSettings* m_useMemoryCache(true), m_cacheLocation(Utils::PathUtils().GetStoragePath() + "mapscache" + QDir::separator()), m_uavSymbol(QString::fromUtf8(":/uavs/images/mapquad.png")), - m_maxUpdateRate(2000) // ms + m_maxUpdateRate(2000), // ms + m_settings(qSettings) { //if a saved configuration exists load it @@ -97,7 +98,21 @@ IUAVGadgetConfiguration * OPMapGadgetConfiguration::clone() return m; } - +void OPMapGadgetConfiguration::saveConfig() const { + if(!m_settings) + return; + m_settings->setValue("mapProvider", m_mapProvider); + m_settings->setValue("defaultZoom", m_defaultZoom); + m_settings->setValue("defaultLatitude", m_defaultLatitude); + m_settings->setValue("defaultLongitude", m_defaultLongitude); + m_settings->setValue("useOpenGL", m_useOpenGL); + m_settings->setValue("showTileGridLines", m_showTileGridLines); + m_settings->setValue("accessMode", m_accessMode); + m_settings->setValue("useMemoryCache", m_useMemoryCache); + m_settings->setValue("uavSymbol", m_uavSymbol); + m_settings->setValue("cacheLocation", Utils::PathUtils().RemoveStoragePath(m_cacheLocation)); + m_settings->setValue("maxUpdateRate", m_maxUpdateRate); +} void OPMapGadgetConfiguration::saveConfig(QSettings* qSettings) const { qSettings->setValue("mapProvider", m_mapProvider); qSettings->setValue("defaultZoom", m_defaultZoom); diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetconfiguration.h b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetconfiguration.h index 1e630dc00..ffd0c5a08 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetconfiguration.h +++ b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetconfiguration.h @@ -65,8 +65,8 @@ public: bool useMemoryCache() const { return m_useMemoryCache; } QString cacheLocation() const { return m_cacheLocation; } QString uavSymbol() const { return m_uavSymbol; } - int maxUpdateRate() const { return m_maxUpdateRate; } - + int maxUpdateRate() const { return m_maxUpdateRate; } + void saveConfig() const; public slots: void setMapProvider(QString provider) { m_mapProvider = provider; } void setZoom(int zoom) { m_defaultZoom = zoom; } @@ -92,6 +92,7 @@ private: QString m_cacheLocation; QString m_uavSymbol; int m_maxUpdateRate; + QSettings * m_settings; }; #endif // OPMAP_GADGETCONFIGURATION_H diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp index 1e9aac754..3dabcd91e 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp @@ -131,17 +131,6 @@ OPMapGadgetWidget::OPMapGadgetWidget(QWidget *parent) : QWidget(parent) m_home_position.altitude = altitude; m_home_position.locked = false; - // ************** - // default magic waypoint params - - m_magic_waypoint.map_wp_item = NULL; - m_magic_waypoint.coord = m_home_position.coord; - m_magic_waypoint.altitude = altitude; - m_magic_waypoint.description = "Magic waypoint"; - m_magic_waypoint.locked = false; - m_magic_waypoint.time_seconds = 0; - m_magic_waypoint.hold_time_seconds = 0; - // ************** // create the widget that holds the user controls and the map @@ -194,50 +183,17 @@ OPMapGadgetWidget::OPMapGadgetWidget(QWidget *parent) : QWidget(parent) layout->addWidget(m_map); m_widget->mapWidget->setLayout(layout); - // ************** - // set the user control options - - // TODO: this switch does not make sense, does it?? - - switch (m_map_mode) - { - case Normal_MapMode: m_widget->toolButtonMagicWaypointMapMode->setChecked(false); m_widget->toolButtonNormalMapMode->setChecked(true); hideMagicWaypointControls(); - break; - - case MagicWaypoint_MapMode: - m_widget->toolButtonNormalMapMode->setChecked(false); - m_widget->toolButtonMagicWaypointMapMode->setChecked(true); - showMagicWaypointControls(); - break; - - default: - m_map_mode = Normal_MapMode; - m_widget->toolButtonMagicWaypointMapMode->setChecked(false); - m_widget->toolButtonNormalMapMode->setChecked(true); - hideMagicWaypointControls(); - break; - } m_widget->labelUAVPos->setText("---"); m_widget->labelMapPos->setText("---"); m_widget->labelMousePos->setText("---"); m_widget->labelMapZoom->setText("---"); - - // Splitter is not used at the moment: - // m_widget->splitter->setCollapsible(1, false); - - // set the size of the collapsable widgets - //QList m_SizeList; - //m_SizeList << 0 << 0 << 0; - //m_widget->splitter->setSizes(m_SizeList); - m_widget->progressBarMap->setMaximum(1); - #if defined(Q_OS_MAC) #elif defined(Q_OS_WIN) m_widget->comboBoxFindPlace->clear(); @@ -246,11 +202,6 @@ OPMapGadgetWidget::OPMapGadgetWidget(QWidget *parent) : QWidget(parent) #else #endif - - - // ************** - // map stuff - connect(m_map, SIGNAL(zoomChanged(double, double, double)), this, SLOT(zoomChanged(double, double, double))); // map zoom change signals connect(m_map, SIGNAL(OnCurrentPositionChanged(internals::PointLatLng)), this, SLOT(OnCurrentPositionChanged(internals::PointLatLng))); // map poisition change signals connect(m_map, SIGNAL(OnTileLoadComplete()), this, SLOT(OnTileLoadComplete())); // tile loading stop signals @@ -260,10 +211,9 @@ OPMapGadgetWidget::OPMapGadgetWidget(QWidget *parent) : QWidget(parent) connect(m_map, SIGNAL(OnMapTypeChanged(MapType::Types)), this, SLOT(OnMapTypeChanged(MapType::Types))); // map type changed connect(m_map, SIGNAL(OnEmptyTileError(int, core::Point)), this, SLOT(OnEmptyTileError(int, core::Point))); // tile error connect(m_map, SIGNAL(OnTilesStillToLoad(int)), this, SLOT(OnTilesStillToLoad(int))); // tile loading signals - connect(m_map, SIGNAL(WPNumberChanged(int const&,int const&,WayPointItem*)), this, SLOT(WPNumberChanged(int const&,int const&,WayPointItem*))); - connect(m_map, SIGNAL(WPValuesChanged(WayPointItem*)), this, SLOT(WPValuesChanged(WayPointItem*))); - connect(m_map, SIGNAL(WPInserted(int const&, WayPointItem*)), this, SLOT(WPInserted(int const&, WayPointItem*))); - connect(m_map, SIGNAL(WPDeleted(int,WayPointItem*)), this, SLOT(WPDeleted(int,WayPointItem*))); + // connect(m_map, SIGNAL(WPNumberChanged(int const&,int const&,WayPointItem*)), this, SLOT(WPNumberChanged(int const&,int const&,WayPointItem*))); + // connect(m_map, SIGNAL(WPInserted(int const&, WayPointItem*)), this, SLOT(WPInserted(int const&, WayPointItem*))); + // connect(m_map, SIGNAL(WPDeleted(int,WayPointItem*)), this, SLOT(WPDeleted(int,WayPointItem*))); connect(m_map,SIGNAL(OnWayPointDoubleClicked(WayPointItem*)),this,SLOT(wpDoubleClickEvent(WayPointItem*))); m_map->SetCurrentPosition(m_home_position.coord); // set the map position m_map->Home->SetCoord(m_home_position.coord); // set the HOME position @@ -274,7 +224,7 @@ OPMapGadgetWidget::OPMapGadgetWidget(QWidget *parent) : QWidget(parent) pathPlanManager * plan=new pathPlanManager(new QWidget(),m_map); plan->show(); - +/* distBearing db; db.distance=100; db.bearing=0; @@ -290,6 +240,9 @@ OPMapGadgetWidget::OPMapGadgetWidget(QWidget *parent) : QWidget(parent) wp=new t_waypoint; wp->map_wp_item=p2; m_waypoint_list.append(wp); +*/ + magicWayPoint=m_map->magicWPCreate(); + magicWayPoint->setVisible(false); // ************** // create various context menu (mouse right click menu) actions @@ -330,10 +283,8 @@ OPMapGadgetWidget::OPMapGadgetWidget(QWidget *parent) : QWidget(parent) m_statusUpdateTimer = new QTimer(); m_statusUpdateTimer->setInterval(200); -// m_statusUpdateTimer->setInterval(m_maxUpdateRate); connect(m_statusUpdateTimer, SIGNAL(timeout()), this, SLOT(updateMousePos())); m_statusUpdateTimer->start(); - // ************** m_map->setFocus(); @@ -349,29 +300,6 @@ OPMapGadgetWidget::~OPMapGadgetWidget() m_map->SetShowUAV(false); // " " } - - // this destructor doesn't appear to be called at shutdown??? - -// #if defined(Q_OS_MAC) -// #elif defined(Q_OS_WIN) -// saveComboBoxLines(m_widget->comboBoxFindPlace, QCoreApplication::applicationDirPath() + "/opmap_find_place_history.txt"); -// #else -// #endif - - m_waypoint_list_mutex.lock(); - foreach (t_waypoint *wp, m_waypoint_list) - { - if (!wp) continue; - - - // todo: - - - delete wp->map_wp_item; - } - m_waypoint_list_mutex.unlock(); - m_waypoint_list.clear(); - if (m_map) { delete m_map; @@ -384,24 +312,18 @@ OPMapGadgetWidget::~OPMapGadgetWidget() void OPMapGadgetWidget::resizeEvent(QResizeEvent *event) { - qDebug("opmap: resizeEvent"); - QWidget::resizeEvent(event); } void OPMapGadgetWidget::mouseMoveEvent(QMouseEvent *event) { - qDebug("opmap: mouseMoveEvent"); - if (m_widget && m_map) { } if (event->buttons() & Qt::LeftButton) { -// QPoint pos = event->pos(); } - QWidget::mouseMoveEvent(event); } void OPMapGadgetWidget::wpDoubleClickEvent(WayPointItem *wp) @@ -424,7 +346,6 @@ void OPMapGadgetWidget::contextMenuEvent(QContextMenuEvent *event) // current mouse position QPoint p = m_map->mapFromGlobal(event->globalPos()); m_context_menu_lat_lon = m_map->GetFromLocalToLatLng(p); -// m_context_menu_lat_lon = m_map->currentMousePosition(); if (!m_map->contentsRect().contains(p)) return; // the mouse click was not on the map @@ -445,26 +366,19 @@ void OPMapGadgetWidget::contextMenuEvent(QContextMenuEvent *event) // **************** // Dynamically create the popup menu - QMenu menu(this); - - menu.addAction(closeAct1); - - menu.addSeparator(); - - menu.addAction(reloadAct); - - menu.addSeparator(); - - menu.addAction(ripAct); - - menu.addSeparator(); + contextMenu.addAction(closeAct1); + contextMenu.addSeparator(); + contextMenu.addAction(reloadAct); + contextMenu.addSeparator(); + contextMenu.addAction(ripAct); + contextMenu.addSeparator(); QMenu maxUpdateRateSubMenu(tr("&Max Update Rate ") + "(" + QString::number(m_maxUpdateRate) + " ms)", this); for (int i = 0; i < maxUpdateRateAct.count(); i++) maxUpdateRateSubMenu.addAction(maxUpdateRateAct.at(i)); - menu.addMenu(&maxUpdateRateSubMenu); + contextMenu.addMenu(&maxUpdateRateSubMenu); - menu.addSeparator(); + contextMenu.addSeparator(); switch (m_map_mode) { @@ -482,19 +396,19 @@ void OPMapGadgetWidget::contextMenuEvent(QContextMenuEvent *event) QMenu mapModeSubMenu(tr("Map mode") + s, this); for (int i = 0; i < mapModeAct.count(); i++) mapModeSubMenu.addAction(mapModeAct.at(i)); - menu.addMenu(&mapModeSubMenu); + contextMenu.addMenu(&mapModeSubMenu); - menu.addSeparator(); + contextMenu.addSeparator(); QMenu copySubMenu(tr("Copy"), this); copySubMenu.addAction(copyMouseLatLonToClipAct); copySubMenu.addAction(copyMouseLatToClipAct); copySubMenu.addAction(copyMouseLonToClipAct); - menu.addMenu(©SubMenu); + contextMenu.addMenu(©SubMenu); - menu.addSeparator(); - - menu.addSeparator(); + contextMenu.addSeparator(); + contextMenu.addAction(changeDefaultLocalAndZoom); + contextMenu.addSeparator(); QMenu safeArea("Safety Area definitions"); // menu.addAction(showSafeAreaAct); @@ -503,68 +417,68 @@ void OPMapGadgetWidget::contextMenuEvent(QContextMenuEvent *event) safeAreaSubMenu.addAction(safeAreaAct.at(i)); safeArea.addMenu(&safeAreaSubMenu); safeArea.addAction(showSafeAreaAct); - menu.addMenu(&safeArea); + contextMenu.addMenu(&safeArea); - menu.addSeparator(); + contextMenu.addSeparator(); - menu.addAction(showCompassAct); + contextMenu.addAction(showCompassAct); - menu.addAction(showDiagnostics); + contextMenu.addAction(showDiagnostics); - menu.addSeparator()->setText(tr("Zoom")); + contextMenu.addSeparator()->setText(tr("Zoom")); - menu.addAction(zoomInAct); - menu.addAction(zoomOutAct); + contextMenu.addAction(zoomInAct); + contextMenu.addAction(zoomOutAct); QMenu zoomSubMenu(tr("&Zoom ") + "(" + QString::number(m_map->ZoomTotal()) + ")", this); for (int i = 0; i < zoomAct.count(); i++) zoomSubMenu.addAction(zoomAct.at(i)); - menu.addMenu(&zoomSubMenu); + contextMenu.addMenu(&zoomSubMenu); - menu.addSeparator(); + contextMenu.addSeparator(); - menu.addAction(goMouseClickAct); + contextMenu.addAction(goMouseClickAct); - menu.addSeparator()->setText(tr("HOME")); + contextMenu.addSeparator()->setText(tr("HOME")); - menu.addAction(setHomeAct); - menu.addAction(showHomeAct); - menu.addAction(goHomeAct); + contextMenu.addAction(setHomeAct); + contextMenu.addAction(showHomeAct); + contextMenu.addAction(goHomeAct); // **** // uav trails - - menu.addSeparator()->setText(tr("UAV Trail")); - + QMenu uav_menu(tr("UAV")); + uav_menu.addSeparator()->setText(tr("UAV Trail")); + contextMenu.addMenu(&uav_menu); QMenu uavTrailTypeSubMenu(tr("UAV trail type") + " (" + mapcontrol::Helper::StrFromUAVTrailType(m_map->UAV->GetTrailType()) + ")", this); for (int i = 0; i < uavTrailTypeAct.count(); i++) uavTrailTypeSubMenu.addAction(uavTrailTypeAct.at(i)); - menu.addMenu(&uavTrailTypeSubMenu); + uav_menu.addMenu(&uavTrailTypeSubMenu); QMenu uavTrailTimeSubMenu(tr("UAV trail time") + " (" + QString::number(m_map->UAV->TrailTime()) + " sec)", this); for (int i = 0; i < uavTrailTimeAct.count(); i++) uavTrailTimeSubMenu.addAction(uavTrailTimeAct.at(i)); - menu.addMenu(&uavTrailTimeSubMenu); + uav_menu.addMenu(&uavTrailTimeSubMenu); QMenu uavTrailDistanceSubMenu(tr("UAV trail distance") + " (" + QString::number(m_map->UAV->TrailDistance()) + " meters)", this); for (int i = 0; i < uavTrailDistanceAct.count(); i++) uavTrailDistanceSubMenu.addAction(uavTrailDistanceAct.at(i)); - menu.addMenu(&uavTrailDistanceSubMenu); + uav_menu.addMenu(&uavTrailDistanceSubMenu); - menu.addAction(showTrailAct); + uav_menu.addAction(showTrailAct); - menu.addAction(showTrailLineAct); + uav_menu.addAction(showTrailLineAct); - menu.addAction(clearUAVtrailAct); + uav_menu.addAction(clearUAVtrailAct); // **** - menu.addSeparator()->setText(tr("UAV")); + uav_menu.addSeparator()->setText(tr("UAV")); - menu.addAction(showUAVAct); - menu.addAction(followUAVpositionAct); - menu.addAction(followUAVheadingAct); - menu.addAction(goUAVAct); + uav_menu.addAction(showUAVAct); + uav_menu.addAction(followUAVpositionAct); + uav_menu.addAction(followUAVheadingAct); + uav_menu.addAction(goUAVAct); // ********* @@ -573,93 +487,51 @@ void OPMapGadgetWidget::contextMenuEvent(QContextMenuEvent *event) case Normal_MapMode: // only show the waypoint stuff if not in 'magic waypoint' mode - menu.addSeparator()->setText(tr("Waypoints")); + contextMenu.addSeparator()->setText(tr("Waypoints")); - menu.addAction(wayPointEditorAct); - menu.addAction(addWayPointAct); + contextMenu.addAction(wayPointEditorAct); + contextMenu.addAction(addWayPointAct); if (m_mouse_waypoint) { // we have a waypoint under the mouse - menu.addAction(editWayPointAct); + contextMenu.addAction(editWayPointAct); lockWayPointAct->setChecked(waypoint_locked); - menu.addAction(lockWayPointAct); + contextMenu.addAction(lockWayPointAct); if (!waypoint_locked) - menu.addAction(deleteWayPointAct); + contextMenu.addAction(deleteWayPointAct); } - m_waypoint_list_mutex.lock(); - if (m_waypoint_list.count() > 0) - menu.addAction(clearWayPointsAct); // we have waypoints - m_waypoint_list_mutex.unlock(); - + if (m_map->WPPresent()) + contextMenu.addAction(clearWayPointsAct); // we have waypoints break; case MagicWaypoint_MapMode: - menu.addSeparator()->setText(tr("Waypoints")); - menu.addAction(homeMagicWaypointAct); + contextMenu.addSeparator()->setText(tr("Waypoints")); + contextMenu.addAction(homeMagicWaypointAct); break; } // ********* - menu.addSeparator(); + contextMenu.addSeparator(); - menu.addAction(closeAct2); + contextMenu.addAction(closeAct2); - menu.exec(event->globalPos()); // popup the menu + contextMenu.exec(event->globalPos()); // popup the menu // **************** } -void OPMapGadgetWidget::keyPressEvent(QKeyEvent* event) -{ - qDebug() << "opmap: keyPressEvent, key =" << event->key() << endl; - - switch (event->key()) - { - case Qt::Key_Escape: - break; - - case Qt::Key_F1: - break; - - case Qt::Key_F2: - break; - - case Qt::Key_Up: - break; - - case Qt::Key_Down: - break; - - case Qt::Key_Left: - break; - - case Qt::Key_Right: - break; - - case Qt::Key_PageUp: - break; - - case Qt::Key_PageDown: - break; - } -} - // ************************************************************************************* // timer signals /** Updates the UAV position on the map. It is called every 200ms by a timer. - - TODO: consider updating upon object update, not timer. - - from Pip: No don't update on object update - had reports that peoples PC's can't cope with high update rates - have had to allow user to set map update from 100ms to 5 seconds (depending on their PC's graphics processing ability), so this needs to be kept on a timer. - */ +*/ void OPMapGadgetWidget::updatePosition() { double uav_latitude, uav_longitude, uav_altitude, uav_yaw; @@ -740,7 +612,7 @@ void OPMapGadgetWidget::updateMousePos() QPoint p = m_map->mapFromGlobal(QCursor::pos()); internals::PointLatLng lat_lon = m_map->GetFromLocalToLatLng(p); // fetch the current lat/lon mouse position - + lastLatLngMouse=lat_lon; if (!m_map->contentsRect().contains(p)) return; // the mouse is not on the map @@ -795,11 +667,6 @@ void OPMapGadgetWidget::updateMousePos() if (getUAVPosition(latitude, longitude, altitude)) // get current UAV position { internals::PointLatLng uav_pos = internals::PointLatLng(latitude, longitude); - -// double dist = distance(home_lat_lon, uav_pos); -// double bear = bearing(home_lat_lon, uav_pos); -// s += " " + QString::number(dist * 1000, 'f', 1) + "m"; -// s += " " + QString::number(bear, 'f', 1) + "deg"; } } m_widget->labelMousePos->setText(s); @@ -855,16 +722,11 @@ void OPMapGadgetWidget::OnTilesStillToLoad(int number) if (!m_widget || !m_map) return; -// if (prev_tile_number < number || m_widget->progressBarMap->maximum() < number) -// m_widget->progressBarMap->setMaximum(number); - if (m_widget->progressBarMap->maximum() < number) m_widget->progressBarMap->setMaximum(number); m_widget->progressBarMap->setValue(m_widget->progressBarMap->maximum() - number); // update the progress bar -// m_widget->labelNumTilesToLoad->setText(QString::number(number)); - m_prev_tile_number = number; } @@ -875,7 +737,6 @@ void OPMapGadgetWidget::OnTileLoadStart() { if (!m_widget || !m_map) return; - m_widget->progressBarMap->setVisible(true); } @@ -915,44 +776,6 @@ void OPMapGadgetWidget::WPNumberChanged(int const &oldnumber, int const &newnumb Q_UNUSED(waypoint); } -void OPMapGadgetWidget::WPValuesChanged(WayPointItem *waypoint) -{ -// qDebug("opmap: WPValuesChanged"); - - switch (m_map_mode) - { - case Normal_MapMode: - m_waypoint_list_mutex.lock(); - foreach (t_waypoint *wp, m_waypoint_list) - { // search for the waypoint in our own waypoint list and update it - if (!wp) continue; - if (!wp->map_wp_item) continue; - if (wp->map_wp_item != waypoint) continue; - // found the waypoint in our list - wp->coord = waypoint->Coord(); - wp->altitude = waypoint->Altitude(); - wp->description = waypoint->Description(); - break; - } - m_waypoint_list_mutex.unlock(); - break; - - case MagicWaypoint_MapMode: - // update our copy of the magic waypoint - if (m_magic_waypoint.map_wp_item && m_magic_waypoint.map_wp_item == waypoint) - { - m_magic_waypoint.coord = waypoint->Coord(); - m_magic_waypoint.altitude = waypoint->Altitude(); - m_magic_waypoint.description = waypoint->Description(); - - // move the UAV to the magic waypoint position - // moveToMagicWaypointPosition(); - } - break; - } - -} - /** TODO: slot to do something upon Waypoint insertion */ @@ -1227,6 +1050,27 @@ void OPMapGadgetWidget::setZoom(int zoom) m_map->SetMouseWheelZoomType(zoom_type); } +void OPMapGadgetWidget::setHomePosition(QPointF pos) +{ + if (!m_widget || !m_map) + return; + + double latitude = pos.y(); + double longitude = pos.x(); + + if (latitude != latitude || longitude != longitude) + return; // nan prevention + + if (latitude > 90) latitude = 90; + else + if (latitude < -90) latitude = -90; + + if (longitude > 180) longitude = 180; + else + if (longitude < -180) longitude = -180; + + m_map->Home->SetCoord(internals::PointLatLng(latitude, longitude)); +} void OPMapGadgetWidget::setPosition(QPointF pos) { @@ -1299,21 +1143,12 @@ void OPMapGadgetWidget::setCacheLocation(QString cacheLocation) if (cacheLocation.isEmpty()) return; -// #if defined(Q_WS_WIN) -// if (!cacheLocation.endsWith('\\')) cacheLocation += '\\'; -// #elif defined(Q_WS_X11) - if (!cacheLocation.endsWith(QDir::separator())) cacheLocation += QDir::separator(); -// #elif defined(Q_WS_MAC) -// if (!cacheLocation.endsWith(QDir::separator())) cacheLocation += QDir::separator(); -// #endif + if (!cacheLocation.endsWith(QDir::separator())) cacheLocation += QDir::separator(); QDir dir; if (!dir.exists(cacheLocation)) if (!dir.mkpath(cacheLocation)) return; - -// qDebug() << "opmap: map cache dir: " << cacheLocation; - m_map->configuration->SetCacheLocation(cacheLocation); } @@ -1351,32 +1186,8 @@ void OPMapGadgetWidget::setMapMode(opMapModeType mode) hideMagicWaypointControls(); - // delete the magic waypoint from the map - if (m_magic_waypoint.map_wp_item) - { - m_magic_waypoint.coord = m_magic_waypoint.map_wp_item->Coord(); - m_magic_waypoint.altitude = m_magic_waypoint.map_wp_item->Altitude(); - m_magic_waypoint.description = m_magic_waypoint.map_wp_item->Description(); - m_magic_waypoint.map_wp_item = NULL; - } - m_map->WPDeleteAll(); - - // restore the normal waypoints on the map - m_waypoint_list_mutex.lock(); - foreach (t_waypoint *wp, m_waypoint_list) - { - if (!wp) continue; - wp->map_wp_item = m_map->WPCreate(wp->coord, wp->altitude, wp->description); - if (!wp->map_wp_item) continue; - wp->map_wp_item->setZValue(10 + wp->map_wp_item->Number()); - wp->map_wp_item->setFlag(QGraphicsItem::ItemIsMovable, !wp->locked); - if (!wp->locked) - wp->map_wp_item->picture.load(QString::fromUtf8(":/opmap/images/waypoint_marker1.png")); - else - wp->map_wp_item->picture.load(QString::fromUtf8(":/opmap/images/waypoint_marker2.png")); - wp->map_wp_item->update(); - } - m_waypoint_list_mutex.unlock(); + magicWayPoint->setVisible(false); + m_map->WPSetVisibleAll(true); break; @@ -1389,25 +1200,9 @@ void OPMapGadgetWidget::setMapMode(opMapModeType mode) showMagicWaypointControls(); // delete the normal waypoints from the map - m_waypoint_list_mutex.lock(); - foreach (t_waypoint *wp, m_waypoint_list) - { - if (!wp) continue; - if (!wp->map_wp_item) continue; - wp->coord = wp->map_wp_item->Coord(); - wp->altitude = wp->map_wp_item->Altitude(); - wp->description = wp->map_wp_item->Description(); - wp->locked = (wp->map_wp_item->flags() & QGraphicsItem::ItemIsMovable) == 0; - wp->map_wp_item = NULL; - } - m_map->WPDeleteAll(); - m_waypoint_list_mutex.unlock(); - // restore the magic waypoint on the map - m_magic_waypoint.map_wp_item = m_map->WPCreate(m_magic_waypoint.coord, m_magic_waypoint.altitude, m_magic_waypoint.description); - m_magic_waypoint.map_wp_item->setZValue(10 + m_magic_waypoint.map_wp_item->Number()); - m_magic_waypoint.map_wp_item->SetShowNumber(false); - m_magic_waypoint.map_wp_item->picture.load(QString::fromUtf8(":/opmap/images/waypoint_marker3.png")); + m_map->WPSetVisibleAll(false); + magicWayPoint->setVisible(true); break; } @@ -1436,7 +1231,7 @@ void OPMapGadgetWidget::createActions() reloadAct->setShortcut(tr("F5")); reloadAct->setStatusTip(tr("Reload the map tiles")); connect(reloadAct, SIGNAL(triggered()), this, SLOT(onReloadAct_triggered())); - + this->addAction(reloadAct); ripAct = new QAction(tr("&Rip map"), this); ripAct->setStatusTip(tr("Rip the map tiles")); connect(ripAct, SIGNAL(triggered()), this, SLOT(onRipAct_triggered())); @@ -1477,15 +1272,21 @@ void OPMapGadgetWidget::createActions() showUAVAct->setChecked(true); connect(showUAVAct, SIGNAL(toggled(bool)), this, SLOT(onShowUAVAct_toggled(bool))); + changeDefaultLocalAndZoom = new QAction(tr("Set default zoom and location"), this); + changeDefaultLocalAndZoom->setStatusTip(tr("Changes the map default zoom and location to the current values")); + connect(changeDefaultLocalAndZoom, SIGNAL(triggered()), this, SLOT(onChangeDefaultLocalAndZoom())); + zoomInAct = new QAction(tr("Zoom &In"), this); zoomInAct->setShortcut(Qt::Key_PageUp); zoomInAct->setStatusTip(tr("Zoom the map in")); connect(zoomInAct, SIGNAL(triggered()), this, SLOT(onGoZoomInAct_triggered())); + this->addAction(zoomInAct); zoomOutAct = new QAction(tr("Zoom &Out"), this); zoomOutAct->setShortcut(Qt::Key_PageDown); zoomOutAct->setStatusTip(tr("Zoom the map out")); connect(zoomOutAct, SIGNAL(triggered()), this, SLOT(onGoZoomOutAct_triggered())); + this->addAction(zoomOutAct); goMouseClickAct = new QAction(tr("Go to where you right clicked the mouse"), this); goMouseClickAct->setStatusTip(tr("Center the map onto where you right clicked the mouse")); @@ -1537,6 +1338,7 @@ void OPMapGadgetWidget::createActions() addWayPointAct->setShortcut(tr("Ctrl+A")); addWayPointAct->setStatusTip(tr("Add waypoint")); connect(addWayPointAct, SIGNAL(triggered()), this, SLOT(onAddWayPointAct_triggered())); + this->addAction(addWayPointAct); editWayPointAct = new QAction(tr("&Edit waypoint"), this); editWayPointAct->setShortcut(tr("Ctrl+E")); @@ -1814,7 +1616,12 @@ void OPMapGadgetWidget::onMaxUpdateRateActGroup_triggered(QAction *action) if (!m_widget || !m_map || !action) return; - setMaxUpdateRate(action->data().toInt()); + setMaxUpdateRate(action->data().toInt()); +} + +void OPMapGadgetWidget::onChangeDefaultLocalAndZoom() +{ + emit defaultLocationAndZoomChanged(m_map->CurrentPosition().Lng(),m_map->CurrentPosition().Lat(),m_map->ZoomTotal()); } void OPMapGadgetWidget::onGoMouseClickAct_triggered() @@ -1940,36 +1747,20 @@ void OPMapGadgetWidget::onAddWayPointAct_triggered() if (m_map_mode != Normal_MapMode) return; - m_waypoint_list_mutex.lock(); + // m_waypoint_list_mutex.lock(); // create a waypoint on the map at the last known mouse position - t_waypoint *wp = new t_waypoint; - wp->map_wp_item = NULL; - wp->coord = m_context_menu_lat_lon; - wp->altitude = 0; - wp->description = ""; - wp->locked = false; - wp->time_seconds = 0; - wp->hold_time_seconds = 0; - wp->map_wp_item = m_map->WPCreate(wp->coord, wp->altitude, wp->description); + internals::PointLatLng coord; + if(this->contextMenu.isVisible()) + coord = m_context_menu_lat_lon; + else + coord=lastLatLngMouse; + m_map->WPCreate(coord, 0, ""); - wp->map_wp_item->setZValue(10 + wp->map_wp_item->Number()); - wp->map_wp_item->setFlag(QGraphicsItem::ItemIsMovable, !wp->locked); + //wp->map_wp_item->picture.load(QString::fromUtf8(":/opmap/images/waypoint_marker1.png")); + //wp->map_wp_item->picture.load(QString::fromUtf8(":/opmap/images/waypoint_marker2.png")); - if (wp->map_wp_item) - { - if (!wp->locked) - wp->map_wp_item->picture.load(QString::fromUtf8(":/opmap/images/waypoint_marker1.png")); - else - wp->map_wp_item->picture.load(QString::fromUtf8(":/opmap/images/waypoint_marker2.png")); - wp->map_wp_item->update(); - } - - // and remember it in our own local waypoint list - m_waypoint_list.append(wp); - - m_waypoint_list_mutex.unlock(); } @@ -2021,11 +1812,6 @@ void OPMapGadgetWidget::onLockWayPointAct_triggered() m_mouse_waypoint = NULL; } - -/** - * TODO: unused for v1.0 - */ - void OPMapGadgetWidget::onDeleteWayPointAct_triggered() { if (!m_widget || !m_map) @@ -2037,51 +1823,10 @@ void OPMapGadgetWidget::onDeleteWayPointAct_triggered() if (!m_mouse_waypoint) return; - bool locked = (m_mouse_waypoint->flags() & QGraphicsItem::ItemIsMovable) == 0; - - if (locked) return; // waypoint is locked - - QMutexLocker locker(&m_waypoint_list_mutex); - - for (int i = 0; i < m_waypoint_list.count(); i++) - { - t_waypoint *wp = m_waypoint_list.at(i); - if (!wp) continue; - if (!wp->map_wp_item || wp->map_wp_item != m_mouse_waypoint) continue; - // delete the waypoint from the map - m_map->WPDelete(wp->map_wp_item); - - // delete the waypoint from our local waypoint list - m_waypoint_list.removeAt(i); - - delete wp; - - break; - } -// -// foreach (t_waypoint *wp, m_waypoint_list) -// { -// if (!wp) continue; -// if (!wp->map_wp_item || wp->map_wp_item != m_mouse_waypoint) continue; -// -// // delete the waypoint from the map -// m_map->WPDelete(wp->map_wp_item); -// -// // delete the waypoint from our local waypoint list -// m_waypoint_list.removeOne(wp); -// -// delete wp; -// -// break; -// } - - m_mouse_waypoint = NULL; + m_map->WPDelete(m_mouse_waypoint); } -/** - * TODO: No Waypoint support in v1.0 - */ void OPMapGadgetWidget::onClearWayPointsAct_triggered() { if (!m_widget || !m_map) @@ -2090,21 +1835,9 @@ void OPMapGadgetWidget::onClearWayPointsAct_triggered() if (m_map_mode != Normal_MapMode) return; - QMutexLocker locker(&m_waypoint_list_mutex); - m_map->WPDeleteAll(); - foreach (t_waypoint *wp, m_waypoint_list) - { - if (wp) - { - delete wp; - wp = NULL; - } - } - - m_waypoint_list.clear(); -} + } void OPMapGadgetWidget::onHomeMagicWaypointAct_triggered() @@ -2147,10 +1880,7 @@ void OPMapGadgetWidget::homeMagicWaypoint() if (m_map_mode != MagicWaypoint_MapMode) return; - m_magic_waypoint.coord = m_home_position.coord; - - if (m_magic_waypoint.map_wp_item) - m_magic_waypoint.map_wp_item->SetCoord(m_magic_waypoint.coord); + magicWayPoint->SetCoord(m_home_position.coord); } // ************************************************************************************* @@ -2163,60 +1893,6 @@ void OPMapGadgetWidget::moveToMagicWaypointPosition() if (m_map_mode != MagicWaypoint_MapMode) return; - -// internals::PointLatLng coord = magic_waypoint.coord; -// double altitude = magic_waypoint.altitude; - - - // ToDo: - -} - -// ************************************************************************************* -// temporary until an object is created for managing the save/restore - -// load the contents of a simple text file into a combobox -void OPMapGadgetWidget::loadComboBoxLines(QComboBox *comboBox, QString filename) -{ - if (!comboBox) return; - if (filename.isNull() || filename.isEmpty()) return; - - QFile file(filename); - if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) - return; - - QTextStream in(&file); - - while (!in.atEnd()) - { - QString line = in.readLine().simplified(); - if (line.isNull() || line.isEmpty()) continue; - comboBox->addItem(line); - } - - file.close(); -} - -// save a combobox text contents to a simple text file -void OPMapGadgetWidget::saveComboBoxLines(QComboBox *comboBox, QString filename) -{ - if (!comboBox) return; - if (filename.isNull() || filename.isEmpty()) return; - - QFile file(filename); - if (!file.open(QIODevice::WriteOnly | QIODevice::Text)) - return; - - QTextStream out(&file); - - for (int i = 0; i < comboBox->count(); i++) - { - QString line = comboBox->itemText(i).simplified(); - if (line.isNull() || line.isEmpty()) continue; - out << line << "\n"; - } - - file.close(); } // ************************************************************************************* @@ -2248,25 +1924,20 @@ void OPMapGadgetWidget::keepMagicWaypointWithInSafeArea() { // calcute the bearing and distance from the home position to the magic waypoint - double dist = distance(m_home_position.coord, m_magic_waypoint.coord); - double bear = bearing(m_home_position.coord, m_magic_waypoint.coord); + double dist = distance(m_home_position.coord, magicWayPoint->Coord()); + double bear = bearing(m_home_position.coord, magicWayPoint->Coord()); // get the maximum safe distance - in kilometers double boundry_dist = (double)m_map->Home->SafeArea() / 1000; -// if (dist <= boundry_dist) -// return; // the magic waypoint is still within the safe area, don't move it - if (dist > boundry_dist) dist = boundry_dist; - // move the magic waypoint - - m_magic_waypoint.coord = destPoint(m_home_position.coord, bear, dist); + // move the magic waypoint; if (m_map_mode == MagicWaypoint_MapMode) { // move the on-screen waypoint - if (m_magic_waypoint.map_wp_item) - m_magic_waypoint.map_wp_item->SetCoord(m_magic_waypoint.coord); + if (magicWayPoint) + magicWayPoint->SetCoord(destPoint(m_home_position.coord, bear, dist)); } } @@ -2281,22 +1952,6 @@ double OPMapGadgetWidget::distance(internals::PointLatLng from, internals::Point double lat2 = to.Lat() * deg_to_rad; double lon2 = to.Lng() * deg_to_rad; - // *********************** - // Haversine formula -/* - double delta_lat = lat2 - lat1; - double delta_lon = lon2 - lon1; - - double t1 = sin(delta_lat / 2); - double t2 = sin(delta_lon / 2); - double a = (t1 * t1) + cos(lat1) * cos(lat2) * (t2 * t2); - double c = 2 * atan2(sqrt(a), sqrt(1 - a)); - - return (earth_mean_radius * c); -*/ - // *********************** - // Spherical Law of Cosines - return (acos(sin(lat1) * sin(lat2) + cos(lat1) * cos(lat2) * cos(lon2 - lon1)) * earth_mean_radius); // *********************** diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.h b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.h index b6074bb8c..50441b837 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.h @@ -74,18 +74,6 @@ typedef struct t_home bool locked; } t_home; -// local waypoint list item structure -typedef struct t_waypoint -{ - mapcontrol::WayPointItem *map_wp_item; - internals::PointLatLng coord; - double altitude; - QString description; - bool locked; - int time_seconds; - int hold_time_seconds; -} t_waypoint; - // ****************************************************** enum opMapModeType { Normal_MapMode = 0, @@ -120,7 +108,9 @@ public: void setMapMode(opMapModeType mode); void SetUavPic(QString UAVPic); void setMaxUpdateRate(int update_rate); - + void setHomePosition(QPointF pos); +signals: + void defaultLocationAndZoomChanged(double lng,double lat,double zoom); public slots: void homePositionUpdated(UAVObject *); @@ -131,7 +121,6 @@ protected: void resizeEvent(QResizeEvent *event); void mouseMoveEvent(QMouseEvent *event); void contextMenuEvent(QContextMenuEvent *event); - void keyPressEvent(QKeyEvent* event); private slots: void wpDoubleClickEvent(WayPointItem *wp); void updatePosition(); @@ -174,7 +163,6 @@ private slots: * Unused for now, hooks for future waypoint support */ void WPNumberChanged(int const& oldnumber,int const& newnumber, WayPointItem* waypoint); - void WPValuesChanged(WayPointItem* waypoint); void WPInserted(int const& number, WayPointItem* waypoint); void WPDeleted(int const& number, WayPointItem* waypoint); @@ -218,7 +206,7 @@ private slots: void onUAVTrailTimeActGroup_triggered(QAction *action); void onUAVTrailDistanceActGroup_triggered(QAction *action); void onMaxUpdateRateActGroup_triggered(QAction *action); - + void onChangeDefaultLocalAndZoom(); void on_tbFind_clicked(); private: @@ -241,8 +229,6 @@ private: t_home m_home_position; - t_waypoint m_magic_waypoint; - QStringList findPlaceWordList; QCompleter *findPlaceCompleter; @@ -265,9 +251,6 @@ private: mapcontrol::WayPointItem *m_mouse_waypoint; - QList m_waypoint_list; - QMutex m_waypoint_list_mutex; - QMutex m_map_mutex; bool m_telemetry_connected; @@ -306,6 +289,7 @@ private: QAction *homeMagicWaypointAct; QAction *showSafeAreaAct; + QAction *changeDefaultLocalAndZoom; QActionGroup *safeAreaActGroup; QList safeAreaAct; @@ -334,9 +318,6 @@ private: void moveToMagicWaypointPosition(); - void loadComboBoxLines(QComboBox *comboBox, QString filename); - void saveComboBoxLines(QComboBox *comboBox, QString filename); - void hideMagicWaypointControls(); void showMagicWaypointControls(); @@ -353,6 +334,9 @@ private: void setMapFollowingMode(); bool setHomeLocationObject(); + QMenu contextMenu; + internals::PointLatLng lastLatLngMouse; + WayPointItem * magicWayPoint; }; #endif /* OPMAP_GADGETWIDGET_H_ */ diff --git a/ground/openpilotgcs/src/plugins/opmap/pathplanmanager.cpp b/ground/openpilotgcs/src/plugins/opmap/pathplanmanager.cpp index 8aa91e853..c4ac4f8e1 100644 --- a/ground/openpilotgcs/src/plugins/opmap/pathplanmanager.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/pathplanmanager.cpp @@ -31,9 +31,9 @@ pathPlanManager::pathPlanManager(QWidget *parent,OPMapWidget *map): QDialog(parent, Qt::Dialog),myMap(map), ui(new Ui::pathPlanManager) { - waypoints=new QList(); + waypoints=new QList >(); ui->setupUi(this); - connect(myMap,SIGNAL(WPDeleted(int,WayPointItem*)),this,SLOT(on_WPDeleted(int,WayPointItem*))); + connect(myMap,SIGNAL(WPDeleted(int,WayPointItem*)),this,SLOT(on_WPDeleted(int,WayPointItem*)),Qt::DirectConnection); connect(myMap,SIGNAL(WPInserted(int,WayPointItem*)),this,SLOT(on_WPInserted(int,WayPointItem*))); connect(myMap,SIGNAL(WPCreated(int,WayPointItem*)),this,SLOT(on_WPInserted(int,WayPointItem*))); connect(myMap,SIGNAL(WPNumberChanged(int,int,WayPointItem*)),this,SLOT(refreshOverlays())); @@ -46,13 +46,20 @@ pathPlanManager::~pathPlanManager() } void pathPlanManager::on_WPDeleted(int wp_numberint,WayPointItem * wp) { + QMutexLocker locker(&wplistmutex); + if(wp_numberint<0) + return; waypoints->removeOne(wp); } void pathPlanManager::on_WPInserted(int wp_number, WayPointItem * wp) { - qDebug()<<"pathplanner waypoint added"; + if(waypoints->contains(wp)) + return; + wplistmutex.lock(); waypoints->append(wp); + wplistmutex.unlock(); + wp->setWPType(WayPointItem::relative); customData data; data.mode=PathAction::MODE_FLYENDPOINT; data.condition=PathAction::ENDCONDITION_NONE; @@ -61,24 +68,16 @@ void pathPlanManager::on_WPInserted(int wp_number, WayPointItem * wp) refreshOverlays(); } -void pathPlanManager::on_WPNumberChanged(int oldNumber, int newNumber, WayPointItem * wp) -{ -} - void pathPlanManager::on_WPValuesChanged(WayPointItem * wp) { } -//typedef enum { MODE_FLYENDPOINT=0, MODE_FLYVECTOR=1, MODE_FLYCIRCLERIGHT=2, -//MODE_FLYCIRCLELEFT=3, MODE_DRIVEENDPOINT=4, MODE_DRIVEVECTOR=5, MODE_DRIVECIRCLELEFT=6, -//MODE_DRIVECIRCLERIGHT=7, MODE_FIXEDATTITUDE=8, MODE_SETACCESSORY=9, MODE_DISARMALARM=10 } ModeOptions; void pathPlanManager::refreshOverlays() { + QMutexLocker locker(&wplistmutex); myMap->deleteAllOverlays(); - qDebug()<<"foreach start"; foreach(WayPointItem * wp,*waypoints) { - qDebug()<<"wp:"<Number(); customData data=wp->customData().value(); switch(data.mode) { @@ -86,7 +85,6 @@ void pathPlanManager::refreshOverlays() case PathAction::MODE_FLYVECTOR: case PathAction::MODE_DRIVEENDPOINT: case PathAction::MODE_DRIVEVECTOR: - qDebug()<<"addline"; if(wp->Number()==0) myMap->WPLineCreate((HomeItem*)myMap->Home,wp); else @@ -105,11 +103,12 @@ void pathPlanManager::refreshOverlays() } } - qDebug()<<"foreach end"; } WayPointItem * pathPlanManager::findWayPointNumber(int number) { + if(number<0) + return NULL; foreach(WayPointItem * wp,*waypoints) { if(wp->Number()==number) diff --git a/ground/openpilotgcs/src/plugins/opmap/pathplanmanager.h b/ground/openpilotgcs/src/plugins/opmap/pathplanmanager.h index 4f58b25e6..0868d44eb 100644 --- a/ground/openpilotgcs/src/plugins/opmap/pathplanmanager.h +++ b/ground/openpilotgcs/src/plugins/opmap/pathplanmanager.h @@ -32,6 +32,8 @@ #include "opmapcontrol/opmapcontrol.h" #include "pathaction.h" #include "waypoint.h" +#include "QMutexLocker" +#include "QPointer" namespace mapcontrol { struct customData @@ -63,12 +65,12 @@ private slots: void refreshOverlays(); void on_WPDeleted(int wp_numberint, WayPointItem *); void on_WPInserted(int,WayPointItem*); - void on_WPNumberChanged(int,int,WayPointItem*); void on_WPValuesChanged(WayPointItem*); private: Ui::pathPlanManager *ui; OPMapWidget * myMap; - QList * waypoints; + QList > * waypoints; + QMutex wplistmutex; }; #endif // PATHPLANMANAGER_H