From 754eaca9095091716a8b0fe946509d5daeeebf63 Mon Sep 17 00:00:00 2001 From: tecnosapiens Date: Tue, 7 Dec 2010 15:29:26 -0600 Subject: [PATCH 01/21] clean class SlugsVideoCamControl --- src/ui/SlugsVideoCamControl.cpp | 45 ++--------------------------------------- 1 file changed, 2 insertions(+), 43 deletions(-) diff --git a/src/ui/SlugsVideoCamControl.cpp b/src/ui/SlugsVideoCamControl.cpp index 30ea6cb..9f3ad7b 100644 --- a/src/ui/SlugsVideoCamControl.cpp +++ b/src/ui/SlugsVideoCamControl.cpp @@ -19,30 +19,11 @@ SlugsVideoCamControl::SlugsVideoCamControl(QWidget *parent) : QWidget(parent), - ui(new Ui::SlugsVideoCamControl), - dragging(0) - + ui(new Ui::SlugsVideoCamControl) { ui->setupUi(this); - x1= 0; - y1 = 0; - connect(ui->viewCamBordeatMap_checkBox,SIGNAL(clicked(bool)),this,SLOT(changeViewCamBorderAtMapStatus(bool))); -// tL = ui->padCamContro_frame->frameGeometry().topLeft(); -// bR = ui->padCamContro_frame->frameGeometry().bottomRight(); - //ui->padCamContro_frame->setVisible(true); - -// // create a layout for camera pad -// QGridLayout* padCameraLayout = new QGridLayout(this); -// padCameraLayout->setSpacing(2); -// padCameraLayout->setMargin(0); -// padCameraLayout->setAlignment(Qt::AlignTop); - - //ui->padCamContro_frame->setLayout(padCameraLayout); - // create a camera pad widget - //test = new QPushButton(QIcon(":/images/actions/list-add.svg"), "", this); - //test->setMaximumWidth(50); - //ui->gridLayout->addWidget(test, 0,0); + connect(ui->viewCamBordeatMap_checkBox,SIGNAL(clicked(bool)),this,SLOT(changeViewCamBorderAtMapStatus(bool))); padCamera = new SlugsPadCameraControl(this); ui->gridLayout->addWidget(padCamera); @@ -53,28 +34,6 @@ SlugsVideoCamControl::SlugsVideoCamControl(QWidget *parent) : connect(padCamera,SIGNAL(changeCursorPosition(double,double,QString)),this,SLOT(getDeltaPositionPad(double,double,QString))); - - //padCamera->setVisible(true); - - - - // padCameraLayout->addWidget(padCamera); - - - -// QGraphicsScene *scene = new QGraphicsScene(ui->CamControlPanel_graphicsView); -// scene->setItemIndexMethod(QGraphicsScene::NoIndex); -// scene->setSceneRect(-200, -200, 400, 400); -// setScene(scene); -// setCacheMode(CacheBackground); -// setViewportUpdateMode(BoundingRectViewportUpdate); -// setRenderHint(QPainter::Antialiasing); -// setTransformationAnchor(AnchorUnderMouse); -// setResizeAnchor(AnchorViewCenter); - -// ui->CamControlPanel_graphicsView->installEventFilter(this); -// ui->label_x->installEventFilter(this); - } SlugsVideoCamControl::~SlugsVideoCamControl() From 6bd785b5103a41d947c38fc86f6a5b32377b6c88 Mon Sep 17 00:00:00 2001 From: tecnosapiens Date: Tue, 7 Dec 2010 17:24:17 -0600 Subject: [PATCH 02/21] connect signal for get all values pid from UAS --- src/ui/SlugsPIDControl.cpp | 1 + src/ui/SlugsPadCameraControl.cpp | 30 +++++++++++++++++++++++------- src/ui/SlugsPadCameraControl.h | 1 + src/ui/WaypointList.cc | 6 +++--- 4 files changed, 28 insertions(+), 10 deletions(-) diff --git a/src/ui/SlugsPIDControl.cpp b/src/ui/SlugsPIDControl.cpp index 18fd516..08630c4 100644 --- a/src/ui/SlugsPIDControl.cpp +++ b/src/ui/SlugsPIDControl.cpp @@ -55,6 +55,7 @@ void SlugsPIDControl::activeUasSet(UASInterface* uas) connect(slugsMav,SIGNAL(slugsPidValues(int,mavlink_pid_t)),this, SLOT(receivePidValues(int,mavlink_pid_t)) ); connect(ui->setGeneral_pushButton,SIGNAL(clicked()),this,SLOT(slugsTimerStartSet())); + connect(ui->getGeneral_pushButton,SIGNAL(clicked()),this,SLOT(slugsTimerStartGet())); } #endif // MAVLINK_ENABLED_SLUG diff --git a/src/ui/SlugsPadCameraControl.cpp b/src/ui/SlugsPadCameraControl.cpp index 40b40c0..071585d 100644 --- a/src/ui/SlugsPadCameraControl.cpp +++ b/src/ui/SlugsPadCameraControl.cpp @@ -23,13 +23,25 @@ SlugsPadCameraControl::~SlugsPadCameraControl() void SlugsPadCameraControl::mouseMoveEvent(QMouseEvent *event) { - emit mouseMoveCoord(event->x(),event->y()); + //emit mouseMoveCoord(event->x(),event->y()); + if(dragging) + { + if(abs(x1-event->x())>20 || abs(y1-event->y())>20) + { + + getDeltaPositionPad(event->x(), event->y()); + x1 = event->x(); + y1 = event->y(); + } + } + } void SlugsPadCameraControl::mousePressEvent(QMouseEvent *event) { - emit mousePressCoord(event->x(),event->y()); + //emit mousePressCoord(event->x(),event->y()); + dragging = true; x1 = event->x(); y1 = event->y(); @@ -37,8 +49,10 @@ void SlugsPadCameraControl::mousePressEvent(QMouseEvent *event) void SlugsPadCameraControl::mouseReleaseEvent(QMouseEvent *event) { - emit mouseReleaseCoord(event->x(),event->y()); - getDeltaPositionPad(event->x(), event->y()); + dragging = false; + //emit mouseReleaseCoord(event->x(),event->y()); + //getDeltaPositionPad(event->x(), event->y()); + } @@ -92,7 +106,7 @@ void SlugsPadCameraControl::getDeltaPositionPad(int x2, int y2) { emit dirCursorText("right up"); //bearing = 315; - dir = "riht up"; + dir = "right up"; } else { @@ -100,7 +114,7 @@ void SlugsPadCameraControl::getDeltaPositionPad(int x2, int y2) { emit dirCursorText("right"); //bearing = 315; - dir = "riht"; + dir = "right"; } else { @@ -108,7 +122,7 @@ void SlugsPadCameraControl::getDeltaPositionPad(int x2, int y2) { emit dirCursorText("right down"); //bearing = 315; - dir = "riht down"; + dir = "right down"; } else { @@ -229,3 +243,5 @@ QPointF SlugsPadCameraControl::ObtenerMarcacionDistanciaPixel(double lon1, doubl return QPointF(marcacion,distancia); } + + diff --git a/src/ui/SlugsPadCameraControl.h b/src/ui/SlugsPadCameraControl.h index 06a9233..ca7c04a 100644 --- a/src/ui/SlugsPadCameraControl.h +++ b/src/ui/SlugsPadCameraControl.h @@ -22,6 +22,7 @@ public slots: double getDistPixel(int x1, int y1, int x2, int y2); QPointF ObtenerMarcacionDistanciaPixel(double lon1, double lat1, double lon2, double lat2); + signals: void mouseMoveCoord(int x, int y); void mousePressCoord(int x, int y); diff --git a/src/ui/WaypointList.cc b/src/ui/WaypointList.cc index 2138b46..a87888f 100644 --- a/src/ui/WaypointList.cc +++ b/src/ui/WaypointList.cc @@ -346,11 +346,11 @@ void WaypointList::waypointListChanged() WaypointGlobalView *wpgv = wpGlobalViews.value(wp); wpgv->updateValues(); listLayout->addWidget(wpgv); - if(loadFileGlobalWP || readGlobalWP) - { +// if(loadFileGlobalWP || readGlobalWP) +// { emit createWaypointAtMap(QPointF(wp->getX(),wp->getY())); qDebug()<<"Emitiendo Pos: "<getX()<<" - "<getY(); - } +// } } } From 20319cba3430f889751bf84ea61f417fbc63a84f Mon Sep 17 00:00:00 2001 From: tecnosapiens Date: Wed, 8 Dec 2010 17:09:51 -0600 Subject: [PATCH 03/21] draw camera orientation over mouse pad in SLugsPadCameraControl --- src/ui/SlugsPIDControl.cpp | 7 ++++--- src/ui/SlugsPIDControl.h | 4 ++-- src/ui/SlugsPIDControl.ui | 14 +++++++------- src/ui/SlugsPadCameraControl.cpp | 34 ++++++++++++++++++++++++++++++++ src/ui/SlugsPadCameraControl.h | 8 ++++++++ src/ui/SlugsVideoCamControl.cpp | 42 ++++++++++++++++++++-------------------- src/ui/SlugsVideoCamControl.h | 41 +++++++++++++++++++++++++++++---------- 7 files changed, 107 insertions(+), 43 deletions(-) diff --git a/src/ui/SlugsPIDControl.cpp b/src/ui/SlugsPIDControl.cpp index 08630c4..779754a 100644 --- a/src/ui/SlugsPIDControl.cpp +++ b/src/ui/SlugsPIDControl.cpp @@ -463,9 +463,9 @@ void SlugsPIDControl::changeColor_GREEN_Pitch2dT_groupBox() //create the packet pidMessage.target = activeUAS->getUASID(); pidMessage.idx = 8; - pidMessage.pVal = ui->dR_P_set->text().toFloat(); - pidMessage.iVal = ui->dR_I_set->text().toFloat(); - pidMessage.dVal = ui->dR_D_set->text().toFloat(); + pidMessage.pVal = ui->P2dT_FF_set->text().toFloat(); + pidMessage.iVal = 0;//ui->dR_I_set->text().toFloat(); + pidMessage.dVal = 0;//ui->dR_D_set->text().toFloat(); mavlink_message_t msg; @@ -532,6 +532,7 @@ void SlugsPIDControl::receivePidValues(int systemId, const mavlink_pid_t &pidVal break; case 8: ui->P2dT_FF_get->setText(QString::number(pidValues.pVal)); + break; default: diff --git a/src/ui/SlugsPIDControl.h b/src/ui/SlugsPIDControl.h index e6e818b..4a0ee96 100644 --- a/src/ui/SlugsPIDControl.h +++ b/src/ui/SlugsPIDControl.h @@ -281,10 +281,10 @@ private: mavlink_slugs_action_t actionSlugs; QTimer* refreshTimerSet; ///< The main timer, controls the update view - QTimer* refreshTimerGet; ///< The main timer, controls the update view + QTimer* refreshTimerGet; ///< The main timer, controls the update view int counterRefreshSet; int counterRefreshGet; - QMutex valuesMutex; + QMutex valuesMutex; }; #endif // SLUGSPIDCONTROL_H diff --git a/src/ui/SlugsPIDControl.ui b/src/ui/SlugsPIDControl.ui index 042f3d9..6de57a0 100644 --- a/src/ui/SlugsPIDControl.ui +++ b/src/ui/SlugsPIDControl.ui @@ -122,13 +122,6 @@ - - - Recepcion - - - - Qt::Vertical @@ -956,6 +949,13 @@ + + + + Recepcion + + + diff --git a/src/ui/SlugsPadCameraControl.cpp b/src/ui/SlugsPadCameraControl.cpp index 071585d..3e198c6 100644 --- a/src/ui/SlugsPadCameraControl.cpp +++ b/src/ui/SlugsPadCameraControl.cpp @@ -13,6 +13,9 @@ SlugsPadCameraControl::SlugsPadCameraControl(QWidget *parent) : ui->setupUi(this); x1= 0; y1 = 0; + bearingPad = 0; + distancePad = 0; + directionPad = "no"; } @@ -53,6 +56,9 @@ void SlugsPadCameraControl::mouseReleaseEvent(QMouseEvent *event) //emit mouseReleaseCoord(event->x(),event->y()); //getDeltaPositionPad(event->x(), event->y()); + xFin = event->x(); + yFin = event->y(); + } @@ -73,6 +79,13 @@ void SlugsPadCameraControl::paintEvent(QPaintEvent *pe) painter.drawLine(QPoint(ui->frame->geometry().topLeft().x(),ui->frame->height()/2), QPoint(ui->frame->geometry().bottomRight().x(),ui->frame->height()/2)); + painter.setPen(Qt::white); + + //QPointF coordTemp = getPointBy_BearingDistance(ui->frame->width()/2,ui->frame->height()/2,bearingPad,distancePad); + + painter.drawLine(QPoint(ui->frame->width()/2,ui->frame->height()/2), + QPoint(xFin,yFin)); + // painter.drawLine(QPoint()); //painter.drawLines(padLines); @@ -172,8 +185,15 @@ void SlugsPadCameraControl::getDeltaPositionPad(int x2, int y2) } + bearingPad = bearing; + distancePad = dist; + directionPad = dir; emit changeCursorPosition(bearing, dist, dir); + update(); + + + } double SlugsPadCameraControl::getDistPixel(int x1, int y1, int x2, int y2) @@ -245,3 +265,17 @@ QPointF SlugsPadCameraControl::ObtenerMarcacionDistanciaPixel(double lon1, doubl } + +QPointF SlugsPadCameraControl::getPointBy_BearingDistance(double lat1, double lon1, double rumbo, double distancia) +{ + double lon2 = 0; + double lat2 = 0; + double rad= M_PI/180; + + rumbo = rumbo*rad; + lon2=(lon1 + ((distancia/60) * (sin(rumbo)))); + lat2=(lat1 + ((distancia/60) * (cos(rumbo)))); + + return QPointF(lon2,lat2); +} + diff --git a/src/ui/SlugsPadCameraControl.h b/src/ui/SlugsPadCameraControl.h index ca7c04a..bb35976 100644 --- a/src/ui/SlugsPadCameraControl.h +++ b/src/ui/SlugsPadCameraControl.h @@ -21,6 +21,8 @@ public slots: void getDeltaPositionPad(int x, int y); double getDistPixel(int x1, int y1, int x2, int y2); QPointF ObtenerMarcacionDistanciaPixel(double lon1, double lat1, double lon2, double lat2); + QPointF getPointBy_BearingDistance(double lat1, double lon1, double rumbo, double distancia); + signals: @@ -37,11 +39,17 @@ protected: void mouseMoveEvent(QMouseEvent* event); void paintEvent(QPaintEvent *pe); + private: Ui::SlugsPadCameraControl *ui; bool dragging; int x1; int y1; + int xFin; + int yFin; + double bearingPad; + double distancePad; + QString directionPad; }; diff --git a/src/ui/SlugsVideoCamControl.cpp b/src/ui/SlugsVideoCamControl.cpp index 9f3ad7b..7e09175 100644 --- a/src/ui/SlugsVideoCamControl.cpp +++ b/src/ui/SlugsVideoCamControl.cpp @@ -41,41 +41,41 @@ SlugsVideoCamControl::~SlugsVideoCamControl() delete ui; } -void SlugsVideoCamControl::mouseMoveEvent(QMouseEvent *event) -{ - Q_UNUSED(event); +//void SlugsVideoCamControl::mouseMoveEvent(QMouseEvent *event) +//{ +// Q_UNUSED(event); -} +//} -void SlugsVideoCamControl::mousePressEvent(QMouseEvent *evnt) -{ - Q_UNUSED(evnt); +//void SlugsVideoCamControl::mousePressEvent(QMouseEvent *evnt) +//{ +// Q_UNUSED(evnt); -} +//} -void SlugsVideoCamControl::mouseReleaseEvent(QMouseEvent *evnt) -{ - Q_UNUSED(evnt); +//void SlugsVideoCamControl::mouseReleaseEvent(QMouseEvent *evnt) +//{ +// Q_UNUSED(evnt); -} +//} -void SlugsVideoCamControl::mousePadMoveEvent(int x, int y) -{ +//void SlugsVideoCamControl::mousePadMoveEvent(int x, int y) +//{ -} +//} -void SlugsVideoCamControl::mousePadPressEvent(int x, int y) -{ +//void SlugsVideoCamControl::mousePadPressEvent(int x, int y) +//{ -} +//} -void SlugsVideoCamControl::mousePadReleaseEvent(int x, int y) -{ +//void SlugsVideoCamControl::mousePadReleaseEvent(int x, int y) +//{ -} +//} void SlugsVideoCamControl::changeViewCamBorderAtMapStatus(bool status) { diff --git a/src/ui/SlugsVideoCamControl.h b/src/ui/SlugsVideoCamControl.h index 2580033..0180fd5 100644 --- a/src/ui/SlugsVideoCamControl.h +++ b/src/ui/SlugsVideoCamControl.h @@ -27,22 +27,43 @@ public: ~SlugsVideoCamControl(); public slots: + /** + * @brief status = true: emit signal to draw a border cam over the map + */ void changeViewCamBorderAtMapStatus(bool status); - void getDeltaPositionPad(double dir, double dist, QString dirText); - - - void mousePadPressEvent(int x, int y); - void mousePadReleaseEvent(int x, int y); - void mousePadMoveEvent(int x, int y); + /** + * @brief show the values of mousepad on ui (labels) and emit a changeCamPosition(signal) + * with values: + * bearing and distance from mouse over the pad + * dirText: direction of mouse movement in text format (up, right,right up,right down, + * left, left up, left down, down) + */ + void getDeltaPositionPad(double bearing, double distance, QString dirText); + +// /** +// * @brief +// */ +// void mousePadPressEvent(int x, int y); +// void mousePadReleaseEvent(int x, int y); +// void mousePadMoveEvent(int x, int y); signals: - void changeCamPosition(double dist, double dir, QString textDir); + /** + * @brief emit values from mousepad: + * bearing and distance from mouse over the pad + * dirText: direction of mouse movement in text format (up, right,right up,right down, + * left, left up, left down, down) + */ + void changeCamPosition(double distance, double bearing, QString textDir); + /** + * @brief emit signal to draw a border cam over the map if status is true + */ void viewCamBorderAtMap(bool status); protected: - void mousePressEvent(QMouseEvent* event); - void mouseReleaseEvent(QMouseEvent* event); - void mouseMoveEvent(QMouseEvent* event); +// void mousePressEvent(QMouseEvent* event); +// void mouseReleaseEvent(QMouseEvent* event); +// void mouseMoveEvent(QMouseEvent* event); From c3ebb99926c38777ca52ba5bbac5d0febf02a5c1 Mon Sep 17 00:00:00 2001 From: Mariano Lizarraga Date: Thu, 9 Dec 2010 11:39:01 -0600 Subject: [PATCH 04/21] Changed the position of the /head tag to eliminate warning in safari developer tools --- images/earth.html | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/images/earth.html b/images/earth.html index 8f8aadf..6f423cb 100644 --- a/images/earth.html +++ b/images/earth.html @@ -1,8 +1,9 @@ - + + QGroundControl Google Earth View From 1c1b5b6924b61d23ebde1afa262b2e0e4207d414 Mon Sep 17 00:00:00 2001 From: tecnosapiens Date: Fri, 10 Dec 2010 09:40:44 -0600 Subject: [PATCH 05/21] add new function to SlugsDataSensorView --- qgroundcontrol.pri | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/qgroundcontrol.pri b/qgroundcontrol.pri index 3d0c61e..c12ad7c 100644 --- a/qgroundcontrol.pri +++ b/qgroundcontrol.pri @@ -369,12 +369,12 @@ win32-g++ { BASEDIR_WIN = $$replace(BASEDIR,"/","\\") TARGETDIR_WIN = $$replace(TARGETDIR,"/","\\") - QMAKE_PRE_LINK += && copy /Y \"$$BASEDIR_WIN\lib\sdl\win32\SDL.dll\" \"$$TARGETDIR_WIN\debug\SDL.dll\" - QMAKE_PRE_LINK += && copy /Y \"$$BASEDIR_WIN\lib\sdl\win32\SDL.dll\" \"$$TARGETDIR_WIN\release\SDL.dll\" - QMAKE_PRE_LINK += && xcopy \"$$BASEDIR_WIN\audio\" \"$$TARGETDIR_WIN\debug\audio\" /S /E /Y - QMAKE_PRE_LINK += && xcopy \"$$BASEDIR_WIN\audio\" \"$$TARGETDIR_WIN\release\audio\" /S /E /Y - QMAKE_PRE_LINK += && xcopy \"$$BASEDIR_WIN\models\" \"$$TARGETDIR_WIN\debug\models\" /S /E /Y - QMAKE_PRE_LINK += && xcopy \"$$BASEDIR_WIN\models\" \"$$TARGETDIR_WIN\release\models\" /S /E /Y + #QMAKE_PRE_LINK += && copy /Y \"$$BASEDIR_WIN\lib\sdl\win32\SDL.dll\" \"$$TARGETDIR_WIN\debug\SDL.dll\" + #QMAKE_PRE_LINK += && copy /Y \"$$BASEDIR_WIN\lib\sdl\win32\SDL.dll\" \"$$TARGETDIR_WIN\release\SDL.dll\" + #QMAKE_PRE_LINK += && xcopy \"$$BASEDIR_WIN\audio\" \"$$TARGETDIR_WIN\debug\audio\" /S /E /Y + #QMAKE_PRE_LINK += && xcopy \"$$BASEDIR_WIN\audio\" \"$$TARGETDIR_WIN\release\audio\" /S /E /Y + #QMAKE_PRE_LINK += && xcopy \"$$BASEDIR_WIN\models\" \"$$TARGETDIR_WIN\debug\models\" /S /E /Y + #QMAKE_PRE_LINK += && xcopy \"$$BASEDIR_WIN\models\" \"$$TARGETDIR_WIN\release\models\" /S /E /Y # osg/osgEarth dynamic casts might fail without this compiler option. # see http://osgearth.org/wiki/FAQ for details. From b49e1e9d392fe98a72b0231b0a0fa7e440288904 Mon Sep 17 00:00:00 2001 From: tecnosapiens Date: Fri, 10 Dec 2010 10:32:42 -0600 Subject: [PATCH 06/21] ready to merge new brach architecture --- src/ui/SlugsDataSensorView.cc | 14 ++++++++++++++ src/ui/SlugsDataSensorView.h | 34 +++++++++++++++++++++------------- src/ui/SlugsDataSensorView.ui | 18 +++++++++--------- 3 files changed, 44 insertions(+), 22 deletions(-) diff --git a/src/ui/SlugsDataSensorView.cc b/src/ui/SlugsDataSensorView.cc index 749f2bd..f25a9ac 100644 --- a/src/ui/SlugsDataSensorView.cc +++ b/src/ui/SlugsDataSensorView.cc @@ -53,6 +53,7 @@ void SlugsDataSensorView::addUAS(UASInterface* uas) connect(slugsMav, SIGNAL(slugsPWM(int,const mavlink_pwm_commands_t&)),this,SLOT(slugsPWMChanged(int,const mavlink_pwm_commands_t&))); connect(slugsMav, SIGNAL(slugsFilteredData(int,const mavlink_filtered_data_t&)),this,SLOT(slugsFilteredDataChanged(int,const mavlink_filtered_data_t&))); connect(slugsMav, SIGNAL(slugsGPSDateTime(int,const mavlink_gps_date_time_t&)),this,SLOT(slugsGPSDateTimeChanged(int,const mavlink_gps_date_time_t&))); + //connect(slugsMav,SIGNAL(slugsAirData(int,mavlink_air_data_t&)),this,SLOT()) #endif // MAVLINK_ENABLED_SLUGS // Set this UAS as active if it is the first one @@ -148,6 +149,7 @@ void SlugsDataSensorView::slugsSensorBiasChanged(int systemId, } + void SlugsDataSensorView::slugsDiagnosticMessageChanged(int systemId, const mavlink_diagnostic_t& diagnostic){ Q_UNUSED(systemId); @@ -240,6 +242,18 @@ void SlugsDataSensorView::slugsGPSDateTimeChanged(int systemId, ui->m_GpsSat->setText(QString::number(gpsDateTime.visSat)); } +/** + * @brief Updates the air data widget - 171 +*/ +void SlugsDataSensorView::slugsAirDataChanged(int systemId, const mavlink_air_data_t &airData) +{ + Q_UNUSED(systemId); + + //ToDo add widget to view value + + +} + #endif // MAVLINK_ENABLED_SLUGS diff --git a/src/ui/SlugsDataSensorView.h b/src/ui/SlugsDataSensorView.h index ed0b9f0..07bccc8 100644 --- a/src/ui/SlugsDataSensorView.h +++ b/src/ui/SlugsDataSensorView.h @@ -114,52 +114,60 @@ public slots: double lon, double alt, quint64 time); + + + /** + * @brief Updates the CPU load widget - 170 + */ + void slugsCpuLoadChanged(int systemId, + const mavlink_cpu_load_t& cpuLoad); + /** - * @brief Updates the sensor bias widget + * @brief Updates the air data widget - 171 + */ + void slugsAirDataChanged(int systemId, + const mavlink_air_data_t& airData); + + /** + * @brief Updates the sensor bias widget - 172 */ void slugsSensorBiasChanged(int systemId, const mavlink_sensor_bias_t& sensorBias); /** - * @brief Updates the diagnostic widget + * @brief Updates the diagnostic widget - 173 */ void slugsDiagnosticMessageChanged(int systemId, const mavlink_diagnostic_t& diagnostic); - /** - * @brief Updates the CPU load widget - */ - void slugsCpuLoadChanged(int systemId, - const mavlink_cpu_load_t& cpuLoad); - /** - * @brief Updates the Navigation widget + * @brief Updates the Navigation widget - 176 */ void slugsNavegationChanged(int systemId, const mavlink_slugs_navigation_t& slugsNavigation); /** - * @brief Updates the Data Log widget + * @brief Updates the Data Log widget - 177 */ void slugsDataLogChanged(int systemId, const mavlink_data_log_t& dataLog); /** - * @brief Updates the PWM Commands widget + * @brief Updates the PWM Commands widget - 175 */ void slugsPWMChanged(int systemId, const mavlink_pwm_commands_t& pwmCommands); /** - * @brief Updates the filtered sensor measurements widget + * @brief Updates the filtered sensor measurements widget - 178 */ void slugsFilteredDataChanged(int systemId, const mavlink_filtered_data_t& filteredData); /** - * @brief Updates the gps Date Time widget + * @brief Updates the gps Date Time widget - 179 */ void slugsGPSDateTimeChanged(int systemId, const mavlink_gps_date_time_t& gpsDateTime); diff --git a/src/ui/SlugsDataSensorView.ui b/src/ui/SlugsDataSensorView.ui index 4a6e2de..5145901 100644 --- a/src/ui/SlugsDataSensorView.ui +++ b/src/ui/SlugsDataSensorView.ui @@ -7,7 +7,7 @@ 0 0 399 - 667 + 604 @@ -24,18 +24,18 @@ - 16777215 - 16777215 + 399 + 604 Form - - + + - 0 + 2 @@ -989,7 +989,7 @@ 20 - 13 + 5 @@ -1791,8 +1791,8 @@ Tab 2 - - + + From cfc16ea1c165ac4a90bae115dbd8665c1e1cc788 Mon Sep 17 00:00:00 2001 From: Mariano Lizarraga Date: Fri, 10 Dec 2010 11:27:19 -0600 Subject: [PATCH 07/21] Added support for Slugs to use the common Remote Control window --- src/uas/SlugsMAV.cc | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/uas/SlugsMAV.cc b/src/uas/SlugsMAV.cc index 025087b..7728ec3 100644 --- a/src/uas/SlugsMAV.cc +++ b/src/uas/SlugsMAV.cc @@ -172,7 +172,8 @@ void SlugsMAV::emitSignals (void){ break; case 3: - emit slugsPilotConsolePWM(uasId,mlPilotConsoleData); + emit remoteControlChannelScaledChanged(0,(mlPilotConsoleData.de- 1000.0f)/1000.0f); + emit remoteControlChannelScaledChanged(1,0.75); emit slugsPWM(uasId, mlPwmCommands); break; From da83f1961112319628c4aba3142a6e35d48b34d9 Mon Sep 17 00:00:00 2001 From: tecnosapiens Date: Fri, 10 Dec 2010 11:53:32 -0600 Subject: [PATCH 08/21] connect mavlink_msg_air_data signal to SlugsDataSensorView widget --- src/ui/SlugsDataSensorView.cc | 9 +++++---- src/ui/SlugsDataSensorView.ui | 5 ----- 2 files changed, 5 insertions(+), 9 deletions(-) diff --git a/src/ui/SlugsDataSensorView.cc b/src/ui/SlugsDataSensorView.cc index f25a9ac..cf6588b 100644 --- a/src/ui/SlugsDataSensorView.cc +++ b/src/ui/SlugsDataSensorView.cc @@ -53,7 +53,7 @@ void SlugsDataSensorView::addUAS(UASInterface* uas) connect(slugsMav, SIGNAL(slugsPWM(int,const mavlink_pwm_commands_t&)),this,SLOT(slugsPWMChanged(int,const mavlink_pwm_commands_t&))); connect(slugsMav, SIGNAL(slugsFilteredData(int,const mavlink_filtered_data_t&)),this,SLOT(slugsFilteredDataChanged(int,const mavlink_filtered_data_t&))); connect(slugsMav, SIGNAL(slugsGPSDateTime(int,const mavlink_gps_date_time_t&)),this,SLOT(slugsGPSDateTimeChanged(int,const mavlink_gps_date_time_t&))); - //connect(slugsMav,SIGNAL(slugsAirData(int,mavlink_air_data_t&)),this,SLOT()) + connect(slugsMav,SIGNAL(slugsAirData(int,mavlink_air_data_t&)),this,SLOT(slugsAirDataChanged(int,mavlink_air_data_t&))); #endif // MAVLINK_ENABLED_SLUGS // Set this UAS as active if it is the first one @@ -248,12 +248,13 @@ void SlugsDataSensorView::slugsGPSDateTimeChanged(int systemId, void SlugsDataSensorView::slugsAirDataChanged(int systemId, const mavlink_air_data_t &airData) { Q_UNUSED(systemId); - - //ToDo add widget to view value - + ui->ed_dynamic->setText(QString::number(airData.dynamicPressure)); + ui->ed_static->setText(QString::number(airData.staticPressure)); + ui->ed_temp->setText(QString::number(airData.temperature)); } + #endif // MAVLINK_ENABLED_SLUGS diff --git a/src/ui/SlugsDataSensorView.ui b/src/ui/SlugsDataSensorView.ui index 5145901..b0ea756 100644 --- a/src/ui/SlugsDataSensorView.ui +++ b/src/ui/SlugsDataSensorView.ui @@ -4483,11 +4483,6 @@ - - - Tab 4 - - From 0f064266abb447527b668d7033742122048cbc6a Mon Sep 17 00:00:00 2001 From: tecnosapiens Date: Fri, 10 Dec 2010 17:12:39 -0600 Subject: [PATCH 09/21] finished the connection of signal of the slugs data in SlugsDataSensorView --- src/uas/SlugsMAV.cc | 35 +++++++++++++----- src/uas/SlugsMAV.h | 3 ++ src/uas/UAS.cc | 4 +++ src/uas/UAS.h | 20 +++++------ src/ui/SlugsDataSensorView.cc | 82 +++++++++++++++++++++++++++++++++++++++---- src/ui/SlugsDataSensorView.h | 16 +++++++++ src/ui/SlugsDataSensorView.ui | 2 +- 7 files changed, 135 insertions(+), 27 deletions(-) diff --git a/src/uas/SlugsMAV.cc b/src/uas/SlugsMAV.cc index 7728ec3..edc2a4d 100644 --- a/src/uas/SlugsMAV.cc +++ b/src/uas/SlugsMAV.cc @@ -169,11 +169,16 @@ void SlugsMAV::emitSignals (void){ case 2: emit slugsAirData(uasId, mlAirData); emit slugsDiagnostic(uasId,mlDiagnosticData); + break; case 3: emit remoteControlChannelScaledChanged(0,(mlPilotConsoleData.de- 1000.0f)/1000.0f); - emit remoteControlChannelScaledChanged(1,0.75); + emit remoteControlChannelScaledChanged(1,(mlPilotConsoleData.dla- 1000.0f)/1000.0f); + emit remoteControlChannelScaledChanged(2,(mlPilotConsoleData.dr- 1000.0f)/1000.0f); + emit remoteControlChannelScaledChanged(3,(mlPilotConsoleData.dra- 1000.0f)/1000.0f); + emit remoteControlChannelScaledChanged(0,(mlPilotConsoleData.dt- 1000.0f)/1000.0f); + emit slugsPWM(uasId, mlPwmCommands); break; @@ -185,11 +190,13 @@ void SlugsMAV::emitSignals (void){ case 5: emit slugsFilteredData(uasId,mlFilteredData); emit slugsGPSDateTime(uasId, mlGpsDateTime); + break; case 6: emit slugsActionAck(uasId,mlActionAck); emit emitGpsSignals(); + break; } @@ -213,20 +220,30 @@ void SlugsMAV::emitSignals (void){ #ifdef MAVLINK_ENABLED_SLUGS void SlugsMAV::emitGpsSignals (void){ - if (mlGpsData.fix_type > 0){ + qDebug()<<"After Emit GPS Signal"< 0){ emit globalPositionChanged(this, mlGpsData.lon, mlGpsData.lat, mlGpsData.alt, 0.0); - // Smaller than threshold and not NaN - if (mlGpsData.v < 1000000 && mlGpsData.v == mlGpsData.v){ - emit speedChanged(this, (double)mlGpsData.v, 0.0, 0.0, 0.0); - } else { - emit textMessageReceived(uasId, uasId, 255, QString("GCS ERROR: RECEIVED INVALID SPEED OF %1 m/s").arg(mlGpsData.v)); - } - } + emit slugsGPSCogSog(uasId,mlGpsData.hdg, mlGpsData.v); + +// // Smaller than threshold and not NaN +// if (mlGpsData.v < 1000000 && mlGpsData.v == mlGpsData.v){ +// // emit speedChanged(this, (double)mlGpsData.v, 0.0, 0.0, 0.0); + +// } +// else { +// emit textMessageReceived(uasId, uasId, 255, QString("GCS ERROR: RECEIVED INVALID SPEED OF %1 m/s").arg(mlGpsData.v)); +// } + //} + } void SlugsMAV::emitPidSignal() diff --git a/src/uas/SlugsMAV.h b/src/uas/SlugsMAV.h index 295c820..c30996d 100644 --- a/src/uas/SlugsMAV.h +++ b/src/uas/SlugsMAV.h @@ -47,6 +47,7 @@ public slots: signals: void slugsRawImu(int uasId, const mavlink_raw_imu_t& rawData); + void slugsGPSCogSog(int uasId, double cog, double sog); #ifdef MAVLINK_ENABLED_SLUGS @@ -67,6 +68,8 @@ signals: void slugsBootMsg(int uasId, mavlink_boot_t& boot); void slugsAttitude(int uasId, mavlink_attitude_t& attitude); + + #endif protected: diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 69af7a9..ba5ca80 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -328,6 +328,10 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) emit valueChanged(uasId, "Vz", pos.vz, time); emit localPositionChanged(this, pos.x, pos.y, pos.z, time); emit speedChanged(this, pos.vx, pos.vy, pos.vz, time); + +// qDebug()<<"Local Position = "<* getLinks(); - double getLocalX() const { return localX; }; - double getLocalY() const { return localY; }; - double getLocalZ() const { return localZ; }; - double getLatitude() const { return latitude; }; - double getLongitude() const { return longitude; }; - double getAltitude() const { return altitude; }; - - double getRoll() const { return roll; }; - double getPitch() const { return pitch; }; - double getYaw() const { return yaw; }; + double getLocalX() const { return localX; } + double getLocalY() const { return localY; } + double getLocalZ() const { return localZ; } + double getLatitude() const { return latitude; } + double getLongitude() const { return longitude; } + double getAltitude() const { return altitude; } + + double getRoll() const { return roll; } + double getPitch() const { return pitch; } + double getYaw() const { return yaw; } friend class UASWaypointManager; diff --git a/src/ui/SlugsDataSensorView.cc b/src/ui/SlugsDataSensorView.cc index cf6588b..c8a12cd 100644 --- a/src/ui/SlugsDataSensorView.cc +++ b/src/ui/SlugsDataSensorView.cc @@ -41,10 +41,12 @@ void SlugsDataSensorView::addUAS(UASInterface* uas) connect(slugsMav, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(slugSpeedLocalPositionChanged(UASInterface*,double,double,double,quint64))); connect(slugsMav, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(slugAttitudeChanged(UASInterface*,double,double,double,quint64))); connect(slugsMav, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(slugsGlobalPositionChanged(UASInterface*,double,double,double,quint64))); + connect(slugsMav,SIGNAL(slugsGPSCogSog(int,double,double)),this,SLOT(slugsGPSCogSog(int,double,double))); - //connect slugs especial messages + + //connect slugs especial messages connect(slugsMav, SIGNAL(slugsSensorBias(int,const mavlink_sensor_bias_t&)), this, SLOT(slugsSensorBiasChanged(int,const mavlink_sensor_bias_t&))); connect(slugsMav, SIGNAL(slugsDiagnostic(int,const mavlink_diagnostic_t&)), this, SLOT(slugsDiagnosticMessageChanged(int,const mavlink_diagnostic_t&))); connect(slugsMav, SIGNAL(slugsCPULoad(int,const mavlink_cpu_load_t&)), this, SLOT(slugsCpuLoadChanged(int,const mavlink_cpu_load_t&))); @@ -53,7 +55,8 @@ void SlugsDataSensorView::addUAS(UASInterface* uas) connect(slugsMav, SIGNAL(slugsPWM(int,const mavlink_pwm_commands_t&)),this,SLOT(slugsPWMChanged(int,const mavlink_pwm_commands_t&))); connect(slugsMav, SIGNAL(slugsFilteredData(int,const mavlink_filtered_data_t&)),this,SLOT(slugsFilteredDataChanged(int,const mavlink_filtered_data_t&))); connect(slugsMav, SIGNAL(slugsGPSDateTime(int,const mavlink_gps_date_time_t&)),this,SLOT(slugsGPSDateTimeChanged(int,const mavlink_gps_date_time_t&))); - connect(slugsMav,SIGNAL(slugsAirData(int,mavlink_air_data_t&)),this,SLOT(slugsAirDataChanged(int,mavlink_air_data_t&))); + connect(slugsMav,SIGNAL(slugsAirData(int, const mavlink_air_data_t&)),this,SLOT(slugsAirDataChanged(int, const mavlink_air_data_t&))); + #endif // MAVLINK_ENABLED_SLUGS // Set this UAS as active if it is the first one @@ -70,6 +73,15 @@ void SlugsDataSensorView::slugRawDataChanged(int uasId, const mavlink_raw_imu_t ui->m_Axr->setText(QString::number(rawData.xacc)); ui->m_Ayr->setText(QString::number(rawData.yacc)); ui->m_Azr->setText(QString::number(rawData.zacc)); + + ui->m_Mxr->setText(QString::number(rawData.xmag)); + ui->m_Myr->setText(QString::number(rawData.ymag)); + ui->m_Mzr->setText(QString::number(rawData.zmag)); + + ui->m_Gxr->setText(QString::number(rawData.xgyro)); + ui->m_Gyr->setText(QString::number(rawData.ygyro)); + ui->m_Gzr->setText(QString::number(rawData.zgyro)); + } void SlugsDataSensorView::setActiveUAS(UASInterface* uas){ @@ -86,9 +98,13 @@ void SlugsDataSensorView::slugsGlobalPositionChanged(UASInterface *uas, Q_UNUSED(uas); Q_UNUSED(time); + + ui->m_GpsLatitude->setText(QString::number(lat)); ui->m_GpsLongitude->setText(QString::number(lon)); ui->m_GpsHeight->setText(QString::number(alt)); + + qDebug()<<"GPS Position = "<ed_y->setPlainText(QString::number(y)); ui->ed_z->setPlainText(QString::number(z)); + //qDebug()<<"Local Position = "<ed_vy->setPlainText(QString::number(vy)); ui->ed_vz->setPlainText(QString::number(vz)); + //qDebug()<<"Speed Local Position = "<m_Pitch->setPlainText(QString::number(slugpitch)); ui->m_Yaw->setPlainText(QString::number(slugyaw)); + qDebug()<<"Attitude change = "<m_GpsDate->setText(QString::number(gpsDateTime.month) + "/" + - QString::number(gpsDateTime.day) + "/" + + + QString month, day; + + month = QString::number(gpsDateTime.month); + day = QString::number(gpsDateTime.day); + + if(gpsDateTime.month < 10) month = "0" + QString::number(gpsDateTime.month); + if(gpsDateTime.day < 10) day = "0" + QString::number(gpsDateTime.day); + + + ui->m_GpsDate->setText(day + "/" + + month + "/" + QString::number(gpsDateTime.year)); - ui->m_GpsTime->setText(QString::number(gpsDateTime.hour) + ":" + - QString::number(gpsDateTime.min) + ":" + - QString::number(gpsDateTime.sec)); + QString hour, min, sec; + + hour = QString::number(gpsDateTime.hour); + min = QString::number(gpsDateTime.min); + sec = QString::number(gpsDateTime.sec); + + if(gpsDateTime.hour < 10) hour = "0" + QString::number(gpsDateTime.hour); + if(gpsDateTime.min < 10) min = "0" + QString::number(gpsDateTime.min); + if(gpsDateTime.sec < 10) sec = "0" + QString::number(gpsDateTime.sec); + + ui->m_GpsTime->setText(hour + ":" + + min + ":" + + sec); ui->m_GpsSat->setText(QString::number(gpsDateTime.visSat)); + + } /** @@ -252,8 +297,31 @@ void SlugsDataSensorView::slugsAirDataChanged(int systemId, const mavlink_air_da ui->ed_static->setText(QString::number(airData.staticPressure)); ui->ed_temp->setText(QString::number(airData.temperature)); +// qDebug()<<"Air Data = "<m_GpsCog->setText(QString::number(cog)); + ui->m_GpsSog->setText(QString::number(sog)); + +} + + + diff --git a/src/ui/SlugsDataSensorView.h b/src/ui/SlugsDataSensorView.h index 07bccc8..b1adcca 100644 --- a/src/ui/SlugsDataSensorView.h +++ b/src/ui/SlugsDataSensorView.h @@ -70,6 +70,9 @@ public slots: void setActiveUAS(UASInterface* uas); + /** + * @brief Updates the Raw Data widget + */ void slugRawDataChanged (int uasId, const mavlink_raw_imu_t& rawData); #ifdef MAVLINK_ENABLED_SLUGS @@ -115,6 +118,16 @@ public slots: double alt, quint64 time); + /** + * @brief set COG and SOG values + * + * COG and SOG GPS display on the Widgets + */ + void slugsGPSCogSog(int systemId, + double cog, + double sog); + + /** * @brief Updates the CPU load widget - 170 @@ -174,6 +187,9 @@ public slots: + + + #endif // MAVLINK_ENABLED_SLUGS protected: diff --git a/src/ui/SlugsDataSensorView.ui b/src/ui/SlugsDataSensorView.ui index b0ea756..5474afb 100644 --- a/src/ui/SlugsDataSensorView.ui +++ b/src/ui/SlugsDataSensorView.ui @@ -35,7 +35,7 @@ - 2 + 1 From 9b61aeec325aaa8695591b632663f773b5a4e112 Mon Sep 17 00:00:00 2001 From: Mariano Lizarraga Date: Fri, 10 Dec 2010 18:50:56 -0600 Subject: [PATCH 10/21] VERY UNSTABLE. Working on changing the way widgets are constructed and shown now based on the Detected autopilot. The basic scaffolding is in place still needs a lot of work. --- src/uas/UAS.cc | 2 + src/uas/UAS.h | 22 +- src/uas/UASInterface.h | 3 + src/ui/MainWindow.cc | 702 ++++++++++++++++++++++++++++------------- src/ui/MainWindow.h | 46 ++- src/ui/MainWindow.ui | 68 ++-- src/ui/QGCRemoteControlView.cc | 4 +- 7 files changed, 580 insertions(+), 267 deletions(-) diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 69af7a9..b1c48e5 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -49,6 +49,7 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(), startTime(MG::TIME::getGroundTimeNow()), commStatus(COMM_DISCONNECTED), name(""), + autopilot(0), links(new QList()), unknownPackets(), mavlink(protocol), @@ -149,6 +150,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) if (this->type != mavlink_msg_heartbeat_get_type(&message)) { this->type = mavlink_msg_heartbeat_get_type(&message); + this->autopilot = mavlink_msg_heartbeat_get_autopilot(&message); emit systemTypeSet(this, type); } break; diff --git a/src/uas/UAS.h b/src/uas/UAS.h index 241e3db..0926887 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -79,25 +79,26 @@ public: /** @brief Get the links associated with this robot */ QList* getLinks(); - double getLocalX() const { return localX; }; - double getLocalY() const { return localY; }; - double getLocalZ() const { return localZ; }; - double getLatitude() const { return latitude; }; - double getLongitude() const { return longitude; }; - double getAltitude() const { return altitude; }; + double getLocalX() const { return localX; } + double getLocalY() const { return localY; } + double getLocalZ() const { return localZ; } + double getLatitude() const { return latitude; } + double getLongitude() const { return longitude; } + double getAltitude() const { return altitude; } - double getRoll() const { return roll; }; - double getPitch() const { return pitch; }; - double getYaw() const { return yaw; }; + double getRoll() const { return roll; } + double getPitch() const { return pitch; } + double getYaw() const { return yaw; } friend class UASWaypointManager; protected: int uasId; ///< Unique system ID - int type; ///< UAS type (from type enum) + unsigned char type; ///< UAS type (from type enum) quint64 startTime; ///< The time the UAS was switched on CommStatus commStatus; ///< Communication status QString name; ///< Human-friendly name of the vehicle, e.g. bravo + int autopilot; ///< Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM QList* links; ///< List of links this UAS can be reached by QList unknownPackets; ///< Packet IDs which are unknown and have been received MAVLinkProtocol* mavlink; ///< Reference to the MAVLink instance @@ -164,6 +165,7 @@ protected: public: UASWaypointManager &getWaypointManager(void) { return waypointManager; } int getSystemType(); + unsigned char getAutopilotType() {return autopilot;} public slots: /** @brief Launches the system **/ diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index 1aa7c88..c8266ac 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -157,6 +157,7 @@ public: return color; } + virtual unsigned char getAutopilotType() = 0; public slots: /** @brief Launches the system/Liftof **/ @@ -241,6 +242,8 @@ public slots: virtual void startGyroscopeCalibration() = 0; virtual void startPressureCalibration() = 0; + + protected: QColor color; diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index df13543..2b138dc 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -54,11 +54,11 @@ MainWindow::MainWindow(QWidget *parent) : // Setup user interface ui.setupUi(this); - buildWidgets(); + buildCommonWidgets(); - connectWidgets(); + connectCommonWidgets(); - arrangeCenterStack(); + arrangeCommonCenterStack(); configureWindowName(); @@ -72,9 +72,8 @@ MainWindow::MainWindow(QWidget *parent) : // Set style sheet as last step reloadStylesheet(); - // Create actions - connectActions(); + connectCommonActions(); // Load widgets and show application windowa loadWidgets(); @@ -90,7 +89,34 @@ MainWindow::~MainWindow() } -void MainWindow::buildWidgets() +void MainWindow::buildCommonWidgets() +{ + //TODO: move protocol outside UI + mavlink = new MAVLinkProtocol(); + + // Center widgets + mapWidget = new MapWidget(this); + protocolWidget = new XMLCommProtocolWidget(this); + + // Dock widgets + controlDockWidget = new QDockWidget(tr("Control"), this); + controlDockWidget->setWidget( new UASControlWidget(this) ); + + listDockWidget = new QDockWidget(tr("Unmanned Systems"), this); + listDockWidget->setWidget( new UASListWidget(this) ); + + waypointsDockWidget = new QDockWidget(tr("Waypoint List"), this); + waypointsDockWidget->setWidget( new WaypointList(this, NULL) ); + + infoDockWidget = new QDockWidget(tr("Status Details"), this); + infoDockWidget->setWidget( new UASInfoWidget(this) ); + + debugConsoleDockWidget = new QDockWidget(tr("Communication Console"), this); + debugConsoleDockWidget->setWidget( new DebugConsole(this) ); + +} + +void MainWindow::buildPxWidgets() { //FIXME: memory of acceptList will never be freed again QStringList* acceptList = new QStringList(); @@ -106,15 +132,11 @@ void MainWindow::buildWidgets() acceptList2->append("Battery"); acceptList2->append("Pressure"); - //TODO: move protocol outside UI - mavlink = new MAVLinkProtocol(); - // Center widgets linechartWidget = new Linecharts(this); hudWidget = new HUD(320, 240, this); - mapWidget = new MapWidget(this); - protocolWidget = new XMLCommProtocolWidget(this); dataplotWidget = new QGCDataPlot2D(this); + #ifdef QGC_OSG_ENABLED _3DWidget = Q3DWidgetFactory::get("PIXHAWK"); #endif @@ -127,24 +149,9 @@ void MainWindow::buildWidgets() #endif // Dock widgets - controlDockWidget = new QDockWidget(tr("Control"), this); - controlDockWidget->setWidget( new UASControlWidget(this) ); - - listDockWidget = new QDockWidget(tr("Unmanned Systems"), this); - listDockWidget->setWidget( new UASListWidget(this) ); - - waypointsDockWidget = new QDockWidget(tr("Waypoint List"), this); - waypointsDockWidget->setWidget( new WaypointList(this, NULL) ); - - infoDockWidget = new QDockWidget(tr("Status Details"), this); - infoDockWidget->setWidget( new UASInfoWidget(this) ); - detectionDockWidget = new QDockWidget(tr("Object Recognition"), this); detectionDockWidget->setWidget( new ObjectDetectionView("images/patterns", this) ); - debugConsoleDockWidget = new QDockWidget(tr("Communication Console"), this); - debugConsoleDockWidget->setWidget( new DebugConsole(this) ); - parametersDockWidget = new QDockWidget(tr("Onboard Parameters"), this); parametersDockWidget->setWidget( new ParameterInterface(this) ); @@ -170,6 +177,22 @@ void MainWindow::buildWidgets() //FIXME: free memory in destructor joystick = new JoystickInput(); +} + +void MainWindow::buildSlugsWidgets() +{ + // Center widgets + linechartWidget = new Linecharts(this); + hudWidget = new HUD(320, 240, this); + + // Dock widgets + rcViewDockWidget = new QDockWidget(tr("Radio Control"), this); + rcViewDockWidget->setWidget( new QGCRemoteControlView(this) ); + + headUpDockWidget = new QDockWidget(tr("Control Indicator"), this); + headUpDockWidget->setWidget( new HUD(320, 240, this)); + + // Dialog widgets slugsDataWidget = new QDockWidget(tr("Slugs Data"), this); slugsDataWidget->setWidget( new SlugsDataSensorView(this)); @@ -183,26 +206,17 @@ void MainWindow::buildWidgets() slugsCamControlWidget->setWidget(new SlugsVideoCamControl(this)); } - /** - * Connect all signals and slots of the main window widgets + * Connect the signals and slots of the common window widgets */ -void MainWindow::connectWidgets() +void MainWindow::connectCommonWidgets() { - if (linechartWidget) - { - connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)), - linechartWidget, SLOT(addSystem(UASInterface*))); - connect(UASManager::instance(), SIGNAL(activeUASSet(int)), - linechartWidget, SLOT(selectSystem(int))); - connect(linechartWidget, SIGNAL(logfileWritten(QString)), - this, SLOT(loadDataView(QString))); - } if (infoDockWidget && infoDockWidget->widget()) { connect(mavlink, SIGNAL(receiveLossChanged(int, float)), infoDockWidget->widget(), SLOT(updateSendLoss(int, float))); } + if (mapWidget && waypointsDockWidget->widget()) { // clear path create on the map @@ -219,8 +233,34 @@ void MainWindow::connectWidgets() //connect(waypointsDockWidget->widget(), SIGNAL(changePositionWPGlobalBySpinBox(int,float,float)), mapWidget, SLOT(changeGlobalWaypointPositionBySpinBox(int,float,float))); // connect(waypointsDockWidget->widget(), SIGNAL(changePositionWPGlobalBySpinBox(int,float,float)), mapWidget, SLOT(changeGlobalWaypointPositionBySpinBox(int,float,float))); - connect(slugsCamControlWidget->widget(),SIGNAL(viewCamBorderAtMap(bool)),mapWidget,SLOT(drawBorderCamAtMap(bool))); - connect(slugsCamControlWidget->widget(),SIGNAL(changeCamPosition(double,double,QString)),mapWidget,SLOT(updateCameraPosition(double,double, QString))); + } + +} + +void MainWindow::connectPxWidgets() +{ + if (linechartWidget) + { + connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)), + linechartWidget, SLOT(addSystem(UASInterface*))); + connect(UASManager::instance(), SIGNAL(activeUASSet(int)), + linechartWidget, SLOT(selectSystem(int))); + connect(linechartWidget, SIGNAL(logfileWritten(QString)), + this, SLOT(loadDataView(QString))); + } + +} + +void MainWindow::connectSlugsWidgets() +{ + if (linechartWidget) + { + connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)), + linechartWidget, SLOT(addSystem(UASInterface*))); + connect(UASManager::instance(), SIGNAL(activeUASSet(int)), + linechartWidget, SLOT(selectSystem(int))); + connect(linechartWidget, SIGNAL(logfileWritten(QString)), + this, SLOT(loadDataView(QString))); } if (slugsHilSimWidget && slugsHilSimWidget->widget()){ @@ -236,14 +276,28 @@ void MainWindow::connectWidgets() } -void MainWindow::arrangeCenterStack() +void MainWindow::arrangeCommonCenterStack() { - QStackedWidget *centerStack = new QStackedWidget(this); + centerStack = new QStackedWidget(this); if (!centerStack) return; - if (linechartWidget) centerStack->addWidget(linechartWidget); - if (protocolWidget) centerStack->addWidget(protocolWidget); + if (mapWidget) centerStack->addWidget(mapWidget); + if (protocolWidget) centerStack->addWidget(protocolWidget); + + setCentralWidget(centerStack); +} + +void MainWindow::arrangePxCenterStack() +{ + + if (!centerStack) { + qDebug() << "Center Stack not Created!"; + return; + } + + if (linechartWidget) centerStack->addWidget(linechartWidget); + #ifdef QGC_OSG_ENABLED if (_3DWidget) centerStack->addWidget(_3DWidget); #endif @@ -256,7 +310,21 @@ void MainWindow::arrangeCenterStack() if (hudWidget) centerStack->addWidget(hudWidget); if (dataplotWidget) centerStack->addWidget(dataplotWidget); - setCentralWidget(centerStack); +} + +void MainWindow::arrangeSlugsCenterStack() +{ + + if (!centerStack) { + qDebug() << "Center Stack not Created!"; + return; + } + + if (linechartWidget) centerStack->addWidget(linechartWidget); + + + if (hudWidget) centerStack->addWidget(hudWidget); + } void MainWindow::configureWindowName() @@ -375,15 +443,71 @@ void MainWindow::showStatusMessage(const QString& status) * @brief Create all actions associated to the main window * **/ -void MainWindow::connectActions() + +void MainWindow::buildHelpMenu (){ + QIcon icon; + + icon = QIcon(":/images/apps/utilities-system-monitor.svg"); + actionOnline_documentation = new QAction(icon,tr("Online Documentation"), this); + + icon = QIcon(":/images/status/software-update-available.svg"); + actionProject_Roadmap = new QAction(icon,tr("Project Roadmap"), this); + + icon = QIcon(":/images/categories/preferences-system.svg"); + actionCredits_Developers = new QAction(icon,tr("Developer Credits"), this); + + helpMenu = ui.menuBar->addMenu(tr("Help")); + + helpMenu->addAction(actionOnline_documentation); + helpMenu->addAction(actionProject_Roadmap); + helpMenu->addAction(actionCredits_Developers); + + +} + + +void MainWindow::buildViewsMenu (){ + QIcon icon; + + icon = QIcon(":/images/status/weather-overcast.svg"); + actionFlightView = new QAction(icon,"Flight View", this); + + icon = QIcon(":/images/apps/utilities-system-monitor.svg"); + actionEngineerView = new QAction(icon,"Engineer View", this); + + icon = QIcon(":/images/status/network-wireless-encrypted.svg"); + actionCalibrationView = new QAction(icon,"Calibration View", this); + + icon = QIcon(":/images/devices/network-wired.svg"); + actionMavlinkView = new QAction(icon,"Mavlink View", this); + + icon = QIcon(":/images/categories/applications-internet.svg"); + actionReloadStyle = new QAction(icon,"Reload Style", this); + + viewsMenu = ui.menuBar->addMenu("Views"); + + viewsMenu->addAction(actionFlightView); + viewsMenu->addAction(actionEngineerView); + viewsMenu->addAction(actionCalibrationView); + viewsMenu->addSeparator(); + viewsMenu->addAction(actionMavlinkView); + viewsMenu->addAction(actionReloadStyle); + +} + + +void MainWindow::connectCommonActions() { + buildViewsMenu(); + buildHelpMenu(); + // Connect actions from ui connect(ui.actionAdd_Link, SIGNAL(triggered()), this, SLOT(addLink())); // Connect internal actions connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)), this, SLOT(UASCreated(UASInterface*))); - // Connect user interface controls + // Unmanned System controls connect(ui.actionLiftoff, SIGNAL(triggered()), UASManager::instance(), SLOT(launchActiveUAS())); connect(ui.actionLand, SIGNAL(triggered()), UASManager::instance(), SLOT(returnActiveUAS())); connect(ui.actionEmergency_Land, SIGNAL(triggered()), UASManager::instance(), SLOT(stopActiveUAS())); @@ -391,43 +515,34 @@ void MainWindow::connectActions() connect(ui.actionConfiguration, SIGNAL(triggered()), UASManager::instance(), SLOT(configureActiveUAS())); - // User interface actions - connect(ui.actionPilotView, SIGNAL(triggered()), this, SLOT(loadPilotView())); - connect(ui.actionEngineerView, SIGNAL(triggered()), this, SLOT(loadEngineerView())); - connect(ui.actionOperatorView, SIGNAL(triggered()), this, SLOT(loadOperatorView())); -#ifdef QGC_OSG_ENABLED - connect(ui.action3DView, SIGNAL(triggered()), this, SLOT(load3DView())); -#else - ui.menuWindow->removeAction(ui.action3DView); -#endif + // Views actions + connect(actionFlightView, SIGNAL(triggered()), this, SLOT(loadPilotView())); + connect(actionEngineerView, SIGNAL(triggered()), this, SLOT(loadEngineerView())); + connect(actionCalibrationView, SIGNAL(triggered()), this, SLOT(loadOperatorView())); -#ifdef QGC_OSGEARTH_ENABLED - connect(ui.action3DMapView, SIGNAL(triggered()), this, SLOT(load3DMapView())); -#else - ui.menuWindow->removeAction(ui.action3DMapView); -#endif - connect(ui.actionShow_full_view, SIGNAL(triggered()), this, SLOT(loadAllView())); - connect(ui.actionShow_MAVLink_view, SIGNAL(triggered()), this, SLOT(loadMAVLinkView())); - connect(ui.actionShow_data_analysis_view, SIGNAL(triggered()), this, SLOT(loadDataView())); - connect(ui.actionStyleConfig, SIGNAL(triggered()), this, SLOT(reloadStylesheet())); - connect(ui.actionGlobalOperatorView, SIGNAL(triggered()), this, SLOT(loadGlobalOperatorView())); - connect(ui.actionOnline_documentation, SIGNAL(triggered()), this, SLOT(showHelp())); - connect(ui.actionCredits_Developers, SIGNAL(triggered()), this, SLOT(showCredits())); - connect(ui.actionProject_Roadmap, SIGNAL(triggered()), this, SLOT(showRoadMap())); -#if (defined Q_OS_WIN) | (defined Q_OS_MAC) - connect(ui.actionGoogleEarthView, SIGNAL(triggered()), this, SLOT(loadGoogleEarthView())); -#else - ui.menuWindow->removeAction(ui.actionGoogleEarthView); -#endif + connect(actionMavlinkView, SIGNAL(triggered()), this, SLOT(loadMAVLinkView())); + connect(actionReloadStyle, SIGNAL(triggered()), this, SLOT(reloadStylesheet())); - // Joystick configuration - connect(ui.actionJoystickSettings, SIGNAL(triggered()), this, SLOT(configure())); + // Help Actions + connect(actionOnline_documentation, SIGNAL(triggered()), this, SLOT(showHelp())); + connect(actionCredits_Developers, SIGNAL(triggered()), this, SLOT(showCredits())); + connect(actionProject_Roadmap, SIGNAL(triggered()), this, SLOT(showRoadMap())); - // Slugs View - connect(ui.actionShow_Slugs_View, SIGNAL(triggered()), this, SLOT(loadSlugsView())); - //GlobalOperatorView - // connect(ui.actionGlobalOperatorView,SIGNAL(triggered()),waypointsDockWidget->widget(),SLOT()) +} + +void MainWindow::connectPxActions() +{ + + ui.actionJoystickSettings->setVisible(true); + + // Joystick configuration + connect(ui.actionJoystickSettings, SIGNAL(triggered()), this, SLOT(configure())); + +} + +void MainWindow::connectSlugsActions() +{ } @@ -582,21 +697,60 @@ void MainWindow::UASCreated(UASInterface* uas) // TODO Stylesheet reloading should in theory not be necessary reloadStylesheet(); - // Check which type this UAS is of - PxQuadMAV* mav = dynamic_cast(uas); - if (mav) loadPixhawkView(); - - if (slugsDataWidget) { - SlugsDataSensorView* dataWidget = dynamic_cast(slugsDataWidget->widget()); - if (dataWidget) { - SlugsMAV* mav2 = dynamic_cast(uas); - if (mav2) { - dataWidget->addUAS(uas); - //loadSlugsView(); - loadGlobalOperatorView(); - } - } + switch (uas->getAutopilotType()){ + case (MAV_AUTOPILOT_GENERIC): + case (MAV_AUTOPILOT_ARDUPILOTMEGA): + case (MAV_AUTOPILOT_PIXHAWK): + { + // Build Pixhawk Widgets + buildPxWidgets(); + + // Connect Pixhawk Widgets + connectPxWidgets(); + + // Arrange Pixhawk Centerstack + arrangePxCenterStack(); + + // Connect Pixhawk Actions + connectPxActions(); + + // FIXME: This type checking might be redundant + // Check which type this UAS is of +// PxQuadMAV* mav = dynamic_cast(uas); +// if (mav) loadPixhawkEngineerView(); + } break; + + case (MAV_AUTOPILOT_SLUGS): + { + // Build Slugs Widgets + buildSlugsWidgets(); + + // Connect Slugs Widgets + connectSlugsWidgets(); + + // Arrange Slugs Centerstack + arrangeSlugsCenterStack(); + + // Connect Slugs Actions + connectSlugsActions(); + + // FIXME: This type checking might be redundant +// if (slugsDataWidget) { +// SlugsDataSensorView* dataWidget = dynamic_cast(slugsDataWidget->widget()); +// if (dataWidget) { +// SlugsMAV* mav2 = dynamic_cast(uas); +// if (mav2) { + (dynamic_cast(slugsDataWidget->widget()))->addUAS(uas); +// //loadSlugsView(); +// loadGlobalOperatorView(); +// } +// } +// } + } break; + + loadEngineerView(); } + } } @@ -714,7 +868,7 @@ void MainWindow::loadSlugsView() this->show(); } -void MainWindow::loadPixhawkView() +void MainWindow::loadPixhawkEngineerView() { clearView(); // Engineer view, used in EMAV2009 @@ -930,7 +1084,7 @@ void MainWindow::loadOperatorView() this->show(); } -void MainWindow::loadGlobalOperatorView() +void MainWindow::loadSlugsEngineerView() { clearView(); @@ -959,76 +1113,20 @@ void MainWindow::loadGlobalOperatorView() slugsDataWidget->show(); } - // Slugs Data View + // Slugs PID Tunning if (slugsPIDControlWidget) { addDockWidget(Qt::LeftDockWidgetArea, slugsPIDControlWidget); slugsPIDControlWidget->show(); } + // Slug Camera Control if (slugsCamControlWidget) { addDockWidget(Qt::BottomDockWidgetArea, slugsCamControlWidget); slugsCamControlWidget->show(); } - - -// // UAS CONTROL -// if (controlDockWidget) -// { -// addDockWidget(Qt::LeftDockWidgetArea, controlDockWidget); -// controlDockWidget->show(); -// } - -// // UAS LIST -// if (listDockWidget) -// { -// addDockWidget(Qt::BottomDockWidgetArea, listDockWidget); -// listDockWidget->show(); -// } - -// // UAS STATUS -// if (infoDockWidget) -// { -// addDockWidget(Qt::LeftDockWidgetArea, infoDockWidget); -// infoDockWidget->show(); -// } - - -// // HORIZONTAL SITUATION INDICATOR -// if (hsiDockWidget) -// { -// HSIDisplay* hsi = dynamic_cast( hsiDockWidget->widget() ); -// if (hsi) -// { -// addDockWidget(Qt::BottomDockWidgetArea, hsiDockWidget); -// hsiDockWidget->show(); -// hsi->start(); -// } -// } - - // PROCESS CONTROL -// if (watchdogControlDockWidget) -// { -// addDockWidget(Qt::RightDockWidgetArea, watchdogControlDockWidget); -// watchdogControlDockWidget->show(); -// } - - // HEAD UP DISPLAY -// if (headUpDockWidget) -// { -// addDockWidget(Qt::RightDockWidgetArea, headUpDockWidget); -// // FIXME Replace with default ->show() call -// HUD* hud = dynamic_cast(headUpDockWidget->widget()); - -// if (hud) -// { -// headUpDockWidget->show(); -// hud->start(); -// } -// } - } void MainWindow::load3DMapView() @@ -1194,64 +1292,16 @@ void MainWindow::loadEngineerView() clearView(); // Engineer view, used in EMAV2009 - // LINE CHART - if (linechartWidget) - { - QStackedWidget *centerStack = dynamic_cast(centralWidget()); - if (centerStack) - { - linechartWidget->setActive(true); - centerStack->setCurrentWidget(linechartWidget); - } - } - - // UAS CONTROL - if (controlDockWidget) - { - addDockWidget(Qt::LeftDockWidgetArea, controlDockWidget); - controlDockWidget->show(); - } - - // UAS LIST - if (listDockWidget) - { - addDockWidget(Qt::BottomDockWidgetArea, listDockWidget); - listDockWidget->show(); - } - - // UAS STATUS - if (infoDockWidget) - { - addDockWidget(Qt::LeftDockWidgetArea, infoDockWidget); - infoDockWidget->show(); - } - - // WAYPOINT LIST - if (waypointsDockWidget) - { - addDockWidget(Qt::BottomDockWidgetArea, waypointsDockWidget); - waypointsDockWidget->show(); - } - - // DEBUG CONSOLE - if (debugConsoleDockWidget) - { - addDockWidget(Qt::BottomDockWidgetArea, debugConsoleDockWidget); - debugConsoleDockWidget->show(); - } + switch (UASManager::instance()->getActiveUAS()->getAutopilotType()){ + case (MAV_AUTOPILOT_GENERIC): + case (MAV_AUTOPILOT_ARDUPILOTMEGA): + case (MAV_AUTOPILOT_PIXHAWK): + loadPixhawkEngineerView(); + break; - // ONBOARD PARAMETERS - if (parametersDockWidget) - { - addDockWidget(Qt::RightDockWidgetArea, parametersDockWidget); - parametersDockWidget->show(); - } - - // RADIO CONTROL VIEW - if (rcViewDockWidget) - { - addDockWidget(Qt::BottomDockWidgetArea, rcViewDockWidget); - rcViewDockWidget->show(); + case (MAV_AUTOPILOT_SLUGS): + loadSlugsEngineerView(); + break; } this->show(); @@ -1368,3 +1418,233 @@ void MainWindow::loadWidgets() loadEngineerView(); //loadPilotView(); } + + +/* + ================================== + ========== ATTIC ================= + ================================== + +void MainWindow::buildCommonWidgets() +{ + //FIXME: memory of acceptList will never be freed again + QStringList* acceptList = new QStringList(); + acceptList->append("roll IMU"); + acceptList->append("pitch IMU"); + acceptList->append("yaw IMU"); + acceptList->append("rollspeed IMU"); + acceptList->append("pitchspeed IMU"); + acceptList->append("yawspeed IMU"); + + //FIXME: memory of acceptList2 will never be freed again + QStringList* acceptList2 = new QStringList(); + acceptList2->append("Battery"); + acceptList2->append("Pressure"); + + //TODO: move protocol outside UI + mavlink = new MAVLinkProtocol(); + + // Center widgets + linechartWidget = new Linecharts(this); + hudWidget = new HUD(320, 240, this); + mapWidget = new MapWidget(this); + protocolWidget = new XMLCommProtocolWidget(this); + dataplotWidget = new QGCDataPlot2D(this); +#ifdef QGC_OSG_ENABLED + _3DWidget = Q3DWidgetFactory::get("PIXHAWK"); +#endif + +#ifdef QGC_OSGEARTH_ENABLED + _3DMapWidget = Q3DWidgetFactory::get("MAP3D"); +#endif +#if (defined Q_OS_WIN) | (defined Q_OS_MAC) + gEarthWidget = new QGCGoogleEarthView(this); +#endif + + // Dock widgets + controlDockWidget = new QDockWidget(tr("Control"), this); + controlDockWidget->setWidget( new UASControlWidget(this) ); + + listDockWidget = new QDockWidget(tr("Unmanned Systems"), this); + listDockWidget->setWidget( new UASListWidget(this) ); + + waypointsDockWidget = new QDockWidget(tr("Waypoint List"), this); + waypointsDockWidget->setWidget( new WaypointList(this, NULL) ); + + infoDockWidget = new QDockWidget(tr("Status Details"), this); + infoDockWidget->setWidget( new UASInfoWidget(this) ); + + detectionDockWidget = new QDockWidget(tr("Object Recognition"), this); + detectionDockWidget->setWidget( new ObjectDetectionView("images/patterns", this) ); + + debugConsoleDockWidget = new QDockWidget(tr("Communication Console"), this); + debugConsoleDockWidget->setWidget( new DebugConsole(this) ); + + parametersDockWidget = new QDockWidget(tr("Onboard Parameters"), this); + parametersDockWidget->setWidget( new ParameterInterface(this) ); + + watchdogControlDockWidget = new QDockWidget(tr("Process Control"), this); + watchdogControlDockWidget->setWidget( new WatchdogControl(this) ); + + hsiDockWidget = new QDockWidget(tr("Horizontal Situation Indicator"), this); + hsiDockWidget->setWidget( new HSIDisplay(this) ); + + headDown1DockWidget = new QDockWidget(tr("Primary Flight Display"), this); + headDown1DockWidget->setWidget( new HDDisplay(acceptList, this) ); + + headDown2DockWidget = new QDockWidget(tr("Payload Status"), this); + headDown2DockWidget->setWidget( new HDDisplay(acceptList2, this) ); + + rcViewDockWidget = new QDockWidget(tr("Radio Control"), this); + rcViewDockWidget->setWidget( new QGCRemoteControlView(this) ); + + headUpDockWidget = new QDockWidget(tr("Control Indicator"), this); + headUpDockWidget->setWidget( new HUD(320, 240, this)); + + // Dialogue widgets + //FIXME: free memory in destructor + joystick = new JoystickInput(); + + slugsDataWidget = new QDockWidget(tr("Slugs Data"), this); + slugsDataWidget->setWidget( new SlugsDataSensorView(this)); + + slugsPIDControlWidget = new QDockWidget(tr("PID Control"), this); + slugsPIDControlWidget->setWidget(new SlugsPIDControl(this)); + + slugsHilSimWidget = new QDockWidget(tr("Slugs Hil Sim"), this); + slugsHilSimWidget->setWidget( new SlugsHilSim(this)); + + slugsCamControlWidget = new QDockWidget(tr("Video Camera Control"), this); + slugsCamControlWidget->setWidget(new SlugsVideoCamControl(this)); + +} + +void MainWindow::connectWidgets() +{ + if (linechartWidget) + { + connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)), + linechartWidget, SLOT(addSystem(UASInterface*))); + connect(UASManager::instance(), SIGNAL(activeUASSet(int)), + linechartWidget, SLOT(selectSystem(int))); + connect(linechartWidget, SIGNAL(logfileWritten(QString)), + this, SLOT(loadDataView(QString))); + } + if (infoDockWidget && infoDockWidget->widget()) + { + connect(mavlink, SIGNAL(receiveLossChanged(int, float)), + infoDockWidget->widget(), SLOT(updateSendLoss(int, float))); + } + if (mapWidget && waypointsDockWidget->widget()) + { + // clear path create on the map + connect(waypointsDockWidget->widget(), SIGNAL(clearPathclicked()), mapWidget, SLOT(clearPath())); + // add Waypoint widget in the WaypointList widget when mouse clicked + connect(mapWidget, SIGNAL(captureMapCoordinateClick(QPointF)), waypointsDockWidget->widget(), SLOT(addWaypointMouse(QPointF))); + // it notifies that a waypoint global goes to do create + //connect(mapWidget, SIGNAL(createGlobalWP(bool, QPointF)), waypointsDockWidget->widget(), SLOT(setIsWPGlobal(bool, QPointF))); + //connect(mapWidget, SIGNAL(sendGeometryEndDrag(QPointF,int)), waypointsDockWidget->widget(), SLOT(waypointGlobalChanged(QPointF,int)) ); + + // it notifies that a waypoint global goes to do create and a map graphic too + connect(waypointsDockWidget->widget(), SIGNAL(createWaypointAtMap(QPointF)), mapWidget, SLOT(createWaypointGraphAtMap(QPointF))); + // it notifies that a waypoint global change it¥s position by spinBox on Widget WaypointView + //connect(waypointsDockWidget->widget(), SIGNAL(changePositionWPGlobalBySpinBox(int,float,float)), mapWidget, SLOT(changeGlobalWaypointPositionBySpinBox(int,float,float))); + // connect(waypointsDockWidget->widget(), SIGNAL(changePositionWPGlobalBySpinBox(int,float,float)), mapWidget, SLOT(changeGlobalWaypointPositionBySpinBox(int,float,float))); + + connect(slugsCamControlWidget->widget(),SIGNAL(viewCamBorderAtMap(bool)),mapWidget,SLOT(drawBorderCamAtMap(bool))); + connect(slugsCamControlWidget->widget(),SIGNAL(changeCamPosition(double,double,QString)),mapWidget,SLOT(updateCameraPosition(double,double, QString))); + } + + if (slugsHilSimWidget && slugsHilSimWidget->widget()){ + connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), + slugsHilSimWidget->widget(), SLOT(activeUasSet(UASInterface*))); + } + + if (slugsDataWidget && slugsDataWidget->widget()){ + connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), + slugsDataWidget->widget(), SLOT(setActiveUAS(UASInterface*))); + } + + +} + +void MainWindow::arrangeCommonCenterStack() +{ + + QStackedWidget *centerStack = new QStackedWidget(this); + if (!centerStack) return; + if (linechartWidget) centerStack->addWidget(linechartWidget); + if (protocolWidget) centerStack->addWidget(protocolWidget); + if (mapWidget) centerStack->addWidget(mapWidget); +#ifdef QGC_OSG_ENABLED + if (_3DWidget) centerStack->addWidget(_3DWidget); +#endif +#ifdef QGC_OSGEARTH_ENABLED + if (_3DMapWidget) centerStack->addWidget(_3DMapWidget); +#endif +#if (defined Q_OS_WIN) | (defined Q_OS_MAC) + if (gEarthWidget) centerStack->addWidget(gEarthWidget); +#endif + if (hudWidget) centerStack->addWidget(hudWidget); + if (dataplotWidget) centerStack->addWidget(dataplotWidget); + + setCentralWidget(centerStack); +} + +void MainWindow::connectActions() +{ + // Connect actions from ui + connect(ui.actionAdd_Link, SIGNAL(triggered()), this, SLOT(addLink())); + + // Connect internal actions + connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)), this, SLOT(UASCreated(UASInterface*))); + + // Connect user interface controls + connect(ui.actionLiftoff, SIGNAL(triggered()), UASManager::instance(), SLOT(launchActiveUAS())); + connect(ui.actionLand, SIGNAL(triggered()), UASManager::instance(), SLOT(returnActiveUAS())); + connect(ui.actionEmergency_Land, SIGNAL(triggered()), UASManager::instance(), SLOT(stopActiveUAS())); + connect(ui.actionEmergency_Kill, SIGNAL(triggered()), UASManager::instance(), SLOT(killActiveUAS())); + + connect(ui.actionConfiguration, SIGNAL(triggered()), UASManager::instance(), SLOT(configureActiveUAS())); + + // User interface actions + connect(ui.actionPilotView, SIGNAL(triggered()), this, SLOT(loadPilotView())); + connect(ui.actionEngineerView, SIGNAL(triggered()), this, SLOT(loadEngineerView())); + connect(ui.actionOperatorView, SIGNAL(triggered()), this, SLOT(loadOperatorView())); +#ifdef QGC_OSG_ENABLED + connect(ui.action3DView, SIGNAL(triggered()), this, SLOT(load3DView())); +#else + ui.menuWindow->removeAction(ui.action3DView); +#endif + +#ifdef QGC_OSGEARTH_ENABLED + connect(ui.action3DMapView, SIGNAL(triggered()), this, SLOT(load3DMapView())); +#else + ui.menuWindow->removeAction(ui.action3DMapView); +#endif + connect(ui.actionShow_full_view, SIGNAL(triggered()), this, SLOT(loadAllView())); + connect(ui.actionShow_MAVLink_view, SIGNAL(triggered()), this, SLOT(loadMAVLinkView())); + connect(ui.actionShow_data_analysis_view, SIGNAL(triggered()), this, SLOT(loadDataView())); + connect(ui.actionStyleConfig, SIGNAL(triggered()), this, SLOT(reloadStylesheet())); + connect(ui.actionGlobalOperatorView, SIGNAL(triggered()), this, SLOT(loadGlobalOperatorView())); + connect(ui.actionOnline_documentation, SIGNAL(triggered()), this, SLOT(showHelp())); + connect(ui.actionCredits_Developers, SIGNAL(triggered()), this, SLOT(showCredits())); + connect(ui.actionProject_Roadmap, SIGNAL(triggered()), this, SLOT(showRoadMap())); +#if (defined Q_OS_WIN) | (defined Q_OS_MAC) + connect(ui.actionGoogleEarthView, SIGNAL(triggered()), this, SLOT(loadGoogleEarthView())); +#else + ui.menuWindow->removeAction(ui.actionGoogleEarthView); +#endif + + // Joystick configuration + connect(ui.actionJoystickSettings, SIGNAL(triggered()), this, SLOT(configure())); + + // Slugs View + connect(ui.actionShow_Slugs_View, SIGNAL(triggered()), this, SLOT(loadSlugsView())); + + //GlobalOperatorView + // connect(ui.actionGlobalOperatorView,SIGNAL(triggered()),waypointsDockWidget->widget(),SLOT()) + +} + +*/ diff --git a/src/ui/MainWindow.h b/src/ui/MainWindow.h index 646dc0a..231c979 100644 --- a/src/ui/MainWindow.h +++ b/src/ui/MainWindow.h @@ -133,9 +133,6 @@ public slots: void loadDataView(); /** @brief Load data view, allowing to plot flight data */ void loadDataView(QString fileName); - /** @brief Load view for global operator, allowing to flight on earth */ - void loadGlobalOperatorView(); - /** @brief Show the online help for users */ void showHelp(); /** @brief Show the authors / credits */ @@ -145,7 +142,9 @@ public slots: // Fixme find a nicer solution that scales to more AP types void loadSlugsView(); - void loadPixhawkView(); + + void loadPixhawkEngineerView(); + void loadSlugsEngineerView(); /** @brief Reload the CSS style sheet */ void reloadStylesheet(); @@ -153,13 +152,31 @@ protected: QStatusBar* statusBar; QStatusBar* createStatusBar(); void loadWidgets(); - void connectActions(); + void clearView(); - void buildWidgets(); - void connectWidgets(); - void arrangeCenterStack(); + + void buildCommonWidgets(); + void buildPxWidgets(); + void buildSlugsWidgets(); + + void connectCommonWidgets(); + void connectPxWidgets(); + void connectSlugsWidgets(); + + void arrangeCommonCenterStack(); + void arrangePxCenterStack(); + void arrangeSlugsCenterStack(); + + void connectCommonActions(); + void connectPxActions(); + void connectSlugsActions(); + + void configureWindowName(); + void buildHelpMenu (); + void buildViewsMenu (); + // TODO Should be moved elsewhere, as the protocol does not belong to the UI MAVLinkProtocol* mavlink; AS4Protocol* as4link; @@ -168,6 +185,8 @@ protected: LinkInterface* udpLink; QSettings settings; + QStackedWidget *centerStack; + // Center widgets QPointer linechartWidget; QPointer hudWidget; @@ -215,6 +234,17 @@ protected: QAction* stopUASAct; QAction* killUASAct; QAction* simulateUASAct; + QAction* actionOnline_documentation; + QAction* actionProject_Roadmap; + QAction* actionCredits_Developers; + QAction* actionCalibrationView; + QAction* actionEngineerView; + QAction* actionFlightView; + QAction* actionMavlinkView; + QAction* actionReloadStyle; + + QMenu* helpMenu; + QMenu* viewsMenu; LogCompressor* comp; QString screenFileName; diff --git a/src/ui/MainWindow.ui b/src/ui/MainWindow.ui index 323c51a..f6d1513 100644 --- a/src/ui/MainWindow.ui +++ b/src/ui/MainWindow.ui @@ -45,22 +45,11 @@ File - + - - - Unmanned System - - - - - - - - Network @@ -68,43 +57,35 @@ - + - Window + Select System - - - - - - - - - - - - - - + - Help + Unmanned System - - - + + false + + + + + + + - + - Select System + Tools - - + @@ -218,6 +199,9 @@ Joystick settings + + true + @@ -348,6 +332,18 @@ Show Slugs View + + + + :/images/devices/input-gaming.svg:/images/devices/input-gaming.svg + + + Joystick Settings + + + false + + diff --git a/src/ui/QGCRemoteControlView.cc b/src/ui/QGCRemoteControlView.cc index 77aa649..106992f 100644 --- a/src/ui/QGCRemoteControlView.cc +++ b/src/ui/QGCRemoteControlView.cc @@ -115,10 +115,10 @@ void QGCRemoteControlView::setUASId(int id) connect(newUAS, SIGNAL(remoteControlRSSIChanged(float)), this, SLOT(setRemoteRSSI(float))); connect(newUAS, SIGNAL(remoteControlChannelRawChanged(int,float)), this, SLOT(setChannelRaw(int,float))); - connect(newUAS, SIGNAL(remoteControlChannelRawChanged(int,float)), calibrationWindow, SLOT(setChannelRaw(int,float))); +// connect(newUAS, SIGNAL(remoteControlChannelRawChanged(int,float)), calibrationWindow, SLOT(setChannelRaw(int,float))); connect(newUAS, SIGNAL(remoteControlChannelScaledChanged(int,float)), this, SLOT(setChannelScaled(int,float))); - connect(newUAS, SIGNAL(remoteControlChannelScaledChanged(int,float)), calibrationWindow, SLOT(setChannelScaled(int,float))); +// connect(newUAS, SIGNAL(remoteControlChannelScaledChanged(int,float)), calibrationWindow, SLOT(setChannelScaled(int,float))); } } From 18b0e07e3b0521d4f49bf21c7996254edd57b7e7 Mon Sep 17 00:00:00 2001 From: Mariano Lizarraga Date: Sat, 11 Dec 2010 17:00:35 -0600 Subject: [PATCH 11/21] VERY EXPERIMENTAL. Saving of location for the common widgets now working and persisted across sessions. Needs to be extended to the individual autopilot widgets --- src/comm/MAVLinkSimulationLink.cc | 2 +- src/ui/MainWindow.cc | 270 +++++++++++++++++++++++++++----------- src/ui/MainWindow.h | 79 +++++++++-- src/ui/MainWindow.ui | 94 +++++++++++++ 4 files changed, 355 insertions(+), 90 deletions(-) diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc index 4d89666..d5647da 100644 --- a/src/comm/MAVLinkSimulationLink.cc +++ b/src/comm/MAVLinkSimulationLink.cc @@ -659,7 +659,7 @@ qint64 MAVLinkSimulationLink::bytesAvailable() void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size) { - qDebug() << "Simulation received " << size << " bytes from groundstation: "; + //qDebug() << "Simulation received " << size << " bytes from groundstation: "; // Increase write counter //bitsSentTotal += size * 8; diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index 2b138dc..5e62395 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -46,6 +46,7 @@ **/ MainWindow::MainWindow(QWidget *parent) : QMainWindow(parent), + toolsMenuActions(), settings() { this->hide(); @@ -89,33 +90,39 @@ MainWindow::~MainWindow() } + void MainWindow::buildCommonWidgets() { //TODO: move protocol outside UI mavlink = new MAVLinkProtocol(); - // Center widgets - mapWidget = new MapWidget(this); - protocolWidget = new XMLCommProtocolWidget(this); - // Dock widgets controlDockWidget = new QDockWidget(tr("Control"), this); controlDockWidget->setWidget( new UASControlWidget(this) ); + addToToolsMenu (controlDockWidget, tr("UAS Control"), SLOT(showToolWidget()), MENU_UAS_CONTROL, Qt::LeftDockWidgetArea); listDockWidget = new QDockWidget(tr("Unmanned Systems"), this); listDockWidget->setWidget( new UASListWidget(this) ); + addToToolsMenu (listDockWidget, tr("UAS List"), SLOT(showToolWidget()), MENU_UAS_LIST, Qt::RightDockWidgetArea); waypointsDockWidget = new QDockWidget(tr("Waypoint List"), this); waypointsDockWidget->setWidget( new WaypointList(this, NULL) ); + addToToolsMenu (waypointsDockWidget, tr("Waypoints List"), SLOT(showToolWidget()), MENU_WAYPOINTS, Qt::BottomDockWidgetArea); infoDockWidget = new QDockWidget(tr("Status Details"), this); infoDockWidget->setWidget( new UASInfoWidget(this) ); + addToToolsMenu (infoDockWidget, tr("Status Details"), SLOT(showToolWidget()), MENU_STATUS, Qt::RightDockWidgetArea); + debugConsoleDockWidget = new QDockWidget(tr("Communication Console"), this); debugConsoleDockWidget->setWidget( new DebugConsole(this) ); + addToToolsMenu (debugConsoleDockWidget, tr("Communication Console"), SLOT(showToolWidget()), MENU_DEBUG_CONSOLE, Qt::BottomDockWidgetArea); -} + // Center widgets + mapWidget = new MapWidget(this); + protocolWidget = new XMLCommProtocolWidget(this); +} void MainWindow::buildPxWidgets() { //FIXME: memory of acceptList will never be freed again @@ -151,27 +158,37 @@ void MainWindow::buildPxWidgets() // Dock widgets detectionDockWidget = new QDockWidget(tr("Object Recognition"), this); detectionDockWidget->setWidget( new ObjectDetectionView("images/patterns", this) ); + addToToolsMenu (detectionDockWidget, tr("Object Recognition"), SLOT(showToolWidget()), MENU_DETECTION, Qt::RightDockWidgetArea); + parametersDockWidget = new QDockWidget(tr("Onboard Parameters"), this); parametersDockWidget->setWidget( new ParameterInterface(this) ); + addToToolsMenu (parametersDockWidget, tr("Onboard Parameters"), SLOT(showToolWidget()), MENU_PARAMETERS, Qt::RightDockWidgetArea); watchdogControlDockWidget = new QDockWidget(tr("Process Control"), this); watchdogControlDockWidget->setWidget( new WatchdogControl(this) ); + addToToolsMenu (watchdogControlDockWidget, tr("Process Control"), SLOT(showToolWidget()), MENU_WATCHDOG, Qt::BottomDockWidgetArea); + hsiDockWidget = new QDockWidget(tr("Horizontal Situation Indicator"), this); hsiDockWidget->setWidget( new HSIDisplay(this) ); + addToToolsMenu (hsiDockWidget, tr("HSI"), SLOT(showToolWidget()), MENU_HSI, Qt::BottomDockWidgetArea); headDown1DockWidget = new QDockWidget(tr("Primary Flight Display"), this); headDown1DockWidget->setWidget( new HDDisplay(acceptList, this) ); + addToToolsMenu (headDown1DockWidget, tr("Flight Display"), SLOT(showToolWidget()), MENU_HDD_1, Qt::RightDockWidgetArea); headDown2DockWidget = new QDockWidget(tr("Payload Status"), this); headDown2DockWidget->setWidget( new HDDisplay(acceptList2, this) ); + addToToolsMenu (headDown2DockWidget, tr("Payload Status"), SLOT(showToolWidget()), MENU_HDD_2, Qt::RightDockWidgetArea); rcViewDockWidget = new QDockWidget(tr("Radio Control"), this); rcViewDockWidget->setWidget( new QGCRemoteControlView(this) ); + addToToolsMenu (rcViewDockWidget, tr("Radio Control"), SLOT(showToolWidget()), MENU_RC_VIEW, Qt::BottomDockWidgetArea); - headUpDockWidget = new QDockWidget(tr("Control Indicator"), this); + headUpDockWidget = new QDockWidget(tr("HUD"), this); headUpDockWidget->setWidget( new HUD(320, 240, this)); + addToToolsMenu (headUpDockWidget, tr("Control Indicator"), SLOT(showToolWidget()), MENU_HUD, Qt::LeftDockWidgetArea); // Dialogue widgets //FIXME: free memory in destructor @@ -183,32 +200,154 @@ void MainWindow::buildSlugsWidgets() { // Center widgets linechartWidget = new Linecharts(this); - hudWidget = new HUD(320, 240, this); // Dock widgets + headUpDockWidget = new QDockWidget(tr("Control Indicator"), this); + headUpDockWidget->setWidget( new HUD(320, 240, this)); + addToToolsMenu (headUpDockWidget, tr("HUD"), SLOT(showToolWidget()), MENU_HUD, Qt::LeftDockWidgetArea); + + rcViewDockWidget = new QDockWidget(tr("Radio Control"), this); rcViewDockWidget->setWidget( new QGCRemoteControlView(this) ); + addToToolsMenu (rcViewDockWidget, tr("Radio Control"), SLOT(showToolWidget()), MENU_RC_VIEW, Qt::BottomDockWidgetArea); - headUpDockWidget = new QDockWidget(tr("Control Indicator"), this); - headUpDockWidget->setWidget( new HUD(320, 240, this)); // Dialog widgets slugsDataWidget = new QDockWidget(tr("Slugs Data"), this); slugsDataWidget->setWidget( new SlugsDataSensorView(this)); + addToToolsMenu (slugsDataWidget, tr("Telemetry Data"), SLOT(showToolWidget()), MENU_SLUGS_DATA, Qt::RightDockWidgetArea); + slugsPIDControlWidget = new QDockWidget(tr("PID Control"), this); slugsPIDControlWidget->setWidget(new SlugsPIDControl(this)); + addToToolsMenu (slugsPIDControlWidget, tr("PID Configuration"), SLOT(showToolWidget()), MENU_SLUGS_PID, Qt::LeftDockWidgetArea); slugsHilSimWidget = new QDockWidget(tr("Slugs Hil Sim"), this); slugsHilSimWidget->setWidget( new SlugsHilSim(this)); + addToToolsMenu (slugsHilSimWidget, tr("HIL Sim Configuration"), SLOT(showToolWidget()), MENU_SLUGS_HIL, Qt::LeftDockWidgetArea); slugsCamControlWidget = new QDockWidget(tr("Video Camera Control"), this); slugsCamControlWidget->setWidget(new SlugsVideoCamControl(this)); + addToToolsMenu (slugsCamControlWidget, tr("Camera Control"), SLOT(showToolWidget()), MENU_SLUGS_CAMERA, Qt::BottomDockWidgetArea); } /** * Connect the signals and slots of the common window widgets */ + +void MainWindow::addToToolsMenu ( QWidget* widget, + const QString title, + const char * slotName, + TOOLS_WIDGET_NAMES tool, + Qt::DockWidgetArea location){ + QAction* tempAction; + QString posKey, chKey; + tempAction = ui.menuTools->addAction(title); + tempAction->setCheckable(true); + tempAction->setData(tool); + + // populate the Hashes + toolsMenuActions[tool] = tempAction; + dockWidgets[tool] = widget; + + // Based on Settings check/uncheck + // Postion key = menu/widget/position/pos_value + posKey = buildMenuKey (SUB_SECTION_LOCATION,tool); + + if (!settings.contains(posKey)){ + settings.setValue(posKey,location); + dockWidgetLocations[tool] = location; + } else { + dockWidgetLocations[tool] = static_cast (settings.value(posKey).toInt()); + } + + // Checked key = menu/widget/checked/pos_value + chKey = buildMenuKey(SUB_SECTION_CHECKED,tool); + + if (!settings.contains(chKey)){ + settings.setValue(chKey,false); + tempAction->setChecked(false); + } else { + tempAction->setChecked(settings.value(chKey).toBool()); + } + + // connect the action + connect(tempAction,SIGNAL(triggered()),this, slotName); + + connect(static_cast (dockWidgets[tool]), + SIGNAL(visibilityChanged(bool)), this, SLOT(updateVisibilitySettings(bool))); + + connect(static_cast (dockWidgets[tool]), + SIGNAL(dockLocationChanged(Qt::DockWidgetArea)), this, SLOT(updateLocationSettings(Qt::DockWidgetArea))); + +} + +QString MainWindow::buildMenuKey(SETTINGS_SECTIONS section, TOOLS_WIDGET_NAMES tool){ + return (QString::number(SECTION_MENU) + "/" + + QString::number(tool) + "/" + + QString::number(section) + "/" ); +} + + +void MainWindow::showToolWidget(){ + QAction* temp = qobject_cast(sender()); + int tool = temp->data().toInt(); + + if (temp && dockWidgets[tool]){ + if (temp->isChecked()){ + addDockWidget(dockWidgetLocations[tool], static_cast (dockWidgets[tool])); + static_cast (dockWidgets[tool])->show(); + } else { + removeDockWidget(static_cast (dockWidgets[tool])); + } + QString chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(tool)); + settings.setValue(chKey,temp->isChecked()); + } +} + + +void MainWindow::updateVisibilitySettings (bool vis){ + Q_UNUSED(vis); + + // This is commented since when the application closes + // sets the visibility to false. + + // TODO: A workaround is needed. The QApplication::aboutToQuit + // did not work + + /* + QDockWidget* temp = qobject_cast(sender()); + + QHashIterator i(dockWidgets); + while (i.hasNext()) { + i.next(); + if ((static_cast (dockWidgets[i.key()])) == temp){ + QString chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(i.key())); + qDebug() << "Key in visibility changed" << chKey; + settings.setValue(chKey,vis); + toolsMenuActions[i.key()]->setChecked(vis); + break; + } + } +*/ +} + +void MainWindow::updateLocationSettings (Qt::DockWidgetArea location){ + QDockWidget* temp = qobject_cast(sender()); + + QHashIterator i(dockWidgets); + while (i.hasNext()) { + i.next(); + if ((static_cast (dockWidgets[i.key()])) == temp){ + QString posKey = buildMenuKey (SUB_SECTION_LOCATION,static_cast(i.key())); + settings.setValue(posKey,location); + break; + } + } +} + + + void MainWindow::connectCommonWidgets() { if (infoDockWidget && infoDockWidget->widget()) @@ -278,8 +417,8 @@ void MainWindow::connectSlugsWidgets() void MainWindow::arrangeCommonCenterStack() { - centerStack = new QStackedWidget(this); + if (!centerStack) return; if (mapWidget) centerStack->addWidget(mapWidget); @@ -444,62 +583,9 @@ void MainWindow::showStatusMessage(const QString& status) * **/ -void MainWindow::buildHelpMenu (){ - QIcon icon; - - icon = QIcon(":/images/apps/utilities-system-monitor.svg"); - actionOnline_documentation = new QAction(icon,tr("Online Documentation"), this); - - icon = QIcon(":/images/status/software-update-available.svg"); - actionProject_Roadmap = new QAction(icon,tr("Project Roadmap"), this); - - icon = QIcon(":/images/categories/preferences-system.svg"); - actionCredits_Developers = new QAction(icon,tr("Developer Credits"), this); - - helpMenu = ui.menuBar->addMenu(tr("Help")); - - helpMenu->addAction(actionOnline_documentation); - helpMenu->addAction(actionProject_Roadmap); - helpMenu->addAction(actionCredits_Developers); - - -} - - -void MainWindow::buildViewsMenu (){ - QIcon icon; - - icon = QIcon(":/images/status/weather-overcast.svg"); - actionFlightView = new QAction(icon,"Flight View", this); - - icon = QIcon(":/images/apps/utilities-system-monitor.svg"); - actionEngineerView = new QAction(icon,"Engineer View", this); - - icon = QIcon(":/images/status/network-wireless-encrypted.svg"); - actionCalibrationView = new QAction(icon,"Calibration View", this); - - icon = QIcon(":/images/devices/network-wired.svg"); - actionMavlinkView = new QAction(icon,"Mavlink View", this); - - icon = QIcon(":/images/categories/applications-internet.svg"); - actionReloadStyle = new QAction(icon,"Reload Style", this); - - viewsMenu = ui.menuBar->addMenu("Views"); - - viewsMenu->addAction(actionFlightView); - viewsMenu->addAction(actionEngineerView); - viewsMenu->addAction(actionCalibrationView); - viewsMenu->addSeparator(); - viewsMenu->addAction(actionMavlinkView); - viewsMenu->addAction(actionReloadStyle); - -} - void MainWindow::connectCommonActions() { - buildViewsMenu(); - buildHelpMenu(); // Connect actions from ui connect(ui.actionAdd_Link, SIGNAL(triggered()), this, SLOT(addLink())); @@ -516,18 +602,17 @@ void MainWindow::connectCommonActions() connect(ui.actionConfiguration, SIGNAL(triggered()), UASManager::instance(), SLOT(configureActiveUAS())); // Views actions - connect(actionFlightView, SIGNAL(triggered()), this, SLOT(loadPilotView())); - connect(actionEngineerView, SIGNAL(triggered()), this, SLOT(loadEngineerView())); - connect(actionCalibrationView, SIGNAL(triggered()), this, SLOT(loadOperatorView())); + connect(ui.actionFlightView, SIGNAL(triggered()), this, SLOT(loadPilotView())); + connect(ui.actionEngineersView, SIGNAL(triggered()), this, SLOT(loadEngineerView())); + connect(ui.actionCalibrationView, SIGNAL(triggered()), this, SLOT(loadOperatorView())); - connect(actionMavlinkView, SIGNAL(triggered()), this, SLOT(loadMAVLinkView())); - connect(actionReloadStyle, SIGNAL(triggered()), this, SLOT(reloadStylesheet())); + connect(ui.actionMavlinkView, SIGNAL(triggered()), this, SLOT(loadMAVLinkView())); + connect(ui.actionReloadStyle, SIGNAL(triggered()), this, SLOT(reloadStylesheet())); // Help Actions - connect(actionOnline_documentation, SIGNAL(triggered()), this, SLOT(showHelp())); - connect(actionCredits_Developers, SIGNAL(triggered()), this, SLOT(showCredits())); - connect(actionProject_Roadmap, SIGNAL(triggered()), this, SLOT(showRoadMap())); - + connect(ui.actionOnline_documentation, SIGNAL(triggered()), this, SLOT(showHelp())); + connect(ui.actionDeveloper_Credits, SIGNAL(triggered()), this, SLOT(showCredits())); + connect(ui.actionProject_Roadmap, SIGNAL(triggered()), this, SLOT(showRoadMap())); } @@ -877,13 +962,16 @@ void MainWindow::loadPixhawkEngineerView() // 3D map if (_3DWidget) { - QStackedWidget *centerStack = dynamic_cast(centralWidget()); if (centerStack) { //map3DWidget->setActive(true); centerStack->setCurrentWidget(_3DWidget); } } +#else + if (centerStack && linechartWidget){ + centerStack->addWidget(linechartWidget); + } #endif // UAS CONTROL @@ -1290,9 +1378,13 @@ void MainWindow::load3DView() void MainWindow::loadEngineerView() { clearView(); - // Engineer view, used in EMAV2009 + // If there is no mav dont siwtch view + int selector = (UASManager::instance()->getActiveUAS())? + UASManager::instance()->getActiveUAS()->getAutopilotType() : + 255; + + switch (selector){ - switch (UASManager::instance()->getActiveUAS()->getAutopilotType()){ case (MAV_AUTOPILOT_GENERIC): case (MAV_AUTOPILOT_ARDUPILOTMEGA): case (MAV_AUTOPILOT_PIXHAWK): @@ -1320,9 +1412,35 @@ void MainWindow::loadMAVLinkView() } } + // TODO: Refactor this code to single function calls + + showTheWidget(MENU_UAS_CONTROL); + + showTheWidget(MENU_UAS_LIST); + + showTheWidget(MENU_WAYPOINTS); + + showTheWidget(MENU_STATUS); + + showTheWidget(MENU_DEBUG_CONSOLE); + this->show(); } +void MainWindow::showTheWidget (TOOLS_WIDGET_NAMES widget){ + bool tempVisible; + Qt::DockWidgetArea tempLocation; + QDockWidget* tempWidget = static_cast (dockWidgets[widget]); + + tempVisible = settings.value(buildMenuKey (SUB_SECTION_CHECKED,widget)).toBool(); + tempLocation = static_cast (settings.value(buildMenuKey (SUB_SECTION_LOCATION,widget)).toInt()); + + if (tempWidget && tempVisible){ + addDockWidget(tempLocation, tempWidget); + tempWidget->show(); + } +} + void MainWindow::loadAllView() { clearView(); @@ -1415,7 +1533,7 @@ void MainWindow::loadAllView() void MainWindow::loadWidgets() { //loadOperatorView(); - loadEngineerView(); + loadMAVLinkView(); //loadPilotView(); } diff --git a/src/ui/MainWindow.h b/src/ui/MainWindow.h index 231c979..ee1ed25 100644 --- a/src/ui/MainWindow.h +++ b/src/ui/MainWindow.h @@ -148,7 +148,70 @@ public slots: /** @brief Reload the CSS style sheet */ void reloadStylesheet(); + + + void showToolWidget(); + void updateVisibilitySettings (bool vis); + void updateLocationSettings (Qt::DockWidgetArea location); protected: + + // These defines are used to save the settings when selecting with + // which widgets populate the views + // FIXME: DO NOT PUT CUSTOM VALUES IN THIS ENUM since it is iterated over + // this will be fixed in a future release. + typedef enum _TOOLS_WIDGET_NAMES { + MENU_UAS_CONTROL, + MENU_UAS_INFO, + MENU_CAMERA, + MENU_UAS_LIST, + MENU_WAYPOINTS, + MENU_STATUS, + MENU_DETECTION, + MENU_DEBUG_CONSOLE, + MENU_PARAMETERS, + MENU_HDD_1, + MENU_HDD_2, + MENU_WATCHDOG, + MENU_HUD, + MENU_HSI, + MENU_RC_VIEW, + MENU_SLUGS_DATA, + MENU_SLUGS_PID, + MENU_SLUGS_HIL, + MENU_SLUGS_CAMERA, + CENTRAL_LINECHART = 255, // Separation from dockwidgets and central widgets + CENTRAL_PROTOCOL, + CENTRAL_MAP, + CENTRAL_3D_LOCAL, + CENTRAL_3D_MAP, + CENTRAL_GOOGLE_EARTH, + CENTRAL_HUD, + CENTRAL_DATA_PLOT, + }TOOLS_WIDGET_NAMES; + + typedef enum _SETTINGS_SECTIONS { + SECTION_MENU, + VIEW_ENGINEER, + VIEW_OPERATOR, + VIEW_CALIBRATION, + VIEW_MAVLINK, + SUB_SECTION_CHECKED, + SUB_SECTION_LOCATION, + } SETTINGS_SECTIONS; + + + QHash toolsMenuActions; // Holds ptr to the Menu Actions + QHash dockWidgets; // Holds ptr to the Actual Dock widget + QHash dockWidgetLocations; // Holds the location + + + void addToToolsMenu (QWidget* widget, const QString title, const char * slotName, TOOLS_WIDGET_NAMES tool, Qt::DockWidgetArea location); + void showTheWidget (TOOLS_WIDGET_NAMES widget); + + int currentView; + int aboutToQuit; + //QHash settingsSections; + QStatusBar* statusBar; QStatusBar* createStatusBar(); void loadWidgets(); @@ -174,8 +237,6 @@ protected: void configureWindowName(); - void buildHelpMenu (); - void buildViewsMenu (); // TODO Should be moved elsewhere, as the protocol does not belong to the UI MAVLinkProtocol* mavlink; @@ -234,17 +295,7 @@ protected: QAction* stopUASAct; QAction* killUASAct; QAction* simulateUASAct; - QAction* actionOnline_documentation; - QAction* actionProject_Roadmap; - QAction* actionCredits_Developers; - QAction* actionCalibrationView; - QAction* actionEngineerView; - QAction* actionFlightView; - QAction* actionMavlinkView; - QAction* actionReloadStyle; - - QMenu* helpMenu; - QMenu* viewsMenu; + LogCompressor* comp; QString screenFileName; @@ -253,6 +304,8 @@ protected: private: Ui::MainWindow ui; + QString buildMenuKey (SETTINGS_SECTIONS section , TOOLS_WIDGET_NAMES tool); + }; #endif /* _MAINWINDOW_H_ */ diff --git a/src/ui/MainWindow.ui b/src/ui/MainWindow.ui index f6d1513..a60e53c 100644 --- a/src/ui/MainWindow.ui +++ b/src/ui/MainWindow.ui @@ -80,12 +80,34 @@ Tools + + + + + Help + + + + + + + + Perspectives + + + + + + + + + @@ -344,6 +366,78 @@ false + + + + :/images/apps/utilities-system-monitor.svg:/images/apps/utilities-system-monitor.svg + + + Online Documentation + + + + + + :/images/status/software-update-available.svg:/images/status/software-update-available.svg + + + Project Roadmap + + + + + + :/images/categories/preferences-system.svg:/images/categories/preferences-system.svg + + + Developer Credits + + + + + + :/images/status/weather-overcast.svg:/images/status/weather-overcast.svg + + + Flight + + + + + + :/images/apps/utilities-system-monitor.svg:/images/apps/utilities-system-monitor.svg + + + Engineer + + + + + + :/images/devices/network-wired.svg:/images/devices/network-wired.svg + + + Mavlink + + + + + + :/images/categories/applications-internet.svg:/images/categories/applications-internet.svg + + + Reload Style + + + + + + :/images/status/network-wireless-encrypted.svg:/images/status/network-wireless-encrypted.svg + + + Calibration + + From 8591b8550ab262405e62eee155926855b7ac4e30 Mon Sep 17 00:00:00 2001 From: Mariano Lizarraga Date: Mon, 13 Dec 2010 18:40:40 -0600 Subject: [PATCH 12/21] Still working on having independent builds for each MAV, most of the work is done, only Central Widgets remain to be instantiated and menu-controlled based on the autopilot --- src/comm/MAVLinkProtocol.cc | 28 +++ src/uas/UAS.cc | 4 +- src/uas/UAS.h | 5 +- src/uas/UASInterface.h | 4 +- src/uas/UASManager.cc | 5 + src/uas/UASManager.h | 1 + src/ui/MainWindow.cc | 577 ++++++++++++++++++++++---------------------- src/ui/MainWindow.h | 29 ++- src/ui/MainWindow.ui | 13 +- 9 files changed, 361 insertions(+), 305 deletions(-) diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index 8350641..69cd6fd 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -159,7 +159,13 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) switch (heartbeat.autopilot) { case MAV_AUTOPILOT_GENERIC: + uas = new UAS(this, message.sysid); + + // Set the autopilot type + uas->setAutopilotType((int)heartbeat.autopilot); + + // Connect this robot to the UAS object connect(this, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), uas, SLOT(receiveMessage(LinkInterface*, mavlink_message_t))); break; @@ -167,6 +173,10 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) { // Fixme differentiate between quadrotor and coaxial here PxQuadMAV* mav = new PxQuadMAV(this, message.sysid); + + // Set the autopilot type + //mav->setAutopilotType((int)heartbeat.autopilot); + // Connect this robot to the UAS object // it is IMPORTANT here to use the right object type, // else the slot of the parent object is called (and thus the special @@ -178,6 +188,10 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) case MAV_AUTOPILOT_SLUGS: { SlugsMAV* mav = new SlugsMAV(this, message.sysid); + + // Set the autopilot type + mav->setAutopilotType((int)heartbeat.autopilot); + // Connect this robot to the UAS object // it is IMPORTANT here to use the right object type, // else the slot of the parent object is called (and thus the special @@ -189,6 +203,10 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) case MAV_AUTOPILOT_ARDUPILOTMEGA: { ArduPilotMegaMAV* mav = new ArduPilotMegaMAV(this, message.sysid); + + // Set the autopilot type + mav->setAutopilotType((int)heartbeat.autopilot); + // Connect this robot to the UAS object // it is IMPORTANT here to use the right object type, // else the slot of the parent object is called (and thus the special @@ -202,11 +220,21 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) break; } + // Set the autopilot type + uas->setAutopilotType((int)heartbeat.autopilot); // Make UAS aware that this link can be used to communicate with the actual robot uas->addLink(link); + // Now add UAS to "official" list, which makes the whole application aware of it UASManager::instance()->addUAS(uas); + + + + qDebug() << "++==============================="; + qDebug() << uas->getAutopilotType(); + qDebug() << "++==============================="; + } // Only count message if UAS exists for this message diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 0ad31fd..2eadd5b 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -44,12 +44,13 @@ This file is part of the QGROUNDCONTROL project #include "MAVLinkProtocol.h" #include "QGCMAVLink.h" + UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(), uasId(id), startTime(MG::TIME::getGroundTimeNow()), commStatus(COMM_DISCONNECTED), name(""), - autopilot(0), + autopilot(-1), links(new QList()), unknownPackets(), mavlink(protocol), @@ -153,6 +154,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) this->autopilot = mavlink_msg_heartbeat_get_autopilot(&message); emit systemTypeSet(this, type); } + break; case MAVLINK_MSG_ID_BOOT: getStatusForCode((int)MAV_STATE_BOOT, uasState, stateDescription); diff --git a/src/uas/UAS.h b/src/uas/UAS.h index 0926887..65b514b 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -98,7 +98,7 @@ protected: quint64 startTime; ///< The time the UAS was switched on CommStatus commStatus; ///< Communication status QString name; ///< Human-friendly name of the vehicle, e.g. bravo - int autopilot; ///< Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM + int autopilot; ///< Type of the Autopilot: -1: None, 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM QList* links; ///< List of links this UAS can be reached by QList unknownPackets; ///< Packet IDs which are unknown and have been received MAVLinkProtocol* mavlink; ///< Reference to the MAVLink instance @@ -165,7 +165,8 @@ protected: public: UASWaypointManager &getWaypointManager(void) { return waypointManager; } int getSystemType(); - unsigned char getAutopilotType() {return autopilot;} + int getAutopilotType() {return autopilot;} + void setAutopilotType(int apType) { autopilot = apType;} public slots: /** @brief Launches the system **/ diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index c8266ac..332f90e 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -157,7 +157,9 @@ public: return color; } - virtual unsigned char getAutopilotType() = 0; + virtual int getAutopilotType() = 0; + virtual void setAutopilotType(int apType)= 0; + public slots: /** @brief Launches the system/Liftof **/ diff --git a/src/uas/UASManager.cc b/src/uas/UASManager.cc index bba66dd..2af5a5f 100644 --- a/src/uas/UASManager.cc +++ b/src/uas/UASManager.cc @@ -96,6 +96,11 @@ UASInterface* UASManager::getActiveUAS() return activeUAS; ///< Return zero pointer if no UAS has been loaded } +UASInterface* UASManager::silentGetActiveUAS() +{ + return activeUAS; ///< Return zero pointer if no UAS has been loaded +} + bool UASManager::launchActiveUAS() { // If the active UAS is set, execute command diff --git a/src/uas/UASManager.h b/src/uas/UASManager.h index 7b8dabf..c5ca03c 100644 --- a/src/uas/UASManager.h +++ b/src/uas/UASManager.h @@ -57,6 +57,7 @@ public: * @return NULL pointer if no UAS exists, active UAS else **/ UASInterface* getActiveUAS(); + UASInterface* silentGetActiveUAS(); /** * @brief Get the UAS with this id * diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index 5e62395..c6e9631 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -47,6 +47,7 @@ MainWindow::MainWindow(QWidget *parent) : QMainWindow(parent), toolsMenuActions(), + currentView(VIEW_MAVLINK), settings() { this->hide(); @@ -76,8 +77,8 @@ MainWindow::MainWindow(QWidget *parent) : // Create actions connectCommonActions(); - // Load widgets and show application windowa - loadWidgets(); + // Load mavlink view as default widget set + loadMAVLinkView(); // Adjust the size adjustSize(); @@ -120,7 +121,11 @@ void MainWindow::buildCommonWidgets() // Center widgets mapWidget = new MapWidget(this); + addToCentralWidgetsMenu (mapWidget, "Maps", SLOT(showCentralWidget()),CENTRAL_MAP); + protocolWidget = new XMLCommProtocolWidget(this); + addToCentralWidgetsMenu (protocolWidget, "Mavlink Generator", SLOT(showCentralWidget()),CENTRAL_PROTOCOL); + } void MainWindow::buildPxWidgets() @@ -231,9 +236,76 @@ void MainWindow::buildSlugsWidgets() addToToolsMenu (slugsCamControlWidget, tr("Camera Control"), SLOT(showToolWidget()), MENU_SLUGS_CAMERA, Qt::BottomDockWidgetArea); } -/** - * Connect the signals and slots of the common window widgets - */ + + +void MainWindow::addToCentralWidgetsMenu ( QWidget* widget, + const QString title, + const char * slotName, + TOOLS_WIDGET_NAMES centralWidget){ + QAction* tempAction; + + // Add the separator that will separate tools from central Widgets + if (!toolsMenuActions[CENTRAL_SEPARATOR]){ + tempAction = ui.menuTools->addSeparator(); + toolsMenuActions[CENTRAL_SEPARATOR] = tempAction; + } + + tempAction = ui.menuTools->addAction(title); + + tempAction->setCheckable(true); + tempAction->setData(centralWidget); + + // populate the Hashes + toolsMenuActions[centralWidget] = tempAction; + dockWidgets[centralWidget] = widget; + + QString chKey = buildMenuKey(SUB_SECTION_CHECKED, centralWidget, currentView); + + if (!settings.contains(chKey)){ + settings.setValue(chKey,false); + tempAction->setChecked(false); + } else { + tempAction->setChecked(settings.value(chKey).toBool()); + } + + // connect the action + connect(tempAction,SIGNAL(triggered()),this, slotName); + +} + + +void MainWindow::showCentralWidget(){ + QAction* senderAction = qobject_cast(sender()); + int tool = senderAction->data().toInt(); + QString chKey; + + if (senderAction && dockWidgets[tool]){ + + // uncheck all central widget actions + QHashIterator i(toolsMenuActions); + while (i.hasNext()) { + i.next(); + + if (i.value()->data().toInt() >= 255){ + i.value()->setChecked(false); + + // update the settings + chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(i.value()->data().toInt()), currentView); + settings.setValue(chKey,false); + } + } + + // check the current action + senderAction->setChecked(true); + + // update the central widget + centerStack->setCurrentWidget(dockWidgets[tool]); + + // store the selected central widget + chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(tool), currentView); + settings.setValue(chKey,true); + } +} void MainWindow::addToToolsMenu ( QWidget* widget, const QString title, @@ -242,7 +314,15 @@ void MainWindow::addToToolsMenu ( QWidget* widget, Qt::DockWidgetArea location){ QAction* tempAction; QString posKey, chKey; - tempAction = ui.menuTools->addAction(title); + + if (toolsMenuActions[CENTRAL_SEPARATOR]){ + tempAction = new QAction(title, this); + ui.menuTools->insertAction(toolsMenuActions[CENTRAL_SEPARATOR], + tempAction); + } else { + tempAction = ui.menuTools->addAction(title); + } + tempAction->setCheckable(true); tempAction->setData(tool); @@ -250,9 +330,7 @@ void MainWindow::addToToolsMenu ( QWidget* widget, toolsMenuActions[tool] = tempAction; dockWidgets[tool] = widget; - // Based on Settings check/uncheck - // Postion key = menu/widget/position/pos_value - posKey = buildMenuKey (SUB_SECTION_LOCATION,tool); + posKey = buildMenuKey (SUB_SECTION_LOCATION,tool, currentView); if (!settings.contains(posKey)){ settings.setValue(posKey,location); @@ -261,8 +339,7 @@ void MainWindow::addToToolsMenu ( QWidget* widget, dockWidgetLocations[tool] = static_cast (settings.value(posKey).toInt()); } - // Checked key = menu/widget/checked/pos_value - chKey = buildMenuKey(SUB_SECTION_CHECKED,tool); + chKey = buildMenuKey(SUB_SECTION_CHECKED,tool, currentView); if (!settings.contains(chKey)){ settings.setValue(chKey,false); @@ -282,8 +359,17 @@ void MainWindow::addToToolsMenu ( QWidget* widget, } -QString MainWindow::buildMenuKey(SETTINGS_SECTIONS section, TOOLS_WIDGET_NAMES tool){ - return (QString::number(SECTION_MENU) + "/" + +QString MainWindow::buildMenuKey(SETTINGS_SECTIONS section, TOOLS_WIDGET_NAMES tool, VIEW_SECTIONS view){ + // Key is built as follows: autopilot_type/section_menu/view/tool/section + int apType; + + apType = (UASManager::instance() && UASManager::instance()->silentGetActiveUAS())? + UASManager::instance()->getActiveUAS()->getAutopilotType(): + -1; + + return (QString::number(apType) + "/" + + QString::number(SECTION_MENU) + "/" + + QString::number(view) + "/" + QString::number(tool) + "/" + QString::number(section) + "/" ); } @@ -300,7 +386,7 @@ void MainWindow::showToolWidget(){ } else { removeDockWidget(static_cast (dockWidgets[tool])); } - QString chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(tool)); + QString chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(tool), currentView); settings.setValue(chKey,temp->isChecked()); } } @@ -339,15 +425,16 @@ void MainWindow::updateLocationSettings (Qt::DockWidgetArea location){ while (i.hasNext()) { i.next(); if ((static_cast (dockWidgets[i.key()])) == temp){ - QString posKey = buildMenuKey (SUB_SECTION_LOCATION,static_cast(i.key())); + QString posKey = buildMenuKey (SUB_SECTION_LOCATION,static_cast(i.key()), currentView); settings.setValue(posKey,location); break; } } } - - +/** + * Connect the signals and slots of the common window widgets + */ void MainWindow::connectCommonWidgets() { if (infoDockWidget && infoDockWidget->widget()) @@ -362,16 +449,9 @@ void MainWindow::connectCommonWidgets() connect(waypointsDockWidget->widget(), SIGNAL(clearPathclicked()), mapWidget, SLOT(clearPath())); // add Waypoint widget in the WaypointList widget when mouse clicked connect(mapWidget, SIGNAL(captureMapCoordinateClick(QPointF)), waypointsDockWidget->widget(), SLOT(addWaypointMouse(QPointF))); - // it notifies that a waypoint global goes to do create - //connect(mapWidget, SIGNAL(createGlobalWP(bool, QPointF)), waypointsDockWidget->widget(), SLOT(setIsWPGlobal(bool, QPointF))); - //connect(mapWidget, SIGNAL(sendGeometryEndDrag(QPointF,int)), waypointsDockWidget->widget(), SLOT(waypointGlobalChanged(QPointF,int)) ); // it notifies that a waypoint global goes to do create and a map graphic too connect(waypointsDockWidget->widget(), SIGNAL(createWaypointAtMap(QPointF)), mapWidget, SLOT(createWaypointGraphAtMap(QPointF))); - // it notifies that a waypoint global change it¥s position by spinBox on Widget WaypointView - //connect(waypointsDockWidget->widget(), SIGNAL(changePositionWPGlobalBySpinBox(int,float,float)), mapWidget, SLOT(changeGlobalWaypointPositionBySpinBox(int,float,float))); - // connect(waypointsDockWidget->widget(), SIGNAL(changePositionWPGlobalBySpinBox(int,float,float)), mapWidget, SLOT(changeGlobalWaypointPositionBySpinBox(int,float,float))); - } } @@ -582,8 +662,6 @@ void MainWindow::showStatusMessage(const QString& status) * @brief Create all actions associated to the main window * **/ - - void MainWindow::connectCommonActions() { @@ -602,9 +680,9 @@ void MainWindow::connectCommonActions() connect(ui.actionConfiguration, SIGNAL(triggered()), UASManager::instance(), SLOT(configureActiveUAS())); // Views actions - connect(ui.actionFlightView, SIGNAL(triggered()), this, SLOT(loadPilotView())); + connect(ui.actionPilotsView, SIGNAL(triggered()), this, SLOT(loadPilotView())); connect(ui.actionEngineersView, SIGNAL(triggered()), this, SLOT(loadEngineerView())); - connect(ui.actionCalibrationView, SIGNAL(triggered()), this, SLOT(loadOperatorView())); + connect(ui.actionOperatorsView, SIGNAL(triggered()), this, SLOT(loadOperatorView())); connect(ui.actionMavlinkView, SIGNAL(triggered()), this, SLOT(loadMAVLinkView())); connect(ui.actionReloadStyle, SIGNAL(triggered()), this, SLOT(reloadStylesheet())); @@ -844,21 +922,25 @@ void MainWindow::UASCreated(UASInterface* uas) */ void MainWindow::clearView() { - // Halt HUD + // Halt HUD central widget if (hudWidget) hudWidget->stop(); + // Disable linechart if (linechartWidget) linechartWidget->setActive(false); + // Halt HDDs if (headDown1DockWidget) { HDDisplay* hddWidget = dynamic_cast( headDown1DockWidget->widget() ); if (hddWidget) hddWidget->stop(); } + if (headDown2DockWidget) { HDDisplay* hddWidget = dynamic_cast( headDown2DockWidget->widget() ); if (hddWidget) hddWidget->stop(); } + // Halt HSI if (hsiDockWidget) { @@ -866,6 +948,13 @@ void MainWindow::clearView() if (hsi) hsi->stop(); } + // Halt HUD if in docked widget mode + if (headUpDockWidget) + { + HUD* hud = dynamic_cast( headUpDockWidget->widget() ); + if (hud) hud->stop(); + } + // Remove all dock widgets from main window QObjectList childList( this->children() ); @@ -955,80 +1044,7 @@ void MainWindow::loadSlugsView() void MainWindow::loadPixhawkEngineerView() { - clearView(); - // Engineer view, used in EMAV2009 - -#ifdef QGC_OSG_ENABLED - // 3D map - if (_3DWidget) - { - if (centerStack) - { - //map3DWidget->setActive(true); - centerStack->setCurrentWidget(_3DWidget); - } - } -#else - if (centerStack && linechartWidget){ - centerStack->addWidget(linechartWidget); - } -#endif - - // UAS CONTROL - if (controlDockWidget) - { - addDockWidget(Qt::LeftDockWidgetArea, controlDockWidget); - controlDockWidget->show(); - } - - // HORIZONTAL SITUATION INDICATOR - if (hsiDockWidget) - { - HSIDisplay* hsi = dynamic_cast( hsiDockWidget->widget() ); - if (hsi) - { - hsi->start(); - addDockWidget(Qt::LeftDockWidgetArea, hsiDockWidget); - hsiDockWidget->show(); - } - } - - // UAS LIST - if (listDockWidget) - { - addDockWidget(Qt::BottomDockWidgetArea, listDockWidget); - listDockWidget->show(); - } - - // UAS STATUS - if (infoDockWidget) - { - addDockWidget(Qt::LeftDockWidgetArea, infoDockWidget); - infoDockWidget->show(); - } - - // WAYPOINT LIST - if (waypointsDockWidget) - { - addDockWidget(Qt::BottomDockWidgetArea, waypointsDockWidget); - waypointsDockWidget->show(); - } - - // DEBUG CONSOLE - if (debugConsoleDockWidget) - { - addDockWidget(Qt::BottomDockWidgetArea, debugConsoleDockWidget); - debugConsoleDockWidget->show(); - } - // ONBOARD PARAMETERS - if (parametersDockWidget) - { - addDockWidget(Qt::RightDockWidgetArea, parametersDockWidget); - parametersDockWidget->show(); - } - - this->show(); } void MainWindow::loadDataView() @@ -1060,160 +1076,12 @@ void MainWindow::loadDataView(QString fileName) } } -void MainWindow::loadPilotView() -{ - clearView(); - // HEAD UP DISPLAY - if (hudWidget) - { - QStackedWidget *centerStack = dynamic_cast(centralWidget()); - if (centerStack) - { - centerStack->setCurrentWidget(hudWidget); - hudWidget->start(); - } - } - //connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), pfd, SLOT(setActiveUAS(UASInterface*))); - if (headDown1DockWidget) - { - HDDisplay *hdd = dynamic_cast(headDown1DockWidget->widget()); - if (hdd) - { - addDockWidget(Qt::RightDockWidgetArea, headDown1DockWidget); - headDown1DockWidget->show(); - hdd->start(); - } - - } - if (headDown2DockWidget) - { - HDDisplay *hdd = dynamic_cast(headDown2DockWidget->widget()); - if (hdd) - { - addDockWidget(Qt::RightDockWidgetArea, headDown2DockWidget); - headDown2DockWidget->show(); - hdd->start(); - } - } - this->show(); -} - -void MainWindow::loadOperatorView() -{ - clearView(); - - // MAP - if (mapWidget) - { - QStackedWidget *centerStack = dynamic_cast(centralWidget()); - if (centerStack) - { - centerStack->setCurrentWidget(mapWidget); - } - } - - // UAS CONTROL - if (controlDockWidget) - { - addDockWidget(Qt::LeftDockWidgetArea, controlDockWidget); - controlDockWidget->show(); - } - - // UAS LIST - if (listDockWidget) - { - addDockWidget(Qt::BottomDockWidgetArea, listDockWidget); - listDockWidget->show(); - } - - // UAS STATUS - if (infoDockWidget) - { - addDockWidget(Qt::LeftDockWidgetArea, infoDockWidget); - infoDockWidget->show(); - } - - // WAYPOINT LIST - if (waypointsDockWidget) - { - addDockWidget(Qt::BottomDockWidgetArea, waypointsDockWidget); - waypointsDockWidget->show(); - } - - // HORIZONTAL SITUATION INDICATOR - if (hsiDockWidget) - { - HSIDisplay* hsi = dynamic_cast( hsiDockWidget->widget() ); - if (hsi) - { - addDockWidget(Qt::BottomDockWidgetArea, hsiDockWidget); - hsiDockWidget->show(); - hsi->start(); - } - } - - // OBJECT DETECTION - if (detectionDockWidget) - { - addDockWidget(Qt::RightDockWidgetArea, detectionDockWidget); - detectionDockWidget->show(); - } - - // PROCESS CONTROL - if (watchdogControlDockWidget) - { - addDockWidget(Qt::RightDockWidgetArea, watchdogControlDockWidget); - watchdogControlDockWidget->show(); - } - - this->show(); -} void MainWindow::loadSlugsEngineerView() { - clearView(); - - // MAP - if (mapWidget) - { - QStackedWidget *centerStack = dynamic_cast(centralWidget()); - if (centerStack) - { - centerStack->setCurrentWidget(mapWidget); - } - } - - // WAYPOINT LIST - if (waypointsDockWidget) - { - addDockWidget(Qt::BottomDockWidgetArea, waypointsDockWidget); - waypointsDockWidget->show(); - - } - - // Slugs Data View - if (slugsDataWidget) - { - addDockWidget(Qt::RightDockWidgetArea, slugsDataWidget); - slugsDataWidget->show(); - } - - // Slugs PID Tunning - if (slugsPIDControlWidget) - { - addDockWidget(Qt::LeftDockWidgetArea, slugsPIDControlWidget); - slugsPIDControlWidget->show(); - } - - // Slug Camera Control - if (slugsCamControlWidget) - { - addDockWidget(Qt::BottomDockWidgetArea, slugsCamControlWidget); - slugsCamControlWidget->show(); - } } @@ -1377,63 +1245,184 @@ void MainWindow::load3DView() void MainWindow::loadEngineerView() { + clearView(); + + currentView = VIEW_ENGINEER; + + presentView(); +} + +void MainWindow::loadOperatorView() +{ clearView(); - // If there is no mav dont siwtch view - int selector = (UASManager::instance()->getActiveUAS())? - UASManager::instance()->getActiveUAS()->getAutopilotType() : - 255; - - switch (selector){ - - case (MAV_AUTOPILOT_GENERIC): - case (MAV_AUTOPILOT_ARDUPILOTMEGA): - case (MAV_AUTOPILOT_PIXHAWK): - loadPixhawkEngineerView(); - break; - - case (MAV_AUTOPILOT_SLUGS): - loadSlugsEngineerView(); - break; - } - this->show(); + currentView = VIEW_OPERATOR; + + presentView(); +} + +void MainWindow::loadPilotView() +{ + clearView(); + + currentView = VIEW_PILOT; + + presentView(); } void MainWindow::loadMAVLinkView() { clearView(); - if (protocolWidget) - { - QStackedWidget *centerStack = dynamic_cast(centralWidget()); - if (centerStack) - { - centerStack->setCurrentWidget(protocolWidget); - } - } + currentView = VIEW_MAVLINK; + + presentView(); +} + +void MainWindow::presentView() { + +#ifdef QGC_OSG_ENABLED + // 3D map + if (_3DWidget) + { + if (centerStack) + { + //map3DWidget->setActive(true); + centerStack->setCurrentWidget(_3DWidget); + } + } +#else + + showCentralWidget(CENTRAL_LINECHART, currentView); - // TODO: Refactor this code to single function calls +#endif - showTheWidget(MENU_UAS_CONTROL); + // MAP + showCentralWidget(CENTRAL_MAP, currentView); - showTheWidget(MENU_UAS_LIST); + // PROTOCOL + showCentralWidget(CENTRAL_PROTOCOL, currentView); - showTheWidget(MENU_WAYPOINTS); + // HEAD UP DISPLAY + if (hudWidget && + settings.value(buildMenuKey (SUB_SECTION_CHECKED,MENU_HSI,currentView)).toBool()){ + if (centerStack) + { + //map3DWidget->setActive(true); + centerStack->setCurrentWidget(hudWidget); + hudWidget->start(); + } + } - showTheWidget(MENU_STATUS); + // Show docked widgets based on current view and autopilot type - showTheWidget(MENU_DEBUG_CONSOLE); + // UAS CONTROL + showTheWidget(MENU_UAS_CONTROL, currentView); + + // UAS LIST + showTheWidget(MENU_UAS_LIST, currentView); + + // WAYPOINT LIST + showTheWidget(MENU_WAYPOINTS, currentView); + + // UAS STATUS + showTheWidget(MENU_STATUS, currentView); + + // DETECTION + showTheWidget(MENU_DETECTION, currentView); + + // DEBUG CONSOLE + showTheWidget(MENU_DEBUG_CONSOLE, currentView); + + // ONBOARD PARAMETERS + showTheWidget(MENU_PARAMETERS, currentView); + + // WATCHDOG + showTheWidget(MENU_WATCHDOG, currentView); + + // HUD + showTheWidget(MENU_HUD, currentView); + + // RC View + showTheWidget(MENU_RC_VIEW, currentView); + + // SLUGS DATA + showTheWidget(MENU_SLUGS_DATA, currentView); + + // SLUGS PID + showTheWidget(MENU_SLUGS_PID, currentView); + + // SLUGS HIL + showTheWidget(MENU_SLUGS_HIL, currentView); + + // SLUGS CAMERA + showTheWidget(MENU_SLUGS_CAMERA, currentView); + + // HORIZONTAL SITUATION INDICATOR + if (hsiDockWidget) + { + HSIDisplay* hsi = dynamic_cast( hsiDockWidget->widget() ); + if (hsi && + settings.value(buildMenuKey (SUB_SECTION_CHECKED,MENU_HSI,currentView)).toBool()) + { + hsi->start(); + addDockWidget(static_cast (settings.value(buildMenuKey (SUB_SECTION_LOCATION,MENU_HSI, currentView)).toInt()), + hsiDockWidget); + hsiDockWidget->show(); + } + } + + // HEAD DOWN 1 + if (headDown1DockWidget) + { + HDDisplay *hdd = dynamic_cast(headDown1DockWidget->widget()); + if (hdd && + settings.value(buildMenuKey (SUB_SECTION_CHECKED,MENU_HDD_1,currentView)).toBool()) + { + addDockWidget(static_cast (settings.value(buildMenuKey (SUB_SECTION_LOCATION,MENU_HDD_1, currentView)).toInt()), + headDown1DockWidget); + headDown1DockWidget->show(); + hdd->start(); + } + + } + + // HEAD DOWN 2 + if (headDown2DockWidget) + { + HDDisplay *hdd = dynamic_cast(headDown2DockWidget->widget()); + if (hdd && + settings.value(buildMenuKey (SUB_SECTION_CHECKED,MENU_HDD_2,currentView)).toBool()) + { + addDockWidget(static_cast (settings.value(buildMenuKey (SUB_SECTION_LOCATION,MENU_HDD_2, currentView)).toInt()), + headDown2DockWidget); + headDown2DockWidget->show(); + hdd->start(); + } + } + + this->show(); - this->show(); } -void MainWindow::showTheWidget (TOOLS_WIDGET_NAMES widget){ + + +void MainWindow::showTheWidget (TOOLS_WIDGET_NAMES widget, VIEW_SECTIONS view){ bool tempVisible; Qt::DockWidgetArea tempLocation; QDockWidget* tempWidget = static_cast (dockWidgets[widget]); - tempVisible = settings.value(buildMenuKey (SUB_SECTION_CHECKED,widget)).toBool(); - tempLocation = static_cast (settings.value(buildMenuKey (SUB_SECTION_LOCATION,widget)).toInt()); + + tempVisible = settings.value(buildMenuKey (SUB_SECTION_CHECKED,widget,view)).toBool(); + + if (toolsMenuActions[widget]){ + toolsMenuActions[widget]->setChecked(tempVisible); + } + + + qDebug() << buildMenuKey (SUB_SECTION_CHECKED,widget,view) << tempVisible; + + tempLocation = static_cast (settings.value(buildMenuKey (SUB_SECTION_LOCATION,widget, view)).toInt()); if (tempWidget && tempVisible){ addDockWidget(tempLocation, tempWidget); @@ -1441,6 +1430,22 @@ void MainWindow::showTheWidget (TOOLS_WIDGET_NAMES widget){ } } + +void MainWindow::showCentralWidget (TOOLS_WIDGET_NAMES centralWidget, VIEW_SECTIONS view){ + bool tempVisible; + QWidget* tempWidget = dockWidgets[centralWidget]; + + tempVisible = settings.value(buildMenuKey (SUB_SECTION_CHECKED,centralWidget,view)).toBool(); + + if (toolsMenuActions[centralWidget]){ + toolsMenuActions[centralWidget]->setChecked(tempVisible); + } + + if (centerStack && tempWidget && tempVisible){ + centerStack->setCurrentWidget(tempWidget); + } +} + void MainWindow::loadAllView() { clearView(); diff --git a/src/ui/MainWindow.h b/src/ui/MainWindow.h index ee1ed25..995519b 100644 --- a/src/ui/MainWindow.h +++ b/src/ui/MainWindow.h @@ -146,11 +146,14 @@ public slots: void loadPixhawkEngineerView(); void loadSlugsEngineerView(); + void presentView(); + /** @brief Reload the CSS style sheet */ void reloadStylesheet(); void showToolWidget(); + void showCentralWidget(); void updateVisibilitySettings (bool vis); void updateLocationSettings (Qt::DockWidgetArea location); protected: @@ -179,7 +182,7 @@ protected: MENU_SLUGS_PID, MENU_SLUGS_HIL, MENU_SLUGS_CAMERA, - CENTRAL_LINECHART = 255, // Separation from dockwidgets and central widgets + CENTRAL_LINECHART = 255, // do not change CENTRAL_PROTOCOL, CENTRAL_MAP, CENTRAL_3D_LOCAL, @@ -187,18 +190,22 @@ protected: CENTRAL_GOOGLE_EARTH, CENTRAL_HUD, CENTRAL_DATA_PLOT, + CENTRAL_SEPARATOR, }TOOLS_WIDGET_NAMES; typedef enum _SETTINGS_SECTIONS { SECTION_MENU, - VIEW_ENGINEER, - VIEW_OPERATOR, - VIEW_CALIBRATION, - VIEW_MAVLINK, SUB_SECTION_CHECKED, SUB_SECTION_LOCATION, } SETTINGS_SECTIONS; + typedef enum _VIEW_SECTIONS { + VIEW_ENGINEER, + VIEW_OPERATOR, + VIEW_PILOT, + VIEW_MAVLINK, + } VIEW_SECTIONS; + QHash toolsMenuActions; // Holds ptr to the Menu Actions QHash dockWidgets; // Holds ptr to the Actual Dock widget @@ -206,9 +213,11 @@ protected: void addToToolsMenu (QWidget* widget, const QString title, const char * slotName, TOOLS_WIDGET_NAMES tool, Qt::DockWidgetArea location); - void showTheWidget (TOOLS_WIDGET_NAMES widget); + void showTheWidget (TOOLS_WIDGET_NAMES widget, VIEW_SECTIONS view = VIEW_MAVLINK); + void showCentralWidget (TOOLS_WIDGET_NAMES centralWidget, VIEW_SECTIONS view); + void addToCentralWidgetsMenu ( QWidget* widget, const QString title,const char * slotName, TOOLS_WIDGET_NAMES centralWidget); - int currentView; + VIEW_SECTIONS currentView; int aboutToQuit; //QHash settingsSections; @@ -250,7 +259,9 @@ protected: // Center widgets QPointer linechartWidget; + QPointer hudWidget; + QPointer mapWidget; QPointer protocolWidget; QPointer dataplotWidget; @@ -273,7 +284,9 @@ protected: QPointer headDown1DockWidget; QPointer headDown2DockWidget; QPointer watchdogControlDockWidget; + QPointer headUpDockWidget; + QPointer hsiDockWidget; QPointer rcViewDockWidget; QPointer slugsDataWidget; @@ -304,7 +317,7 @@ protected: private: Ui::MainWindow ui; - QString buildMenuKey (SETTINGS_SECTIONS section , TOOLS_WIDGET_NAMES tool); + QString buildMenuKey (SETTINGS_SECTIONS section , TOOLS_WIDGET_NAMES tool, VIEW_SECTIONS view); }; diff --git a/src/ui/MainWindow.ui b/src/ui/MainWindow.ui index a60e53c..1593eee 100644 --- a/src/ui/MainWindow.ui +++ b/src/ui/MainWindow.ui @@ -80,7 +80,6 @@ Tools - @@ -94,9 +93,9 @@ Perspectives - + - + @@ -393,13 +392,13 @@ Developer Credits - + :/images/status/weather-overcast.svg:/images/status/weather-overcast.svg - Flight + Operator @@ -429,13 +428,13 @@ Reload Style - + :/images/status/network-wireless-encrypted.svg:/images/status/network-wireless-encrypted.svg - Calibration + Pilot From 1c61eec81b74186e3d0f94620b0109f3033a4070 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Tue, 14 Dec 2010 10:22:51 -0500 Subject: [PATCH 13/21] Fixed build for linux. --- qgroundcontrol.pri | 1 + src/ui/map3D/GCManipulator.cc | 6 +++--- src/ui/map3D/QMap3D.cc | 2 +- 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/qgroundcontrol.pri b/qgroundcontrol.pri index d6a975b..2b2e46d 100644 --- a/qgroundcontrol.pri +++ b/qgroundcontrol.pri @@ -158,6 +158,7 @@ linux-g++ { QMAKE_POST_LINK += cp -rf $$BASEDIR/audio $$DESTDIR/. INCLUDEPATH += /usr/include \ + /usr/local/include \ /usr/include/qt4/phonon # $$BASEDIR/lib/flite/include \ # $$BASEDIR/lib/flite/lang diff --git a/src/ui/map3D/GCManipulator.cc b/src/ui/map3D/GCManipulator.cc index 6f2fb0e..05c9b24 100644 --- a/src/ui/map3D/GCManipulator.cc +++ b/src/ui/map3D/GCManipulator.cc @@ -254,10 +254,10 @@ GCManipulator::calcMovement(void) if (buttonMask == GUIEventAdapter::LEFT_MOUSE_BUTTON) { // rotate camera -#ifdef __APPLE__ - osg::Vec3d axis; -#else +#ifdef __WIN32__ osg::Vec3 axis; +#else + osg::Vec3d axis; #endif float angle; diff --git a/src/ui/map3D/QMap3D.cc b/src/ui/map3D/QMap3D.cc index ab4c98b..83ff901 100644 --- a/src/ui/map3D/QMap3D.cc +++ b/src/ui/map3D/QMap3D.cc @@ -39,7 +39,7 @@ QMap3D::QMap3D(QWidget * parent, const char * name, WindowFlags f) : QWidget(parent,f) { setupUi(this); - graphicsView->setCameraManipulator(new osgEarthUtil::EarthManipulator); + graphicsView->setCameraManipulator(new osgEarth::Util::EarthManipulator); graphicsView->setSceneData(new osg::Group); graphicsView->updateCamera(); show(); From 870ad6de6806bb65bcec949662d9c94ac056e0f4 Mon Sep 17 00:00:00 2001 From: Mariano Lizarraga Date: Wed, 15 Dec 2010 01:08:25 -0600 Subject: [PATCH 14/21] First semi-usable version with new menu layout based on autopilot. Still very experimental. Current Bug: Linecharts do not work. Need to find out why --- src/comm/MAVLinkProtocol.cc | 22 -- src/comm/MAVLinkSimulationLink.cc | 4 +- src/comm/SerialLink.cc | 2 +- src/ui/MainWindow.cc | 767 +++++++++++++++++++------------------- src/ui/MainWindow.h | 128 +++++-- src/ui/MapWidget.cc | 1 + 6 files changed, 489 insertions(+), 435 deletions(-) diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index 69cd6fd..b992633 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -162,10 +162,6 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) uas = new UAS(this, message.sysid); - // Set the autopilot type - uas->setAutopilotType((int)heartbeat.autopilot); - - // Connect this robot to the UAS object connect(this, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), uas, SLOT(receiveMessage(LinkInterface*, mavlink_message_t))); break; @@ -173,10 +169,6 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) { // Fixme differentiate between quadrotor and coaxial here PxQuadMAV* mav = new PxQuadMAV(this, message.sysid); - - // Set the autopilot type - //mav->setAutopilotType((int)heartbeat.autopilot); - // Connect this robot to the UAS object // it is IMPORTANT here to use the right object type, // else the slot of the parent object is called (and thus the special @@ -188,10 +180,6 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) case MAV_AUTOPILOT_SLUGS: { SlugsMAV* mav = new SlugsMAV(this, message.sysid); - - // Set the autopilot type - mav->setAutopilotType((int)heartbeat.autopilot); - // Connect this robot to the UAS object // it is IMPORTANT here to use the right object type, // else the slot of the parent object is called (and thus the special @@ -203,10 +191,6 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) case MAV_AUTOPILOT_ARDUPILOTMEGA: { ArduPilotMegaMAV* mav = new ArduPilotMegaMAV(this, message.sysid); - - // Set the autopilot type - mav->setAutopilotType((int)heartbeat.autopilot); - // Connect this robot to the UAS object // it is IMPORTANT here to use the right object type, // else the slot of the parent object is called (and thus the special @@ -229,12 +213,6 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) // Now add UAS to "official" list, which makes the whole application aware of it UASManager::instance()->addUAS(uas); - - - qDebug() << "++==============================="; - qDebug() << uas->getAutopilotType(); - qDebug() << "++==============================="; - } // Only count message if UAS exists for this message diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc index d5647da..82eb080 100644 --- a/src/comm/MAVLinkSimulationLink.cc +++ b/src/comm/MAVLinkSimulationLink.cc @@ -837,9 +837,9 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size) } unsigned char v=data[i]; - fprintf(stderr,"%02x ", v); + //fprintf(stderr,"%02x ", v); } - fprintf(stderr,"\n"); + //fprintf(stderr,"\n"); readyBufferMutex.lock(); for (int i = 0; i < streampointer; i++) diff --git a/src/comm/SerialLink.cc b/src/comm/SerialLink.cc index d4b48cd..1e9b74f 100644 --- a/src/comm/SerialLink.cc +++ b/src/comm/SerialLink.cc @@ -170,7 +170,7 @@ void SerialLink::writeBytes(const char* data, qint64 size) { unsigned char v=data[i]; - fprintf(stderr,"%02x ", v); + //fprintf(stderr,"%02x ", v); } } } diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index c6e9631..ca98ec2 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -44,7 +44,7 @@ * * @see QMainWindow::show() **/ -MainWindow::MainWindow(QWidget *parent) : +MainWindow::MainWindow(QWidget *parent): QMainWindow(parent), toolsMenuActions(), currentView(VIEW_MAVLINK), @@ -90,8 +90,6 @@ MainWindow::~MainWindow() statusBar = NULL; } - - void MainWindow::buildCommonWidgets() { //TODO: move protocol outside UI @@ -128,6 +126,7 @@ void MainWindow::buildCommonWidgets() } + void MainWindow::buildPxWidgets() { //FIXME: memory of acceptList will never be freed again @@ -146,18 +145,30 @@ void MainWindow::buildPxWidgets() // Center widgets linechartWidget = new Linecharts(this); + addToCentralWidgetsMenu(linechartWidget, "Line Plots", SLOT(showCentralWidget()), CENTRAL_LINECHART); + + hudWidget = new HUD(320, 240, this); + addToCentralWidgetsMenu(hudWidget, "HUD", SLOT(showCentralWidget()), CENTRAL_HUD); + dataplotWidget = new QGCDataPlot2D(this); + addToCentralWidgetsMenu(dataplotWidget, "Data Plots", SLOT(showCentralWidget()), CENTRAL_DATA_PLOT); #ifdef QGC_OSG_ENABLED _3DWidget = Q3DWidgetFactory::get("PIXHAWK"); + addToCentralWidgetsMenu(_3DWidget, "Local 3D", SLOT(showCentralWidget()), CENTRAL_3D_LOCAL); + #endif #ifdef QGC_OSGEARTH_ENABLED _3DMapWidget = Q3DWidgetFactory::get("MAP3D"); + addToCentralWidgetsMenu(_3DMapWidget, "OSG Earth 3D", SLOT(showCentralWidget()), CENTRAL_OSGEARTH); + #endif #if (defined Q_OS_WIN) | (defined Q_OS_MAC) gEarthWidget = new QGCGoogleEarthView(this); + addToCentralWidgetsMenu(gEarthWidget, "Google Earth", SLOT(showCentralWidget()), CENTRAL_GOOGLE_EARTH); + #endif // Dock widgets @@ -244,10 +255,12 @@ void MainWindow::addToCentralWidgetsMenu ( QWidget* widget, TOOLS_WIDGET_NAMES centralWidget){ QAction* tempAction; + // Add the separator that will separate tools from central Widgets if (!toolsMenuActions[CENTRAL_SEPARATOR]){ tempAction = ui.menuTools->addSeparator(); toolsMenuActions[CENTRAL_SEPARATOR] = tempAction; + tempAction->setData(CENTRAL_SEPARATOR); } tempAction = ui.menuTools->addAction(title); @@ -264,9 +277,10 @@ void MainWindow::addToCentralWidgetsMenu ( QWidget* widget, if (!settings.contains(chKey)){ settings.setValue(chKey,false); tempAction->setChecked(false); - } else { - tempAction->setChecked(settings.value(chKey).toBool()); } +// else { +// tempAction->setChecked(settings.value(chKey).toBool()); +// } // connect the action connect(tempAction,SIGNAL(triggered()),this, slotName); @@ -279,14 +293,16 @@ void MainWindow::showCentralWidget(){ int tool = senderAction->data().toInt(); QString chKey; + // check the current action + if (senderAction && dockWidgets[tool]){ // uncheck all central widget actions QHashIterator i(toolsMenuActions); while (i.hasNext()) { - i.next(); - - if (i.value()->data().toInt() >= 255){ + i.next(); + qDebug() << "shCW" << i.key() << "read"; + if (i.value() && i.value()->data().toInt() > 255){ i.value()->setChecked(false); // update the settings @@ -296,6 +312,7 @@ void MainWindow::showCentralWidget(){ } // check the current action + qDebug() << senderAction->text(); senderAction->setChecked(true); // update the central widget @@ -304,6 +321,8 @@ void MainWindow::showCentralWidget(){ // store the selected central widget chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(tool), currentView); settings.setValue(chKey,true); + + presentView(); } } @@ -315,6 +334,7 @@ void MainWindow::addToToolsMenu ( QWidget* widget, QAction* tempAction; QString posKey, chKey; + if (toolsMenuActions[CENTRAL_SEPARATOR]){ tempAction = new QAction(title, this); ui.menuTools->insertAction(toolsMenuActions[CENTRAL_SEPARATOR], @@ -329,6 +349,7 @@ void MainWindow::addToToolsMenu ( QWidget* widget, // populate the Hashes toolsMenuActions[tool] = tempAction; dockWidgets[tool] = widget; + qDebug() << widget; posKey = buildMenuKey (SUB_SECTION_LOCATION,tool, currentView); @@ -351,14 +372,55 @@ void MainWindow::addToToolsMenu ( QWidget* widget, // connect the action connect(tempAction,SIGNAL(triggered()),this, slotName); - connect(static_cast (dockWidgets[tool]), + connect(qobject_cast (dockWidgets[tool]), SIGNAL(visibilityChanged(bool)), this, SLOT(updateVisibilitySettings(bool))); - connect(static_cast (dockWidgets[tool]), + connect(qobject_cast (dockWidgets[tool]), SIGNAL(dockLocationChanged(Qt::DockWidgetArea)), this, SLOT(updateLocationSettings(Qt::DockWidgetArea))); } +void MainWindow::showToolWidget(){ + QAction* temp = qobject_cast(sender()); + int tool = temp->data().toInt(); + + + if (temp && dockWidgets[tool]){ + if (temp->isChecked()){ + addDockWidget(dockWidgetLocations[tool], qobject_cast (dockWidgets[tool])); + qobject_cast(dockWidgets[tool])->show(); + } else { + removeDockWidget(qobject_cast(dockWidgets[tool])); + } + QString chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(tool), currentView); + settings.setValue(chKey,temp->isChecked()); + } +} + + +void MainWindow::showTheWidget (TOOLS_WIDGET_NAMES widget, VIEW_SECTIONS view){ + bool tempVisible; + Qt::DockWidgetArea tempLocation; + QDockWidget* tempWidget = static_cast (dockWidgets[widget]); + + tempVisible = settings.value(buildMenuKey (SUB_SECTION_CHECKED,widget,view)).toBool(); + + if (tempWidget){ + toolsMenuActions[widget]->setChecked(tempVisible); + } + + + //qDebug() << buildMenuKey (SUB_SECTION_CHECKED,widget,view) << tempVisible; + + tempLocation = static_cast (settings.value(buildMenuKey (SUB_SECTION_LOCATION,widget, view)).toInt()); + + if (tempWidget && tempVisible){ + addDockWidget(tempLocation, tempWidget); + tempWidget->show(); + } + +} + QString MainWindow::buildMenuKey(SETTINGS_SECTIONS section, TOOLS_WIDGET_NAMES tool, VIEW_SECTIONS view){ // Key is built as follows: autopilot_type/section_menu/view/tool/section int apType; @@ -374,24 +436,6 @@ QString MainWindow::buildMenuKey(SETTINGS_SECTIONS section, TOOLS_WIDGET_NAMES t QString::number(section) + "/" ); } - -void MainWindow::showToolWidget(){ - QAction* temp = qobject_cast(sender()); - int tool = temp->data().toInt(); - - if (temp && dockWidgets[tool]){ - if (temp->isChecked()){ - addDockWidget(dockWidgetLocations[tool], static_cast (dockWidgets[tool])); - static_cast (dockWidgets[tool])->show(); - } else { - removeDockWidget(static_cast (dockWidgets[tool])); - } - QString chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(tool), currentView); - settings.setValue(chKey,temp->isChecked()); - } -} - - void MainWindow::updateVisibilitySettings (bool vis){ Q_UNUSED(vis); @@ -974,359 +1018,109 @@ void MainWindow::clearView() } } -void MainWindow::loadSlugsView() +void MainWindow::loadEngineerView() { - clearView(); - // Engineer view, used in EMAV2009 - - // LINE CHART - if (linechartWidget) - { - QStackedWidget *centerStack = dynamic_cast(centralWidget()); - if (centerStack) - { - linechartWidget->setActive(true); - centerStack->setCurrentWidget(linechartWidget); - } - } - - // UAS CONTROL - if (controlDockWidget) - { - addDockWidget(Qt::LeftDockWidgetArea, controlDockWidget); - controlDockWidget->show(); - } - - // UAS LIST - if (listDockWidget) - { - addDockWidget(Qt::BottomDockWidgetArea, listDockWidget); - listDockWidget->show(); - } - - // UAS STATUS - if (infoDockWidget) - { - addDockWidget(Qt::LeftDockWidgetArea, infoDockWidget); - infoDockWidget->show(); - } - - - // WAYPOINT LIST - if (waypointsDockWidget) - { - addDockWidget(Qt::BottomDockWidgetArea, waypointsDockWidget); - waypointsDockWidget->show(); - } - - // DEBUG CONSOLE - if (debugConsoleDockWidget) - { - addDockWidget(Qt::BottomDockWidgetArea, debugConsoleDockWidget); - debugConsoleDockWidget->show(); - } + clearView(); - // Slugs Data View - if (slugsDataWidget) - { - addDockWidget(Qt::RightDockWidgetArea, slugsDataWidget); - slugsDataWidget->show(); - } + currentView = VIEW_ENGINEER; - // Slugs Data View - if (slugsHilSimWidget) - { - addDockWidget(Qt::LeftDockWidgetArea, slugsHilSimWidget); - slugsHilSimWidget->show(); - } - this->show(); + presentView(); } -void MainWindow::loadPixhawkEngineerView() +void MainWindow::loadOperatorView() { + clearView(); + + currentView = VIEW_OPERATOR; + presentView(); } -void MainWindow::loadDataView() +void MainWindow::loadPilotView() { clearView(); - // DATAPLOT - if (dataplotWidget) - { - QStackedWidget *centerStack = dynamic_cast(centralWidget()); - if (centerStack) - centerStack->setCurrentWidget(dataplotWidget); - } + currentView = VIEW_PILOT; + + presentView(); } -void MainWindow::loadDataView(QString fileName) +void MainWindow::loadMAVLinkView() { clearView(); - // DATAPLOT - if (dataplotWidget) - { - QStackedWidget *centerStack = dynamic_cast(centralWidget()); - if (centerStack) - { - centerStack->setCurrentWidget(dataplotWidget); - dataplotWidget->loadFile(fileName); - } - } + currentView = VIEW_MAVLINK; + + presentView(); } +void MainWindow::presentView() { +#ifdef QGC_OSG_ENABLED + // 3D map + if (_3DWidget) + { + if (centerStack) + { + //map3DWidget->setActive(true); + centerStack->setCurrentWidget(_3DWidget); + } + } +#endif + qDebug() << "LC"; + if (linechartWidget){ + qDebug () << buildMenuKey (SUB_SECTION_CHECKED,CENTRAL_LINECHART,currentView) << + settings.value(buildMenuKey (SUB_SECTION_CHECKED,CENTRAL_LINECHART,currentView)).toBool() ; + if (settings.value(buildMenuKey (SUB_SECTION_CHECKED,CENTRAL_LINECHART,currentView)).toBool()){ + if (centerStack) { + linechartWidget->setActive(true); + centerStack->setCurrentWidget(linechartWidget); + } + } else { + linechartWidget->setActive(false); + } + } -void MainWindow::loadSlugsEngineerView() -{ -} -void MainWindow::load3DMapView() -{ -#ifdef QGC_OSGEARTH_ENABLED - clearView(); + // MAP + qDebug() << "MAP"; + showTheCentralWidget(CENTRAL_MAP, currentView); - // 3D map - if (_3DMapWidget) - { - QStackedWidget *centerStack = dynamic_cast(centralWidget()); - if (centerStack) - { - //map3DWidget->setActive(true); - centerStack->setCurrentWidget(_3DMapWidget); - } - } + // PROTOCOL + qDebug() << "CP"; + showTheCentralWidget(CENTRAL_PROTOCOL, currentView); - // UAS CONTROL - if (controlDockWidget) - { - addDockWidget(Qt::LeftDockWidgetArea, controlDockWidget); - controlDockWidget->show(); - } + // HEAD UP DISPLAY + qDebug() << "HUD"; + if (hudWidget){ + qDebug() << buildMenuKey(SUB_SECTION_CHECKED,CENTRAL_HUD,currentView) << + settings.value(buildMenuKey (SUB_SECTION_CHECKED,CENTRAL_HUD,currentView)).toBool(); + if (settings.value(buildMenuKey (SUB_SECTION_CHECKED,CENTRAL_HUD,currentView)).toBool()){ + if (centerStack) { + centerStack->setCurrentWidget(hudWidget); + hudWidget->start(); + } + } else { + hudWidget->stop(); + } + } - // UAS LIST - if (listDockWidget) - { - addDockWidget(Qt::BottomDockWidgetArea, listDockWidget); - listDockWidget->show(); - } + // Show docked widgets based on current view and autopilot type - // WAYPOINT LIST - if (waypointsDockWidget) - { - addDockWidget(Qt::BottomDockWidgetArea, waypointsDockWidget); - waypointsDockWidget->show(); - } + // UAS CONTROL + showTheWidget(MENU_UAS_CONTROL, currentView); - // HORIZONTAL SITUATION INDICATOR - if (hsiDockWidget) - { - HSIDisplay* hsi = dynamic_cast( hsiDockWidget->widget() ); - if (hsi) - { - hsi->start(); - addDockWidget(Qt::LeftDockWidgetArea, hsiDockWidget); - hsiDockWidget->show(); - } - } -#endif - this->show(); -} + // UAS LIST + showTheWidget(MENU_UAS_LIST, currentView); -void MainWindow::loadGoogleEarthView() -{ - #if (defined Q_OS_WIN) | (defined Q_OS_MAC) - clearView(); + // WAYPOINT LIST + showTheWidget(MENU_WAYPOINTS, currentView); - // 3D map - if (gEarthWidget) - { - QStackedWidget *centerStack = dynamic_cast(centralWidget()); - if (centerStack) - { - centerStack->setCurrentWidget(gEarthWidget); - } - } - - // UAS CONTROL - if (controlDockWidget) - { - addDockWidget(Qt::LeftDockWidgetArea, controlDockWidget); - controlDockWidget->show(); - } - - // UAS LIST - if (listDockWidget) - { - addDockWidget(Qt::BottomDockWidgetArea, listDockWidget); - listDockWidget->show(); - } - - // WAYPOINT LIST - if (waypointsDockWidget) - { - addDockWidget(Qt::BottomDockWidgetArea, waypointsDockWidget); - waypointsDockWidget->show(); - } - - // HORIZONTAL SITUATION INDICATOR - if (hsiDockWidget) - { - HSIDisplay* hsi = dynamic_cast( hsiDockWidget->widget() ); - if (hsi) - { - hsi->start(); - addDockWidget(Qt::LeftDockWidgetArea, hsiDockWidget); - hsiDockWidget->show(); - } - } - this->show(); -#endif - -} - - -void MainWindow::load3DView() -{ -#ifdef QGC_OSG_ENABLED - clearView(); - - // 3D map - if (_3DWidget) - { - QStackedWidget *centerStack = dynamic_cast(centralWidget()); - if (centerStack) - { - //map3DWidget->setActive(true); - centerStack->setCurrentWidget(_3DWidget); - } - } - - // UAS CONTROL - if (controlDockWidget) - { - addDockWidget(Qt::LeftDockWidgetArea, controlDockWidget); - controlDockWidget->show(); - } - - // UAS LIST - if (listDockWidget) - { - addDockWidget(Qt::BottomDockWidgetArea, listDockWidget); - listDockWidget->show(); - } - - // WAYPOINT LIST - if (waypointsDockWidget) - { - addDockWidget(Qt::BottomDockWidgetArea, waypointsDockWidget); - waypointsDockWidget->show(); - } - - // HORIZONTAL SITUATION INDICATOR - if (hsiDockWidget) - { - HSIDisplay* hsi = dynamic_cast( hsiDockWidget->widget() ); - if (hsi) - { - hsi->start(); - addDockWidget(Qt::LeftDockWidgetArea, hsiDockWidget); - hsiDockWidget->show(); - } - } -#endif - this->show(); - -} - -void MainWindow::loadEngineerView() -{ - clearView(); - - currentView = VIEW_ENGINEER; - - presentView(); -} - -void MainWindow::loadOperatorView() -{ - clearView(); - - currentView = VIEW_OPERATOR; - - presentView(); -} - -void MainWindow::loadPilotView() -{ - clearView(); - - currentView = VIEW_PILOT; - - presentView(); -} - -void MainWindow::loadMAVLinkView() -{ - clearView(); - - currentView = VIEW_MAVLINK; - - presentView(); -} - -void MainWindow::presentView() { - -#ifdef QGC_OSG_ENABLED - // 3D map - if (_3DWidget) - { - if (centerStack) - { - //map3DWidget->setActive(true); - centerStack->setCurrentWidget(_3DWidget); - } - } -#else - - showCentralWidget(CENTRAL_LINECHART, currentView); - -#endif - - // MAP - showCentralWidget(CENTRAL_MAP, currentView); - - // PROTOCOL - showCentralWidget(CENTRAL_PROTOCOL, currentView); - - // HEAD UP DISPLAY - if (hudWidget && - settings.value(buildMenuKey (SUB_SECTION_CHECKED,MENU_HSI,currentView)).toBool()){ - if (centerStack) - { - //map3DWidget->setActive(true); - centerStack->setCurrentWidget(hudWidget); - hudWidget->start(); - } - } - - // Show docked widgets based on current view and autopilot type - - // UAS CONTROL - showTheWidget(MENU_UAS_CONTROL, currentView); - - // UAS LIST - showTheWidget(MENU_UAS_LIST, currentView); - - // WAYPOINT LIST - showTheWidget(MENU_WAYPOINTS, currentView); - - // UAS STATUS - showTheWidget(MENU_STATUS, currentView); + // UAS STATUS + showTheWidget(MENU_STATUS, currentView); // DETECTION showTheWidget(MENU_DETECTION, currentView); @@ -1342,6 +1136,22 @@ void MainWindow::presentView() { // HUD showTheWidget(MENU_HUD, currentView); + if (headUpDockWidget) + { + HUD* tmpHud = dynamic_cast( headUpDockWidget->widget() ); + if (tmpHud){ + if (settings.value(buildMenuKey (SUB_SECTION_CHECKED,MENU_HUD,currentView)).toBool()){ + tmpHud->start(); + addDockWidget(static_cast (settings.value(buildMenuKey (SUB_SECTION_LOCATION,MENU_HUD, currentView)).toInt()), + hsiDockWidget); + headUpDockWidget->show(); + } else { + tmpHud->stop(); + headUpDockWidget->hide(); + } + } + } + // RC View showTheWidget(MENU_RC_VIEW, currentView); @@ -1359,45 +1169,56 @@ void MainWindow::presentView() { showTheWidget(MENU_SLUGS_CAMERA, currentView); // HORIZONTAL SITUATION INDICATOR + showTheWidget(MENU_HSI, currentView); if (hsiDockWidget) { HSIDisplay* hsi = dynamic_cast( hsiDockWidget->widget() ); - if (hsi && - settings.value(buildMenuKey (SUB_SECTION_CHECKED,MENU_HSI,currentView)).toBool()) - { + if (hsi){ + if (settings.value(buildMenuKey (SUB_SECTION_CHECKED,MENU_HSI,currentView)).toBool()){ hsi->start(); addDockWidget(static_cast (settings.value(buildMenuKey (SUB_SECTION_LOCATION,MENU_HSI, currentView)).toInt()), hsiDockWidget); hsiDockWidget->show(); + } else { + hsi->stop(); + hsiDockWidget->hide(); + } } } // HEAD DOWN 1 + showTheWidget(MENU_HDD_1, currentView); if (headDown1DockWidget) { HDDisplay *hdd = dynamic_cast(headDown1DockWidget->widget()); - if (hdd && - settings.value(buildMenuKey (SUB_SECTION_CHECKED,MENU_HDD_1,currentView)).toBool()) - { + if (hdd) { + if (settings.value(buildMenuKey (SUB_SECTION_CHECKED,MENU_HDD_1,currentView)).toBool()) { addDockWidget(static_cast (settings.value(buildMenuKey (SUB_SECTION_LOCATION,MENU_HDD_1, currentView)).toInt()), headDown1DockWidget); headDown1DockWidget->show(); hdd->start(); + } else { + headDown1DockWidget->hide();; + hdd->stop(); + } } - } // HEAD DOWN 2 + showTheWidget(MENU_HDD_2, currentView); if (headDown2DockWidget) { HDDisplay *hdd = dynamic_cast(headDown2DockWidget->widget()); - if (hdd && - settings.value(buildMenuKey (SUB_SECTION_CHECKED,MENU_HDD_2,currentView)).toBool()) - { + if (hdd){ + if (settings.value(buildMenuKey (SUB_SECTION_CHECKED,MENU_HDD_2,currentView)).toBool()){ addDockWidget(static_cast (settings.value(buildMenuKey (SUB_SECTION_LOCATION,MENU_HDD_2, currentView)).toInt()), headDown2DockWidget); headDown2DockWidget->show(); hdd->start(); + } else { + headDown2DockWidget->hide(); + hdd->stop(); + } } } @@ -1407,36 +1228,13 @@ void MainWindow::presentView() { -void MainWindow::showTheWidget (TOOLS_WIDGET_NAMES widget, VIEW_SECTIONS view){ - bool tempVisible; - Qt::DockWidgetArea tempLocation; - QDockWidget* tempWidget = static_cast (dockWidgets[widget]); - - - tempVisible = settings.value(buildMenuKey (SUB_SECTION_CHECKED,widget,view)).toBool(); - - if (toolsMenuActions[widget]){ - toolsMenuActions[widget]->setChecked(tempVisible); - } - - - qDebug() << buildMenuKey (SUB_SECTION_CHECKED,widget,view) << tempVisible; - - tempLocation = static_cast (settings.value(buildMenuKey (SUB_SECTION_LOCATION,widget, view)).toInt()); - - if (tempWidget && tempVisible){ - addDockWidget(tempLocation, tempWidget); - tempWidget->show(); - } -} - -void MainWindow::showCentralWidget (TOOLS_WIDGET_NAMES centralWidget, VIEW_SECTIONS view){ +void MainWindow::showTheCentralWidget (TOOLS_WIDGET_NAMES centralWidget, VIEW_SECTIONS view){ bool tempVisible; QWidget* tempWidget = dockWidgets[centralWidget]; tempVisible = settings.value(buildMenuKey (SUB_SECTION_CHECKED,centralWidget,view)).toBool(); - + qDebug() << buildMenuKey (SUB_SECTION_CHECKED,centralWidget,view) << tempVisible; if (toolsMenuActions[centralWidget]){ toolsMenuActions[centralWidget]->setChecked(tempVisible); } @@ -1446,6 +1244,20 @@ void MainWindow::showCentralWidget (TOOLS_WIDGET_NAMES centralWidget, VIEW_SECTI } } + + +/* +========================================================== + Potentially Deprecated +========================================================== +*/ + +void MainWindow::loadPixhawkEngineerView() +{ + +} + + void MainWindow::loadAllView() { clearView(); @@ -1459,7 +1271,7 @@ void MainWindow::loadAllView() headDown1DockWidget->show(); hdd->start(); } - + } if (headDown2DockWidget) { @@ -1542,6 +1354,191 @@ void MainWindow::loadWidgets() //loadPilotView(); } +void MainWindow::loadDataView() +{ + clearView(); + + // DATAPLOT + if (dataplotWidget) + { + QStackedWidget *centerStack = dynamic_cast(centralWidget()); + if (centerStack) + centerStack->setCurrentWidget(dataplotWidget); + } +} + +void MainWindow::loadDataView(QString fileName) +{ + clearView(); + + // DATAPLOT + if (dataplotWidget) + { + QStackedWidget *centerStack = dynamic_cast(centralWidget()); + if (centerStack) + { + centerStack->setCurrentWidget(dataplotWidget); + dataplotWidget->loadFile(fileName); + } + } +} + +void MainWindow::load3DMapView() +{ +#ifdef QGC_OSGEARTH_ENABLED + clearView(); + + // 3D map + if (_3DMapWidget) + { + QStackedWidget *centerStack = dynamic_cast(centralWidget()); + if (centerStack) + { + //map3DWidget->setActive(true); + centerStack->setCurrentWidget(_3DMapWidget); + } + } + + // UAS CONTROL + if (controlDockWidget) + { + addDockWidget(Qt::LeftDockWidgetArea, controlDockWidget); + controlDockWidget->show(); + } + + // UAS LIST + if (listDockWidget) + { + addDockWidget(Qt::BottomDockWidgetArea, listDockWidget); + listDockWidget->show(); + } + + // WAYPOINT LIST + if (waypointsDockWidget) + { + addDockWidget(Qt::BottomDockWidgetArea, waypointsDockWidget); + waypointsDockWidget->show(); + } + + // HORIZONTAL SITUATION INDICATOR + if (hsiDockWidget) + { + HSIDisplay* hsi = dynamic_cast( hsiDockWidget->widget() ); + if (hsi) + { + hsi->start(); + addDockWidget(Qt::LeftDockWidgetArea, hsiDockWidget); + hsiDockWidget->show(); + } + } +#endif + this->show(); +} + +void MainWindow::loadGoogleEarthView() +{ + #if (defined Q_OS_WIN) | (defined Q_OS_MAC) + clearView(); + + // 3D map + if (gEarthWidget) + { + QStackedWidget *centerStack = dynamic_cast(centralWidget()); + if (centerStack) + { + centerStack->setCurrentWidget(gEarthWidget); + } + } + + // UAS CONTROL + if (controlDockWidget) + { + addDockWidget(Qt::LeftDockWidgetArea, controlDockWidget); + controlDockWidget->show(); + } + + // UAS LIST + if (listDockWidget) + { + addDockWidget(Qt::BottomDockWidgetArea, listDockWidget); + listDockWidget->show(); + } + + // WAYPOINT LIST + if (waypointsDockWidget) + { + addDockWidget(Qt::BottomDockWidgetArea, waypointsDockWidget); + waypointsDockWidget->show(); + } + + // HORIZONTAL SITUATION INDICATOR + if (hsiDockWidget) + { + HSIDisplay* hsi = dynamic_cast( hsiDockWidget->widget() ); + if (hsi) + { + hsi->start(); + addDockWidget(Qt::LeftDockWidgetArea, hsiDockWidget); + hsiDockWidget->show(); + } + } + this->show(); +#endif + +} + +void MainWindow::load3DView() +{ +#ifdef QGC_OSG_ENABLED + clearView(); + + // 3D map + if (_3DWidget) + { + QStackedWidget *centerStack = dynamic_cast(centralWidget()); + if (centerStack) + { + //map3DWidget->setActive(true); + centerStack->setCurrentWidget(_3DWidget); + } + } + + // UAS CONTROL + if (controlDockWidget) + { + addDockWidget(Qt::LeftDockWidgetArea, controlDockWidget); + controlDockWidget->show(); + } + + // UAS LIST + if (listDockWidget) + { + addDockWidget(Qt::BottomDockWidgetArea, listDockWidget); + listDockWidget->show(); + } + + // WAYPOINT LIST + if (waypointsDockWidget) + { + addDockWidget(Qt::BottomDockWidgetArea, waypointsDockWidget); + waypointsDockWidget->show(); + } + + // HORIZONTAL SITUATION INDICATOR + if (hsiDockWidget) + { + HSIDisplay* hsi = dynamic_cast( hsiDockWidget->widget() ); + if (hsi) + { + hsi->start(); + addDockWidget(Qt::LeftDockWidgetArea, hsiDockWidget); + hsiDockWidget->show(); + } + } +#endif + this->show(); + +} /* ================================== diff --git a/src/ui/MainWindow.h b/src/ui/MainWindow.h index 995519b..0ef7596 100644 --- a/src/ui/MainWindow.h +++ b/src/ui/MainWindow.h @@ -119,20 +119,9 @@ public slots: void loadEngineerView(); /** @brief Load view for operator */ void loadOperatorView(); - /** @brief Load 3D view */ - void load3DView(); - /** @brief Load 3D Google Earth view */ - void loadGoogleEarthView(); - /** @brief Load 3D map view */ - void load3DMapView(); - /** @brief Load view with all widgets */ - void loadAllView(); /** @brief Load MAVLink XML generator view */ void loadMAVLinkView(); - /** @brief Load data view, allowing to plot flight data */ - void loadDataView(); - /** @brief Load data view, allowing to plot flight data */ - void loadDataView(QString fileName); + /** @brief Show the online help for users */ void showHelp(); /** @brief Show the authors / credits */ @@ -140,22 +129,63 @@ public slots: /** @brief Show the project roadmap */ void showRoadMap(); - // Fixme find a nicer solution that scales to more AP types - void loadSlugsView(); - - void loadPixhawkEngineerView(); - void loadSlugsEngineerView(); - + /** @brief Shows the widgets based on configuration and current view and autopilot */ void presentView(); /** @brief Reload the CSS style sheet */ void reloadStylesheet(); + /* + ========================================================== + Potentially Deprecated + ========================================================== + */ + + void loadPixhawkEngineerView(); + + /** @brief Load view with all widgets */ + void loadAllView(); + + void loadWidgets(); + + /** @brief Load data view, allowing to plot flight data */ + void loadDataView(); + /** @brief Load data view, allowing to plot flight data */ + void loadDataView(QString fileName); + + /** @brief Load 3D map view */ + void load3DMapView(); + + /** @brief Load 3D Google Earth view */ + void loadGoogleEarthView(); + + /** @brief Load 3D view */ + void load3DView(); + /** + * @brief Shows a Docked Widget based on the action sender + * + * This slot is written to be used in conjunction with the addToToolsMenu function + * It shows the QDockedWidget based on the action sender + * + */ void showToolWidget(); + + /** + * @brief Shows a Widget from the center stack based on the action sender + * + * This slot is written to be used in conjunction with the addToCentralWidgetsMenu function + * It shows the Widget based on the action sender + * + */ void showCentralWidget(); + + /** @brief Updates a QDockWidget's checked status based on its visibility */ void updateVisibilitySettings (bool vis); + + /** @brief Updates a QDockWidget's location */ void updateLocationSettings (Qt::DockWidgetArea location); + protected: // These defines are used to save the settings when selecting with @@ -182,15 +212,17 @@ protected: MENU_SLUGS_PID, MENU_SLUGS_HIL, MENU_SLUGS_CAMERA, - CENTRAL_LINECHART = 255, // do not change + CENTRAL_SEPARATOR= 255, // do not change + CENTRAL_LINECHART, CENTRAL_PROTOCOL, CENTRAL_MAP, CENTRAL_3D_LOCAL, CENTRAL_3D_MAP, + CENTRAL_OSGEARTH, CENTRAL_GOOGLE_EARTH, CENTRAL_HUD, CENTRAL_DATA_PLOT, - CENTRAL_SEPARATOR, + }TOOLS_WIDGET_NAMES; typedef enum _SETTINGS_SECTIONS { @@ -211,19 +243,65 @@ protected: QHash dockWidgets; // Holds ptr to the Actual Dock widget QHash dockWidgetLocations; // Holds the location - + /** + * @brief Adds an already instantiated QDockedWidget to the Tools Menu + * + * This function does all the hosekeeping to have a QDockedWidget added to the + * tools menu and connects the QMenuAction to a slot that shows the widget and + * checks/unchecks the tools menu item + * + * @param widget The QDockedWidget being added + * @param title The entry that will appear in the Menu and in the QDockedWidget title bar + * @param slotName The slot to which the triggered() signal of the menu action will be connected. + * @param tool The ENUM defined in MainWindow.h that is associated to the widget + * @param location The default location for the QDockedWidget in case there is no previous key in the settings + */ void addToToolsMenu (QWidget* widget, const QString title, const char * slotName, TOOLS_WIDGET_NAMES tool, Qt::DockWidgetArea location); + + /** + * @brief Determines if a QDockWidget needs to be show and if so, shows it + * + * Based on the the autopilot and the current view it queries the settings and shows the + * widget if necessary + * + * @param widget The QDockWidget requested to be shown + * @param view The view for which the QDockWidget is requested + */ void showTheWidget (TOOLS_WIDGET_NAMES widget, VIEW_SECTIONS view = VIEW_MAVLINK); - void showCentralWidget (TOOLS_WIDGET_NAMES centralWidget, VIEW_SECTIONS view); + + /** + * @brief Adds an already instantiated QWidget to the center stack + * + * This function does all the hosekeeping to have a QWidget added to the tools menu + * tools menu and connects the QMenuAction to a slot that shows the widget and + * checks/unchecks the tools menu item. This is used for all the central widgets (those in + * the center stack. + * + * @param widget The QWidget being added + * @param title The entry that will appear in the Menu + * @param slotName The slot to which the triggered() signal of the menu action will be connected. + * @param centralWidget The ENUM defined in MainWindow.h that is associated to the widget + */ void addToCentralWidgetsMenu ( QWidget* widget, const QString title,const char * slotName, TOOLS_WIDGET_NAMES centralWidget); + /** + * @brief Determines if a QWidget needs to be show and if so, shows it + * + * Based on the the autopilot and the current view it queries the settings and shows the + * widget if necessary + * + * @param centralWidget The QWidget requested to be shown + * @param view The view for which the QWidget is requested + */ + void showTheCentralWidget (TOOLS_WIDGET_NAMES centralWidget, VIEW_SECTIONS view); + + + /** @brief Keeps track of the current view */ VIEW_SECTIONS currentView; - int aboutToQuit; - //QHash settingsSections; QStatusBar* statusBar; QStatusBar* createStatusBar(); - void loadWidgets(); + void clearView(); diff --git a/src/ui/MapWidget.cc b/src/ui/MapWidget.cc index 1f244dd..45fe505 100644 --- a/src/ui/MapWidget.cc +++ b/src/ui/MapWidget.cc @@ -562,6 +562,7 @@ void MapWidget::updateGlobalPosition(UASInterface* uas, double lat, double lon, // Icon QPen* pointpen = new QPen(uasColor); + qDebug() << uas->getUASName(); MAV2DIcon* p = new MAV2DIcon(lat, lon, 20, uas->getUASName(), qmapcontrol::Point::Middle, pointpen); uasIcons.insert(uas->getUASID(), p); geomLayer->addGeometry(p); From a07656f388b39af1b41bfe95da91c199e66112ea Mon Sep 17 00:00:00 2001 From: Mariano Lizarraga Date: Wed, 15 Dec 2010 01:46:07 -0600 Subject: [PATCH 15/21] Fixed issue where central menus where not updated correctly --- src/ui/MainWindow.cc | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index ca98ec2..54011b1 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -1070,6 +1070,7 @@ void MainWindow::presentView() { #endif qDebug() << "LC"; + showTheCentralWidget(CENTRAL_LINECHART, currentView); if (linechartWidget){ qDebug () << buildMenuKey (SUB_SECTION_CHECKED,CENTRAL_LINECHART,currentView) << settings.value(buildMenuKey (SUB_SECTION_CHECKED,CENTRAL_LINECHART,currentView)).toBool() ; @@ -1094,6 +1095,7 @@ void MainWindow::presentView() { showTheCentralWidget(CENTRAL_PROTOCOL, currentView); // HEAD UP DISPLAY + showTheCentralWidget(CENTRAL_HUD, currentView); qDebug() << "HUD"; if (hudWidget){ qDebug() << buildMenuKey(SUB_SECTION_CHECKED,CENTRAL_HUD,currentView) << From a6de3c3bbc4a4f0a289e80ae4baf810e8964b41d Mon Sep 17 00:00:00 2001 From: James Goppert Date: Wed, 15 Dec 2010 15:24:55 -0500 Subject: [PATCH 16/21] Fixed build errors for linux. --- src/ui/map3D/GCManipulator.cc | 6 +++--- src/ui/map3D/QMap3D.cc | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/ui/map3D/GCManipulator.cc b/src/ui/map3D/GCManipulator.cc index 6f2fb0e..05c9b24 100644 --- a/src/ui/map3D/GCManipulator.cc +++ b/src/ui/map3D/GCManipulator.cc @@ -254,10 +254,10 @@ GCManipulator::calcMovement(void) if (buttonMask == GUIEventAdapter::LEFT_MOUSE_BUTTON) { // rotate camera -#ifdef __APPLE__ - osg::Vec3d axis; -#else +#ifdef __WIN32__ osg::Vec3 axis; +#else + osg::Vec3d axis; #endif float angle; diff --git a/src/ui/map3D/QMap3D.cc b/src/ui/map3D/QMap3D.cc index ab4c98b..83ff901 100644 --- a/src/ui/map3D/QMap3D.cc +++ b/src/ui/map3D/QMap3D.cc @@ -39,7 +39,7 @@ QMap3D::QMap3D(QWidget * parent, const char * name, WindowFlags f) : QWidget(parent,f) { setupUi(this); - graphicsView->setCameraManipulator(new osgEarthUtil::EarthManipulator); + graphicsView->setCameraManipulator(new osgEarth::Util::EarthManipulator); graphicsView->setSceneData(new osg::Group); graphicsView->updateCamera(); show(); From 13a505117b6dd02b35919c88c9bd723675cb25cd Mon Sep 17 00:00:00 2001 From: Brandon Wampler Date: Wed, 15 Dec 2010 15:56:08 -0500 Subject: [PATCH 17/21] implemented delete for communication links --- src/comm/LinkManager.cc | 26 ++++++++++++++++++++++++-- src/comm/LinkManager.h | 2 ++ src/comm/SerialLink.cc | 7 +++++-- src/uas/UAS.cc | 9 ++++++--- src/ui/CommConfigurationWindow.cc | 22 ++++++++++++++++++---- src/ui/CommConfigurationWindow.h | 9 ++++++++- src/ui/CommSettings.ui | 25 ++----------------------- src/ui/SerialConfigurationWindow.cc | 9 --------- src/ui/SerialConfigurationWindow.h | 6 ------ src/ui/map3D/GCManipulator.cc | 6 +++--- src/ui/map3D/QMap3D.cc | 2 +- 11 files changed, 69 insertions(+), 54 deletions(-) diff --git a/src/comm/LinkManager.cc b/src/comm/LinkManager.cc index 76c4fde..91b92bf 100644 --- a/src/comm/LinkManager.cc +++ b/src/comm/LinkManager.cc @@ -32,6 +32,7 @@ This file is part of the QGROUNDCONTROL project #include #include #include "LinkManager.h" +#include #include @@ -65,6 +66,7 @@ LinkManager::~LinkManager() void LinkManager::add(LinkInterface* link) { + if(!link) return; links.append(link); emit newLink(link); } @@ -73,6 +75,7 @@ void LinkManager::addProtocol(LinkInterface* link, ProtocolInterface* protocol) { // Connect link to protocol // the protocol will receive new bytes from the link + if(!link || !protocol) return; connect(link, SIGNAL(bytesReceived(LinkInterface*, QByteArray)), protocol, SLOT(receiveBytes(LinkInterface*, QByteArray))); // Store the connection information in the protocol links map protocolLinks.insertMulti(protocol, link); @@ -91,7 +94,8 @@ bool LinkManager::connectAll() foreach (LinkInterface* link, links) { - if(! link->connect()) allConnected = false; + if(!link) {} + else if(!link->connect()) allConnected = false; } return allConnected; @@ -103,7 +107,9 @@ bool LinkManager::disconnectAll() foreach (LinkInterface* link, links) { - if(! link->disconnect()) allDisconnected = false; + static int i=0; + if(!link){} + else if(!link->disconnect()) allDisconnected = false; } return allDisconnected; @@ -111,14 +117,30 @@ bool LinkManager::disconnectAll() bool LinkManager::connectLink(LinkInterface* link) { + if(!link) return false; return link->connect(); } bool LinkManager::disconnectLink(LinkInterface* link) { + if(!link) return false; return link->disconnect(); } +bool LinkManager::removeLink(LinkInterface* link) +{ + if(link) + { + for (int i=0; i < QList(links).size(); i++) + { + if(link==links.at(i)) + { + links.removeAt(i); //remove from link list + } + } + } +} + /** * The access time is linear in the number of links. * diff --git a/src/comm/LinkManager.h b/src/comm/LinkManager.h index b176875..8d283c3 100644 --- a/src/comm/LinkManager.h +++ b/src/comm/LinkManager.h @@ -67,6 +67,8 @@ public slots: void add(LinkInterface* link); void addProtocol(LinkInterface* link, ProtocolInterface* protocol); + bool removeLink(LinkInterface* link); + bool connectAll(); bool connectLink(LinkInterface* link); diff --git a/src/comm/SerialLink.cc b/src/comm/SerialLink.cc index d4b48cd..9bf345b 100644 --- a/src/comm/SerialLink.cc +++ b/src/comm/SerialLink.cc @@ -34,6 +34,7 @@ This file is part of the QGROUNDCONTROL project #include "SerialLink.h" #include "LinkManager.h" #include +#include #ifdef _WIN32 #include "windows.h" #endif @@ -106,7 +107,7 @@ SerialLink::SerialLink(QString portname, BaudRateType baudrate, FlowType flow, P SerialLink::~SerialLink() { disconnect(); - delete port; + if(port) delete port; port = NULL; } @@ -239,6 +240,8 @@ bool SerialLink::disconnect() port->close(); dataMutex.unlock(); + if(this->isRunning()) this->terminate(); //stop running the thread, restart it upon connect + bool closed = true; //port->isOpen(); @@ -516,7 +519,7 @@ bool SerialLink::setPortName(QString portName) this->porthandle = "\\\\.\\" + this->porthandle; } #endif - delete port; + if(port) delete port; port = new QextSerialPort(porthandle, QextSerialPort::Polling); port->setBaudRate(baudrate); diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 69af7a9..263102e 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -93,6 +93,7 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(), UAS::~UAS() { delete links; + links=NULL; } int UAS::getUASID() const @@ -124,6 +125,7 @@ void UAS::setSelected() void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) { + if (!link) return; if (!links->contains(link)) { addLink(link); @@ -365,9 +367,9 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) positionLock = true; // Send to patch antenna - mavlink_message_t msg; - mavlink_msg_global_position_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, pos.usec, pos.lat, pos.lon, pos.alt, pos.vx, pos.vy, pos.vz); - sendMessage(msg); +// mavlink_message_t msg; +// mavlink_msg_global_position_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, pos.usec, pos.lat, pos.lon, pos.alt, pos.vx, pos.vy, pos.vz); +// sendMessage(msg); } break; case MAVLINK_MSG_ID_GPS_RAW: @@ -831,6 +833,7 @@ void UAS::sendMessage(mavlink_message_t message) void UAS::sendMessage(LinkInterface* link, mavlink_message_t message) { + if(!link) return; // Create buffer uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; // Write message into buffer, prepending start sign diff --git a/src/ui/CommConfigurationWindow.cc b/src/ui/CommConfigurationWindow.cc index 0708aee..33e57ba 100644 --- a/src/ui/CommConfigurationWindow.cc +++ b/src/ui/CommConfigurationWindow.cc @@ -47,6 +47,7 @@ This file is part of the QGROUNDCONTROL project #endif #include "MAVLinkProtocol.h" #include "MAVLinkSettingsWidget.h" +#include "LinkManager.h" CommConfigurationWindow::CommConfigurationWindow(LinkInterface* link, ProtocolInterface* protocol, QWidget *parent, Qt::WindowFlags flags) : QWidget(parent, flags) { @@ -58,6 +59,10 @@ CommConfigurationWindow::CommConfigurationWindow(LinkInterface* link, ProtocolIn // add link types ui.linkType->addItem("Serial",QGC_LINK_SERIAL); ui.linkType->addItem("UDP",QGC_LINK_UDP); + ui.linkType->addItem("Simulation",QGC_LINK_SIMULATION); + ui.linkType->addItem("Serial Forwarding",QGC_LINK_FORWARDING); + + ui.connectionType->addItem("MAVLink", QGC_PROTOCOL_MAVLINK); // Create action to open this menu // Create configuration action for this link @@ -195,11 +200,20 @@ void CommConfigurationWindow::setLinkName(QString name) void CommConfigurationWindow::remove() { - link->disconnect(); - //delete link; - //delete action; + if(action) delete action; //delete action first since it has a pointer to link + action=NULL; + + if(link) + { + LinkManager::instance()->removeLink(link); //remove link from LinkManager list + link->disconnect(); //disconnect port, and also calls terminate() to stop the thread + if (link->isRunning()) link->terminate(); // terminate() the serial thread just in case it is still running + link->wait(); // wait() until thread is stoped before deleting + delete link; + } + link=NULL; + this->window()->close(); - qDebug() << "TODO: Link cannot be deleted: CommConfigurationWindow::remove() NOT IMPLEMENTED!"; } void CommConfigurationWindow::connectionState(bool connect) diff --git a/src/ui/CommConfigurationWindow.h b/src/ui/CommConfigurationWindow.h index fce37db..93f1566 100644 --- a/src/ui/CommConfigurationWindow.h +++ b/src/ui/CommConfigurationWindow.h @@ -42,7 +42,14 @@ This file is part of the QGROUNDCONTROL project enum qgc_link_t { QGC_LINK_SERIAL, - QGC_LINK_UDP + QGC_LINK_UDP, + QGC_LINK_SIMULATION, + QGC_LINK_FORWARDING +}; + +enum qgc_protocol_t +{ + QGC_PROTOCOL_MAVLINK }; #ifdef OPAL_RT diff --git a/src/ui/CommSettings.ui b/src/ui/CommSettings.ui index 8aec750..71a4c30 100644 --- a/src/ui/CommSettings.ui +++ b/src/ui/CommSettings.ui @@ -43,23 +43,7 @@ - - - - Serial Link - - - - - UDP - - - - - Simulation - - - + @@ -71,13 +55,8 @@ - 0 + -1 - - - MAVLink - - diff --git a/src/ui/SerialConfigurationWindow.cc b/src/ui/SerialConfigurationWindow.cc index 5542bc9..e070fca 100644 --- a/src/ui/SerialConfigurationWindow.cc +++ b/src/ui/SerialConfigurationWindow.cc @@ -509,12 +509,3 @@ void SerialConfigurationWindow::setLinkName(QString name) setWindowTitle(tr("Configuration of ") + link->getName()); } -void SerialConfigurationWindow::remove() -{ - link->disconnect(); - //delete link; - //delete action; - this->window()->close(); - qDebug() << "TODO: Link cannot be deleted: SerialConfigurationWindow::remove() NOT IMPLEMENTED!"; -} - diff --git a/src/ui/SerialConfigurationWindow.h b/src/ui/SerialConfigurationWindow.h index d71fb95..abfa5c1 100644 --- a/src/ui/SerialConfigurationWindow.h +++ b/src/ui/SerialConfigurationWindow.h @@ -60,12 +60,6 @@ public slots: void setParityEven(); void setPortName(QString port); void setLinkName(QString name); - /** - * @brief Remove this link - * - * Disconnects the associated link, removes it from all menus and closes the window. - */ - void remove(); void setupPortList(); protected slots: diff --git a/src/ui/map3D/GCManipulator.cc b/src/ui/map3D/GCManipulator.cc index 6f2fb0e..05c9b24 100644 --- a/src/ui/map3D/GCManipulator.cc +++ b/src/ui/map3D/GCManipulator.cc @@ -254,10 +254,10 @@ GCManipulator::calcMovement(void) if (buttonMask == GUIEventAdapter::LEFT_MOUSE_BUTTON) { // rotate camera -#ifdef __APPLE__ - osg::Vec3d axis; -#else +#ifdef __WIN32__ osg::Vec3 axis; +#else + osg::Vec3d axis; #endif float angle; diff --git a/src/ui/map3D/QMap3D.cc b/src/ui/map3D/QMap3D.cc index ab4c98b..83ff901 100644 --- a/src/ui/map3D/QMap3D.cc +++ b/src/ui/map3D/QMap3D.cc @@ -39,7 +39,7 @@ QMap3D::QMap3D(QWidget * parent, const char * name, WindowFlags f) : QWidget(parent,f) { setupUi(this); - graphicsView->setCameraManipulator(new osgEarthUtil::EarthManipulator); + graphicsView->setCameraManipulator(new osgEarth::Util::EarthManipulator); graphicsView->setSceneData(new osg::Group); graphicsView->updateCamera(); show(); From c9a863b7c78bbf0806ec3c96e365f45f1f448deb Mon Sep 17 00:00:00 2001 From: lm Date: Thu, 16 Dec 2010 11:14:53 +0100 Subject: [PATCH 18/21] Minor bugfixes and compile fixes --- src/uas/UASManager.cc | 5 +++++ src/uas/UASManager.h | 2 ++ src/ui/MainWindow.cc | 4 ---- src/ui/ParameterInterface.cc | 9 +++++++++ src/ui/linechart/LinechartPlot.cc | 2 +- src/ui/linechart/Linecharts.cc | 13 +++++++++++++ src/ui/map3D/GCManipulator.cc | 7 ++++--- src/ui/map3D/QMap3D.cc | 4 ++++ 8 files changed, 38 insertions(+), 8 deletions(-) diff --git a/src/uas/UASManager.cc b/src/uas/UASManager.cc index 2af5a5f..fabfec6 100644 --- a/src/uas/UASManager.cc +++ b/src/uas/UASManager.cc @@ -85,6 +85,11 @@ void UASManager::addUAS(UASInterface* uas) } } +QList UASManager::getUASList() +{ + return systems.values(); +} + UASInterface* UASManager::getActiveUAS() { if(!activeUAS) diff --git a/src/uas/UASManager.h b/src/uas/UASManager.h index c5ca03c..b4d9778 100644 --- a/src/uas/UASManager.h +++ b/src/uas/UASManager.h @@ -69,6 +69,8 @@ public: **/ UASInterface* getUASForId(int id); + QList getUASList(); + public slots: diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index 54011b1..b6827ac 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -504,10 +504,6 @@ void MainWindow::connectPxWidgets() { if (linechartWidget) { - connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)), - linechartWidget, SLOT(addSystem(UASInterface*))); - connect(UASManager::instance(), SIGNAL(activeUASSet(int)), - linechartWidget, SLOT(selectSystem(int))); connect(linechartWidget, SIGNAL(logfileWritten(QString)), this, SLOT(loadDataView(QString))); } diff --git a/src/ui/ParameterInterface.cc b/src/ui/ParameterInterface.cc index f44e083..d6a3a59 100644 --- a/src/ui/ParameterInterface.cc +++ b/src/ui/ParameterInterface.cc @@ -51,6 +51,15 @@ ParameterInterface::ParameterInterface(QWidget *parent) : // Setup UI connections connect(m_ui->vehicleComboBox, SIGNAL(activated(int)), this, SLOT(selectUAS(int))); + // Get current MAV list + QList systems = UASManager::instance()->getUASList(); + + // Add each of them + foreach (UASInterface* sys, systems) + { + addUAS(sys); + } + // Setup MAV connections connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)), this, SLOT(addUAS(UASInterface*))); this->setVisible(false); diff --git a/src/ui/linechart/LinechartPlot.cc b/src/ui/linechart/LinechartPlot.cc index 7d16138..3a7cb01 100644 --- a/src/ui/linechart/LinechartPlot.cc +++ b/src/ui/linechart/LinechartPlot.cc @@ -56,7 +56,7 @@ maxInterval(MAX_STORAGE_INTERVAL), timeScaleStep(DEFAULT_SCALE_INTERVAL), // 10 seconds automaticScrollActive(false), m_active(true), -m_groundTime(false), +m_groundTime(true), d_data(NULL), d_curve(NULL) { diff --git a/src/ui/linechart/Linecharts.cc b/src/ui/linechart/Linecharts.cc index 2cfaf83..b47ea71 100644 --- a/src/ui/linechart/Linecharts.cc +++ b/src/ui/linechart/Linecharts.cc @@ -1,4 +1,5 @@ #include "Linecharts.h" +#include "UASManager.h" Linecharts::Linecharts(QWidget *parent) : QStackedWidget(parent), @@ -6,6 +7,18 @@ Linecharts::Linecharts(QWidget *parent) : active(true) { this->setVisible(false); + // Get current MAV list + QList systems = UASManager::instance()->getUASList(); + + // Add each of them + foreach (UASInterface* sys, systems) + { + addSystem(sys); + } + connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)), + this, SLOT(addSystem(UASInterface*))); + connect(UASManager::instance(), SIGNAL(activeUASSet(int)), + this, SLOT(selectSystem(int))); } diff --git a/src/ui/map3D/GCManipulator.cc b/src/ui/map3D/GCManipulator.cc index 05c9b24..06a7527 100644 --- a/src/ui/map3D/GCManipulator.cc +++ b/src/ui/map3D/GCManipulator.cc @@ -30,6 +30,7 @@ This file is part of the QGROUNDCONTROL project */ #include "GCManipulator.h" +#include GCManipulator::GCManipulator() { @@ -254,10 +255,10 @@ GCManipulator::calcMovement(void) if (buttonMask == GUIEventAdapter::LEFT_MOUSE_BUTTON) { // rotate camera -#ifdef __WIN32__ - osg::Vec3 axis; -#else +#if ((OPENSCENEGRAPH_MAJOR_VERSION == 2) & (OPENSCENEGRAPH_MINOR_VERSION > 8)) | (OPENSCENEGRAPH_MAJOR_VERSION > 2) osg::Vec3d axis; +#else + osg::Vec3 axis; #endif float angle; diff --git a/src/ui/map3D/QMap3D.cc b/src/ui/map3D/QMap3D.cc index 83ff901..0a6e11f 100644 --- a/src/ui/map3D/QMap3D.cc +++ b/src/ui/map3D/QMap3D.cc @@ -39,7 +39,11 @@ QMap3D::QMap3D(QWidget * parent, const char * name, WindowFlags f) : QWidget(parent,f) { setupUi(this); +#if ((OPENSCENEGRAPH_MAJOR_VERSION == 2) & (OPENSCENEGRAPH_MINOR_VERSION > 8)) | (OPENSCENEGRAPH_MAJOR_VERSION > 2) graphicsView->setCameraManipulator(new osgEarth::Util::EarthManipulator); +#else + graphicsView->setCameraManipulator(new osgEarthUtil::EarthManipulator); +#endif graphicsView->setSceneData(new osg::Group); graphicsView->updateCamera(); show(); From f731943ff2274351c3a7075bba646b7946a5ed46 Mon Sep 17 00:00:00 2001 From: lm Date: Thu, 16 Dec 2010 13:14:45 +0100 Subject: [PATCH 19/21] Fixed issue with serial port --- src/comm/SerialLink.cc | 65 +++++++++++++++++++++++++++++++++----------------- 1 file changed, 43 insertions(+), 22 deletions(-) diff --git a/src/comm/SerialLink.cc b/src/comm/SerialLink.cc index 1692744..404a8db 100644 --- a/src/comm/SerialLink.cc +++ b/src/comm/SerialLink.cc @@ -60,9 +60,18 @@ SerialLink::SerialLink(QString portname, BaudRateType baudrate, FlowType flow, P // Load defaults from settings QSettings settings(QGC::COMPANYNAME, QGC::APPNAME); + settings.sync(); if (settings.contains("SERIALLINK_COMM_PORT")) { this->porthandle = settings.value("SERIALLINK_COMM_PORT").toString(); + } + + // *nix (Linux, MacOS tested) serial port support + port = new QextSerialPort(porthandle, QextSerialPort::Polling); + //port = new QextSerialPort(porthandle, QextSerialPort::EventDriven); + + if (settings.contains("SERIALLINK_COMM_PORT")) + { setBaudRate(settings.value("SERIALLINK_COMM_BAUD").toInt()); setParityType(settings.value("SERIALLINK_COMM_PARITY").toInt()); setStopBitsType(settings.value("SERIALLINK_COMM_STOPBITS").toInt()); @@ -77,6 +86,12 @@ SerialLink::SerialLink(QString portname, BaudRateType baudrate, FlowType flow, P this->stopBits = stopBits; this->timeout = 1; ///< The timeout controls how long the program flow should wait for new serial bytes. As we're polling, we don't want to wait at all. } + port->setTimeout(timeout); // Timeout of 0 ms, we don't want to wait for data, we just poll again next time + port->setBaudRate(baudrate); + port->setFlowControl(flow); + port->setParity(parity); + port->setDataBits(dataBits); + port->setStopBits(stopBits); // Set the port name if (porthandle == "") @@ -105,15 +120,7 @@ SerialLink::SerialLink(QString portname, BaudRateType baudrate, FlowType flow, P //some other error occurred. Inform user. } #else - // *nix (Linux, MacOS tested) serial port support - port = new QextSerialPort(porthandle, QextSerialPort::Polling); - //port = new QextSerialPort(porthandle, QextSerialPort::EventDriven); - port->setTimeout(timeout); // Timeout of 0 ms, we don't want to wait for data, we just poll again next time - port->setBaudRate(baudrate); - port->setFlowControl(flow); - port->setParity(parity); - port->setDataBits(dataBits); - port->setStopBits(stopBits); + #endif // Link is setup, register it with link manager @@ -301,15 +308,6 @@ bool SerialLink::connect() **/ bool SerialLink::hardwareConnect() { - // Store settings - QSettings settings(QGC::COMPANYNAME, QGC::APPNAME); - settings.setValue("SERIALLINK_COMM_PORT", this->porthandle); - settings.setValue("SERIALLINK_COMM_BAUD", this->baudrate); - settings.setValue("SERIALLINK_COMM_PARITY", this->parity); - settings.setValue("SERIALLINK_COMM_STOPBITS", this->stopBits); - settings.setValue("SERIALLINK_COMM_DATABITS", this->dataBits); - settings.sync(); - QObject::connect(port, SIGNAL(aboutToClose()), this, SIGNAL(disconnected())); port->open(QIODevice::ReadWrite); @@ -326,6 +324,15 @@ bool SerialLink::hardwareConnect() if(connectionUp) { emit connected(); emit connected(true); + + // Store settings + QSettings settings(QGC::COMPANYNAME, QGC::APPNAME); + settings.setValue("SERIALLINK_COMM_PORT", this->porthandle); + settings.setValue("SERIALLINK_COMM_BAUD", getBaudRate()); + settings.setValue("SERIALLINK_COMM_PARITY", getParityType()); + settings.setValue("SERIALLINK_COMM_STOPBITS", getStopBitsType()); + settings.setValue("SERIALLINK_COMM_DATABITS", getDataBitsType()); + settings.sync(); } return connectionUp; @@ -339,7 +346,14 @@ bool SerialLink::hardwareConnect() **/ bool SerialLink::isConnected() { - return port->isOpen(); + if (port) + { + return port->isOpen(); + } + else + { + return false; + } } int SerialLink::getId() @@ -735,9 +749,16 @@ bool SerialLink::setBaudRate(int rate) break; } - port->setBaudRate(this->baudrate); - if(reconnect) connect(); - return accepted; + if (port) + { + port->setBaudRate(this->baudrate); + if(reconnect) connect(); + return accepted; + } + else + { + return false; + } } bool SerialLink::setFlowType(int flow) From a43f265042ecf63241a9b090f0f67ad02ea61d2f Mon Sep 17 00:00:00 2001 From: James Goppert Date: Thu, 16 Dec 2010 13:41:55 -0500 Subject: [PATCH 20/21] Added orbit radius to navigation waypoints. --- src/ui/WaypointView.cc | 1 + 1 file changed, 1 insertion(+) diff --git a/src/ui/WaypointView.cc b/src/ui/WaypointView.cc index b291fc8..ca7f981 100644 --- a/src/ui/WaypointView.cc +++ b/src/ui/WaypointView.cc @@ -146,6 +146,7 @@ void WaypointView::changedAction(int index) break; case MAV_ACTION_NAVIGATE: m_ui->autoContinue->show(); + m_ui->orbitSpinBox->show(); break; case MAV_ACTION_LOITER: m_ui->orbitSpinBox->show(); From f14ba61fbf087d0aa5e8ec654383aaeb28506f98 Mon Sep 17 00:00:00 2001 From: Brandon Wampler Date: Thu, 16 Dec 2010 15:02:48 -0500 Subject: [PATCH 21/21] hacked a forwarding function for patch antennas added forwardMessage() function to send out Global Position over serial links that are not part of the UAS's link list. --- src/uas/UAS.cc | 28 +++++++++++++++++++++++++++- src/uas/UAS.h | 3 +++ 2 files changed, 30 insertions(+), 1 deletion(-) diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 95d941a..f503a06 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -43,6 +43,8 @@ This file is part of the QGROUNDCONTROL project #include "GAudioOutput.h" #include "MAVLinkProtocol.h" #include "QGCMAVLink.h" +#include "LinkManager.h" +#include "SerialLink.h" UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(), @@ -373,6 +375,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) GAudioOutput::instance()->notifyPositive(); } positionLock = true; + //TODO fix this hack for forwarding of global position for patch antenna tracking + forwardMessage(message); } break; case MAVLINK_MSG_ID_GPS_RAW: @@ -834,6 +838,28 @@ void UAS::sendMessage(mavlink_message_t message) } } +void UAS::forwardMessage(mavlink_message_t message) +{ + // Emit message on all links that are currently connected + QListlink_list = LinkManager::instance()->getLinksForProtocol(mavlink); + foreach(LinkInterface* link, link_list) + { + SerialLink* serial = dynamic_cast(link); + if(serial != 0) + { + + for(int i=0;isize();i++) + { + if(serial != links->at(i)) + { + qDebug()<<"Forwarding Over link: "<getName()<<" "<append(link); } //links->append(link); - //qDebug() << " ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK"; + qDebug() << link<<" ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK"; } /** diff --git a/src/uas/UAS.h b/src/uas/UAS.h index 65b514b..22c655f 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -218,6 +218,9 @@ public slots: /** @brief Send a message over all links this UAS can be reached with (!= all links) */ void sendMessage(mavlink_message_t message); + /** @brief Temporary Hack for sending packets to patch Antenna. Send a message over all serial links except for this UAS's */ + void forwardMessage(mavlink_message_t message); + /** @brief Set this UAS as the system currently in focus, e.g. in the main display widgets */ void setSelected();