diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.cpp index 4186f73e8..77193e5fd 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.cpp @@ -104,11 +104,11 @@ namespace mapcontrol GPS->SetUavPic(UAVPic); } - WayPointLine * OPMapWidget::WPLineCreate(WayPointItem *from, WayPointItem *to) + WayPointLine * OPMapWidget::WPLineCreate(WayPointItem *from, WayPointItem *to,QColor color) { if(!from|!to) return NULL; - return new WayPointLine(from,to,map); + return new WayPointLine(from,to,map,color); } WayPointLine * OPMapWidget::WPLineCreate(HomeItem *from, WayPointItem *to) { @@ -116,11 +116,11 @@ namespace mapcontrol return NULL; return new WayPointLine(from,to,map); } - WayPointCircle * OPMapWidget::WPCircleCreate(WayPointItem *center, WayPointItem *radius, bool clockwise) + WayPointCircle * OPMapWidget::WPCircleCreate(WayPointItem *center, WayPointItem *radius, bool clockwise,QColor color) { if(!center|!radius) return NULL; - return new WayPointCircle(center,radius,clockwise,map); + return new WayPointCircle(center,radius,clockwise,map,color); } WayPointCircle *OPMapWidget::WPCircleCreate(HomeItem *center, WayPointItem *radius, bool clockwise) @@ -290,11 +290,22 @@ namespace mapcontrol } WayPointItem* OPMapWidget::WPInsert(internals::PointLatLng const& coord,int const& altitude, QString const& description,const int &position) { - WayPointItem* item=new WayPointItem(coord,altitude,description,map); + internals::PointLatLng mcoord; + bool reloc=false; + if(mcoord==internals::PointLatLng(0,0)) + { + mcoord=CurrentPosition(); + reloc=true; + } + else + mcoord=coord; + WayPointItem* item=new WayPointItem(mcoord,altitude,description,map); item->SetNumber(position); ConnectWP(item); item->setParentItem(map); emit WPInserted(position,item); + if(reloc) + emit WPValuesChanged(item); return item; } void OPMapWidget::WPDelete(WayPointItem *item) diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.h index b98b87ae1..862c1187e 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.h @@ -361,9 +361,9 @@ namespace mapcontrol bool ShowHome()const{return showhome;} void SetShowDiagnostics(bool const& value); void SetUavPic(QString UAVPic); - WayPointLine * WPLineCreate(WayPointItem *from,WayPointItem *to); + WayPointLine * WPLineCreate(WayPointItem *from,WayPointItem *to, QColor color); WayPointLine * WPLineCreate(HomeItem *from,WayPointItem *to); - WayPointCircle *WPCircleCreate(WayPointItem *center, WayPointItem *radius,bool clockwise); + WayPointCircle *WPCircleCreate(WayPointItem *center, WayPointItem *radius,bool clockwise,QColor color); WayPointCircle *WPCircleCreate(HomeItem *center, WayPointItem *radius,bool clockwise); void deleteAllOverlays(); void WPSetVisibleAll(bool value); diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointcircle.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointcircle.cpp index 751d0788a..9bd99d407 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointcircle.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointcircle.cpp @@ -34,8 +34,9 @@ namespace mapcontrol WayPointCircle::WayPointCircle(WayPointItem *center, WayPointItem *radius,bool clockwise, MapGraphicItem *map,QColor color):my_center(center), my_radius(radius),my_map(map),QGraphicsEllipseItem(map),myColor(color),myClockWise(clockwise) { - connect(center,SIGNAL(localPositionChanged(QPointF)),this,SLOT(refreshLocations())); - connect(radius,SIGNAL(localPositionChanged(QPointF)),this,SLOT(refreshLocations())); + qDebug()<<"circle clock:"<setLine(to->pos().x(),to->pos().y(),from->pos().x(),from->pos().y()); - connect(from,SIGNAL(localPositionChanged(QPointF)),this,SLOT(refreshLocations())); - connect(to,SIGNAL(localPositionChanged(QPointF)),this,SLOT(refreshLocations())); + connect(from,SIGNAL(localPositionChanged(QPointF,WayPointItem*)),this,SLOT(refreshLocations())); + connect(to,SIGNAL(localPositionChanged(QPointF,WayPointItem*)),this,SLOT(refreshLocations())); connect(from,SIGNAL(aboutToBeDeleted(WayPointItem*)),this,SLOT(waypointdeleted())); connect(to,SIGNAL(aboutToBeDeleted(WayPointItem*)),this,SLOT(waypointdeleted())); } diff --git a/ground/openpilotgcs/src/plugins/opmap/flightdatamodel.cpp b/ground/openpilotgcs/src/plugins/opmap/flightdatamodel.cpp index a8c6508fa..afb250389 100644 --- a/ground/openpilotgcs/src/plugins/opmap/flightdatamodel.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/flightdatamodel.cpp @@ -22,7 +22,7 @@ QVariant flightDataModel::data(const QModelIndex &index, int role) const { int rowNumber=index.row(); int columnNumber=index.column(); - if(rowNumber>dataStorage.length()-1) + if(rowNumber>dataStorage.length()-1 || rowNumber<0) return QVariant(); pathPlanData * myRow=dataStorage.at(rowNumber); QVariant ret=getColumnByIndex(myRow,columnNumber); diff --git a/ground/openpilotgcs/src/plugins/opmap/modelmapproxy.cpp b/ground/openpilotgcs/src/plugins/opmap/modelmapproxy.cpp index edb2a8052..d4c44031e 100644 --- a/ground/openpilotgcs/src/plugins/opmap/modelmapproxy.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/modelmapproxy.cpp @@ -80,6 +80,51 @@ void modelMapProxy::on_selectedWPChanged(QList list) selection->setCurrentIndex(index,QItemSelectionModel::Select | QItemSelectionModel::Rows); } } + +modelMapProxy::overlayType modelMapProxy::overlayTranslate(int type) +{ + switch(type) + { + case ComboBoxDelegate::MODE_FLYENDPOINT: + case ComboBoxDelegate::MODE_FLYVECTOR: + case ComboBoxDelegate::MODE_DRIVEENDPOINT: + case ComboBoxDelegate::MODE_DRIVEVECTOR: + return OVERLAY_LINE; + break; + case ComboBoxDelegate::MODE_FLYCIRCLERIGHT: + case ComboBoxDelegate::MODE_DRIVECIRCLERIGHT: + return OVERLAY_CIRCLE_RIGHT; + break; + case ComboBoxDelegate::MODE_FLYCIRCLELEFT: + case ComboBoxDelegate::MODE_DRIVECIRCLELEFT: + return OVERLAY_CIRCLE_LEFT; + break; + default: + break; + } +} + +void modelMapProxy::createOverlay(WayPointItem *from, WayPointItem *to, modelMapProxy::overlayType type,QColor color) +{ + if(from==NULL || to==NULL || from==to) + return; + switch(type) + { + case OVERLAY_LINE: + myMap->WPLineCreate(from,to,color); + break; + case OVERLAY_CIRCLE_RIGHT: + myMap->WPCircleCreate(to,from,true,color); + break; + case OVERLAY_CIRCLE_LEFT: + myMap->WPCircleCreate(to,from,false,color); + break; + default: + break; + + } +} + /* 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, @@ -93,49 +138,61 @@ typedef enum { COMMAND_ONCONDITIONNEXTWAYPOINT=0, COMMAND_ONNOTCONDITIONNEXTWAYP */ void modelMapProxy::refreshOverlays() { - /* - QMutexLocker locker(&wplistmutex); + qDebug()<<"REFRESH OVERLAYS START"; myMap->deleteAllOverlays(); - foreach(WayPointItem * wp,*waypoints) + WayPointItem * wp_current=NULL; + WayPointItem * wp_next=NULL; + int wp_jump; + int wp_error; + overlayType wp_next_overlay; + overlayType wp_jump_overlay; + overlayType wp_error_overlay; + for(int x=0;xrowCount();++x) { - customData data=wp->data(0).value(); - - switch(data.condition) + qDebug()<<"REFRESH OVERLAYS WP:"<data(model->index(x,flightDataModel::JUMPDESTINATION)).toInt(); + wp_error=model->data(model->index(x,flightDataModel::ERRORDESTINATION)).toInt(); + wp_next_overlay=overlayTranslate(model->data(model->index(x+1,flightDataModel::MODE)).toInt()); + wp_jump_overlay=overlayTranslate(model->data(model->index(wp_jump,flightDataModel::MODE)).toInt()); + wp_error_overlay=overlayTranslate(model->data(model->index(wp_error,flightDataModel::MODE)).toInt()); + createOverlay(wp_current,findWayPointNumber(wp_error),wp_error_overlay,Qt::red); + switch(model->data(model->index(x,flightDataModel::COMMAND)).toInt()) { - - } - - switch(data.mode) - { - case PathAction::MODE_FLYENDPOINT: - case PathAction::MODE_FLYVECTOR: - case PathAction::MODE_DRIVEENDPOINT: - case PathAction::MODE_DRIVEVECTOR: - if(wp->Number()==0) - myMap->WPLineCreate((HomeItem*)myMap->Home,wp); - else - myMap->WPLineCreate(findWayPointNumber(wp->Number()-1),wp); + case ComboBoxDelegate::COMMAND_ONCONDITIONNEXTWAYPOINT: + wp_next=findWayPointNumber(x+1); + createOverlay(wp_current,wp_next,wp_next_overlay,Qt::green); + qDebug()<<"REFRESH OVERLAYS"<<"AKI0"; break; - case PathAction::MODE_FLYCIRCLERIGHT: - case PathAction::MODE_DRIVECIRCLERIGHT: - if(wp->Number()==0) - myMap->WPCircleCreate((HomeItem*)myMap->Home,wp,true); - myMap->WPCircleCreate(findWayPointNumber(wp->Number()-1),wp,true); + case ComboBoxDelegate::COMMAND_ONCONDITIONJUMPWAYPOINT: + wp_next=findWayPointNumber(wp_jump); + createOverlay(wp_current,wp_next,wp_jump_overlay,Qt::green); + qDebug()<<"REFRESH OVERLAYS"<<"AKI1"; break; - case PathAction::MODE_FLYCIRCLELEFT: - case PathAction::MODE_DRIVECIRCLELEFT: - if(wp->Number()==0) - myMap->WPCircleCreate((HomeItem*)myMap->Home,wp,false); - myMap->WPCircleCreate(findWayPointNumber(wp->Number()-1),wp,false); + case ComboBoxDelegate::COMMAND_ONNOTCONDITIONJUMPWAYPOINT: + wp_next=findWayPointNumber(wp_jump); + createOverlay(wp_current,wp_next,wp_jump_overlay,Qt::yellow); + qDebug()<<"REFRESH OVERLAYS"<<"AKI2"; break; - default: + case ComboBoxDelegate::COMMAND_ONNOTCONDITIONNEXTWAYPOINT: + wp_next=findWayPointNumber(x+1); + createOverlay(wp_current,wp_next,wp_next_overlay,Qt::yellow); + qDebug()<<"REFRESH OVERLAYS"<<"AKI3"; + break; + case ComboBoxDelegate::COMMAND_IFCONDITIONJUMPWAYPOINTELSENEXTWAYPOINT: + wp_next=findWayPointNumber(wp_jump); + createOverlay(wp_current,wp_next,wp_jump_overlay,Qt::green); + wp_next=findWayPointNumber(x+1); + createOverlay(wp_current,wp_next,wp_next_overlay,Qt::green); + qDebug()<<"REFRESH OVERLAYS"<<"AKI4"; break; - } } - */ } + + + WayPointItem * modelMapProxy::findWayPointNumber(int number) { if(number<0) @@ -158,6 +215,7 @@ void modelMapProxy::on_rowsRemoved(const QModelIndex &parent, int first, int las { myMap->WPDelete(x); } + refreshOverlays(); } void modelMapProxy::on_dataChanged(const QModelIndex &topLeft, const QModelIndex &bottomRight) @@ -174,6 +232,13 @@ void modelMapProxy::on_dataChanged(const QModelIndex &topLeft, const QModelIndex QString desc; switch(topLeft.column()) { + case flightDataModel::COMMAND: + case flightDataModel::CONDITION: + case flightDataModel::JUMPDESTINATION: + case flightDataModel::ERRORDESTINATION: + case flightDataModel::MODE: + refreshOverlays(); + break; case flightDataModel::WPDESCRITPTION: index=model->index(x,flightDataModel::WPDESCRITPTION); desc=index.data(Qt::DisplayRole).toString(); @@ -250,6 +315,7 @@ void modelMapProxy::on_rowsInserted(const QModelIndex &parent, int first, int la if(relative) item->setWPType(mapcontrol::WayPointItem::relative); } + refreshOverlays(); } void modelMapProxy::deleteWayPoint(int number) { diff --git a/ground/openpilotgcs/src/plugins/opmap/modelmapproxy.h b/ground/openpilotgcs/src/plugins/opmap/modelmapproxy.h index ec7e1e434..ad49e414a 100644 --- a/ground/openpilotgcs/src/plugins/opmap/modelmapproxy.h +++ b/ground/openpilotgcs/src/plugins/opmap/modelmapproxy.h @@ -8,12 +8,13 @@ #include "QPointer" #include "flightdatamodel.h" #include - +#include using namespace mapcontrol; class modelMapProxy:public QObject { + typedef enum {OVERLAY_LINE,OVERLAY_CIRCLE_RIGHT,OVERLAY_CIRCLE_LEFT} overlayType; Q_OBJECT public: explicit modelMapProxy(QObject *parent,OPMapWidget * map,flightDataModel * model,QItemSelectionModel * selectionModel); @@ -32,6 +33,8 @@ private slots: void on_currentRowChanged(QModelIndex,QModelIndex); void on_selectedWPChanged(QList); private: + overlayType overlayTranslate(int type); + void createOverlay(WayPointItem * from,WayPointItem * to,overlayType type,QColor color); OPMapWidget * myMap; flightDataModel * model; void refreshOverlays(); diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapgadget.cpp b/ground/openpilotgcs/src/plugins/opmap/opmapgadget.cpp index 388fd57bb..192575a55 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmapgadget.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/opmapgadget.cpp @@ -52,16 +52,16 @@ void OPMapGadget::saveConfiguration(double lng,double lat,double zoom) void OPMapGadget::loadConfiguration(IUAVGadgetConfiguration *config) { m_config = qobject_cast(config); - 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()); + 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())); + } diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp index 6d105905c..f15ea146f 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp @@ -212,17 +212,23 @@ OPMapGadgetWidget::OPMapGadgetWidget(QWidget *parent) : QWidget(parent) m_map->UAV->SetUAVPos(m_home_position.coord, 0.0); // set the UAV position if(m_map->GPS) m_map->GPS->SetUAVPos(m_home_position.coord, 0.0); // set the UAV position - - +/* + qDebug()<<"here0"; model=new flightDataModel(this); + qDebug()<<"here1"; table=new pathPlanner(); + qDebug()<<"here2"; selectionModel=new QItemSelectionModel(model); + qDebug()<<"here3"; proxy=new modelMapProxy(this,m_map,model,selectionModel); + qDebug()<<"here4"; table->setModel(model,selectionModel); + qDebug()<<"here5"; table->show(); + qDebug()<<"here6"; waypoint_edit_dialog=new opmap_edit_waypoint_dialog(NULL,model,selectionModel); - - + qDebug()<<"here7"; +*/ /* distBearing db; db.distance=100;