diff --git a/.cproject b/.cproject deleted file mode 100644 index 43f99020af95d6f5d0d494e76fd4774c485bbc46..0000000000000000000000000000000000000000 --- a/.cproject +++ /dev/null @@ -1,148 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - mingw32-make - - release - false - false - true - - - mingw32-make - - debug - false - false - true - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/.gitignore b/.gitignore index ef46a13f4a61d32bab84e378d9ca59f4085e5951..ddd3a437bcf567f86a35d64d86f8ecc3575d2098 100644 --- a/.gitignore +++ b/.gitignore @@ -30,4 +30,13 @@ user_config.pri *.ncb *.vcproj -*.sln \ No newline at end of file +*.sln +*.sln +*.vcproj +*.user +*.ncb +*.idb +*.project +*.cproject +*.sln +*.suo \ No newline at end of file diff --git a/.project b/.project deleted file mode 100644 index fc9b8c2de7d855347764478762ca7b92aed604e6..0000000000000000000000000000000000000000 --- a/.project +++ /dev/null @@ -1,88 +0,0 @@ - - - qgroundcontrol - - - - - - com.trolltech.qtcppproject.QtMakefileGenerator - - - - - org.eclipse.cdt.make.core.makeBuilder - clean,full,incremental, - - - org.eclipse.cdt.core.errorOutputParser - org.eclipse.cdt.core.GASErrorParser;org.eclipse.cdt.core.GCCErrorParser;org.eclipse.cdt.core.GLDErrorParser;org.eclipse.cdt.core.GmakeErrorParser;org.eclipse.cdt.core.VCErrorParser;org.eclipse.cdt.core.CWDLocator;org.eclipse.cdt.core.MakeErrorParser; - - - org.eclipse.cdt.make.core.append_environment - true - - - org.eclipse.cdt.make.core.build.arguments - - - - org.eclipse.cdt.make.core.build.command - mingw32-make - - - org.eclipse.cdt.make.core.build.target.auto - debug - - - org.eclipse.cdt.make.core.build.target.clean - clean - - - org.eclipse.cdt.make.core.build.target.inc - debug - - - org.eclipse.cdt.make.core.enableAutoBuild - false - - - org.eclipse.cdt.make.core.enableCleanBuild - true - - - org.eclipse.cdt.make.core.enableFullBuild - true - - - org.eclipse.cdt.make.core.enabledIncrementalBuild - true - - - org.eclipse.cdt.make.core.environment - QMAKESPEC=win32-g++|PATH=C:\\Qt\\2010.04\\qt\\bin;${env_var:PATH}| - - - org.eclipse.cdt.make.core.stopOnError - false - - - org.eclipse.cdt.make.core.useDefaultBuildCmd - false - - - - - org.eclipse.cdt.make.core.ScannerConfigBuilder - - - - - - org.eclipse.cdt.core.cnature - org.eclipse.cdt.core.ccnature - org.eclipse.cdt.make.core.makeNature - org.eclipse.cdt.make.core.ScannerConfigNature - com.trolltech.qtcppproject.QtNature - - diff --git a/images/earth.html b/images/earth.html index 6f423cb2604b37f014a83614ee796939fd27aec8..791c1f2f794231e3c4a8ea73f884c710260aad33 100644 --- a/images/earth.html +++ b/images/earth.html @@ -13,12 +13,12 @@ google.load("earth", "1", { 'language': 'en'}); var ge = null; var initialized = false; -var aircraft = new Array(); var currAircraft = 220; -var followAircraft = false; +var followEnabled = false; var currLat = 47.3769; var currLon = 8.549444; var currAlt = 470; +var currFollowHeading = 0.0; var homeLat = 0; var homeLon = 0; @@ -32,11 +32,20 @@ var currTilt = 40.0; ///<< The tilt angle (in degrees) var currFollowTilt = 40.0; var currView = null; +var M_PI = 3.14159265; + var planeOrient; var planeLoc; +var aircraft = []; +var attitudes = []; +var locations = []; +var trails = []; +var trail; +var lineStringPlacemark; +var lineStyle; // Aircraft class @@ -53,7 +62,10 @@ function init() { - +function setCurrAircraft(id) +{ + currAircraft = id; +} function setGCSHome(lat, lon, alt) { @@ -86,36 +98,11 @@ function setGCSHome(lat, lon, alt) { homeGroundLevel = alt; } - - - - - - - goHome(); } -function initCallback(object) +function createAircraft(id, type, color) { - ge = object; - ge.getWindow().setVisibility(true); - ge.getOptions().setStatusBarVisibility(true); - ge.getOptions().setScaleLegendVisibility(true); - ge.getOptions().setFlyToSpeed(ge.SPEED_TELEPORT); - ge.getNavigationControl().setVisibility(ge.VISIBILITY_AUTO); - - ge.getLayerRoot().enableLayerById(ge.LAYER_TERRAIN, true); - ge.getLayerRoot().enableLayerById(ge.LAYER_BUILDINGS, true); - ge.getLayerRoot().enableLayerById(ge.LAYER_BORDERS, true); - ge.getLayerRoot().enableLayerById(ge.LAYER_TREES, true); - - // Now after the Google Earth initialization, initialize the GCS view - setGCSHome(currLat, currLon, currAlt); - - - // Create the first aircraft model - // Load 3D model - var planePlacemark = ge.createPlacemark(''); + planePlacemark = ge.createPlacemark(''); planePlacemark.setName('aircraft'); planeModel = ge.createModel(''); ge.getFeatures().appendChild(planePlacemark); @@ -135,9 +122,77 @@ function initCallback(object) planePlacemark.setGeometry(planeModel); - setAircraftPositionAttitude(220, 47.3772, currLon, currAlt+50, 20, 15, 50); - enableFollowing(true); - updateFollowAircraft(); + // Write into global structure + aircraft[id] = planePlacemark; + attitudes[id] = planeOrient; + locations[id] = planeLoc; + + //createTrail(id, color); +} + +function createTrail(id, color) +{ + lineStringPlacemark = ge.createPlacemark(''); + // Create the placemark +// Create the LineString; set it to extend down to the ground +// and set the altitude mode +trail = ge.createLineString(''); +lineStringPlacemark.setGeometry(trail); +trail.setExtrude(true); +trail.setAltitudeMode(ge.ALTITUDE_ABSOLUTE); + +// Add LineString points +//lineString.getCoordinates().pushLatLngAlt(48.754, -121.835, 700); + +// Create a style and set width and color of line +lineStringPlacemark.setStyleSelector(ge.createStyle('')); +lineStyle = lineStringPlacemark.getStyleSelector().getLineStyle(); +lineStyle.setWidth(5); +lineStyle.getColor().set(color); +//lineStyle.getColor().set(color);  // aabbggrr format + +// Add the feature to Earth +ge.getFeatures().appendChild(lineStringPlacemark); + +} + +function addTrailPosition(id, lat, lon, alt) +{ + trail.setExtrude(true); +trail.setAltitudeMode(ge.ALTITUDE_ABSOLUTE); + +// Add LineString points +//lineString.getCoordinates().pushLatLngAlt(48.754, -121.835, 700); +trail.getCoordinates().pushLatLngAlt(currLat, currLon, currAlt); + +// Create a style and set width and color of line +lineStringPlacemark.setStyleSelector(ge.createStyle('')); +lineStyle = lineStringPlacemark.getStyleSelector().getLineStyle(); +lineStyle.setWidth(5); +lineStyle.getColor().set('99bbaaff'); +//lineStyle.getColor().set(color);  // aabbggrr format + +// Add the feature to Earth +ge.getFeatures().replaceChild(lineStringPlacemark, lineStringPlacemark); +} + +function initCallback(object) +{ + ge = object; + ge.getWindow().setVisibility(true); + ge.getOptions().setStatusBarVisibility(true); + ge.getOptions().setScaleLegendVisibility(true); + ge.getOptions().setFlyToSpeed(ge.SPEED_TELEPORT); + ge.getNavigationControl().setVisibility(ge.VISIBILITY_AUTO); + + ge.getLayerRoot().enableLayerById(ge.LAYER_TERRAIN, true); + ge.getLayerRoot().enableLayerById(ge.LAYER_BUILDINGS, true); + ge.getLayerRoot().enableLayerById(ge.LAYER_BORDERS, true); + ge.getLayerRoot().enableLayerById(ge.LAYER_TREES, true); + + createTrail(220, 'bb2222ff'); + createAircraft(220); + initialized = true; @@ -150,20 +205,34 @@ function setAircraftPositionAttitude(id, lat, lon, alt, roll, pitch, yaw) currLat = lat; currLon = lon; currAlt = alt; + currFollowHeading = ((yaw/M_PI)+1.0)*360.0; } - planeOrient.setRoll(roll); - planeOrient.setTilt(pitch); - planeOrient.setHeading(yaw); + // FIXME Currently invalid conversion from right-handed z-down to z-up frame + planeOrient.setRoll(((roll/M_PI)+1.0)*360.0); + planeOrient.setTilt(((pitch/M_PI)+1.0)*360.0); + planeOrient.setHeading(((yaw/M_PI)+1.0)*360.0); - planeLoc.setLatitude(lat); - planeLoc.setLongitude(lon); - planeLoc.setAltitude(alt); + planeLoc.setLatitude(lat); + planeLoc.setLongitude(lon); + planeLoc.setAltitude(alt); } +function enableDaylight(enabled) +{ + if(enabled) + { + ge.getSun().setVisibility(true); + } + else + { + ge.getSun().setVisibility(false); + } +} + function goHome() { var currView = ge.getView().copyAsLookAt(ge.ALTITUDE_ABSOLUTE); @@ -180,24 +249,16 @@ function setCurrentAircraft(id) currAircraft = id; } -function enableFollowing(follow) -{ - followEnabled = follow; -} - - function updateFollowAircraft() { - if (followEnabled) - { currView = ge.getView().copyAsLookAt(ge.ALTITUDE_ABSOLUTE); currView.setLatitude(currLat); currView.setLongitude(currLon); currView.setAltitude(currAlt); currView.setRange(currViewRange); currView.setTilt(currFollowTilt); + currView.setHeading(currFollowHeading-90.0); ge.getView().setAbstractView(currView); - } } function failureCallback(object) diff --git a/images/patterns/abby.jpg b/images/patterns/abby.jpg new file mode 100644 index 0000000000000000000000000000000000000000..b39b9a5e53e0b741934eec8c7d8d5bcf4b88faa7 Binary files /dev/null and b/images/patterns/abby.jpg differ diff --git a/images/patterns/cake.jpg b/images/patterns/cake.jpg new file mode 100644 index 0000000000000000000000000000000000000000..a6ea3434ab67b1a0bd5e045e08d4efef8a796ffa Binary files /dev/null and b/images/patterns/cake.jpg differ diff --git a/images/patterns/cola.jpg b/images/patterns/cola.jpg new file mode 100644 index 0000000000000000000000000000000000000000..2faed5d5235fbbe96b85c4e62a485cb0f2e29f98 Binary files /dev/null and b/images/patterns/cola.jpg differ diff --git a/images/patterns/flag.jpg b/images/patterns/flag.jpg new file mode 100644 index 0000000000000000000000000000000000000000..667a7c18679512186570155a409d3ddc3ca31c9f Binary files /dev/null and b/images/patterns/flag.jpg differ diff --git a/images/patterns/mona.jpg b/images/patterns/mona.jpg new file mode 100644 index 0000000000000000000000000000000000000000..3f440d7f24af41efd3ca47c987c41d6427f92ec0 Binary files /dev/null and b/images/patterns/mona.jpg differ diff --git a/images/patterns/sign.jpg b/images/patterns/sign.jpg new file mode 100644 index 0000000000000000000000000000000000000000..89efc8f9a36d0a1939d93f5b3402191e532168e0 Binary files /dev/null and b/images/patterns/sign.jpg differ diff --git a/images/patterns/supa.png b/images/patterns/supa.png new file mode 100644 index 0000000000000000000000000000000000000000..5d8552f45c2476d8c82aa8feb0740925d66e877e Binary files /dev/null and b/images/patterns/supa.png differ diff --git a/images/patterns/turm.jpg b/images/patterns/turm.jpg new file mode 100644 index 0000000000000000000000000000000000000000..5ccbfe36075278aa3dfe40fd32d6af3739582212 Binary files /dev/null and b/images/patterns/turm.jpg differ diff --git a/images/patterns/work.jpg b/images/patterns/work.jpg new file mode 100644 index 0000000000000000000000000000000000000000..1fa2cb12959351de9649814a0193c1312d938dd1 Binary files /dev/null and b/images/patterns/work.jpg differ diff --git a/qgroundcontrol.pri b/qgroundcontrol.pri index 2b2e46dc3fef245e67bb6fc6756828ec0070251c..8466718845bec9c5fe9f5df6694b031c97803863 100644 --- a/qgroundcontrol.pri +++ b/qgroundcontrol.pri @@ -23,6 +23,7 @@ # #------------------------------------------------- + #$$BASEDIR/lib/qextserialport/include # $$BASEDIR/lib/openjaus/libjaus/include \ # $$BASEDIR/lib/openjaus/libopenJaus/include @@ -40,9 +41,9 @@ QMAKE_POST_LINK += echo "Copying files" #QMAKE_POST_LINK += && cp -rf $$BASEDIR/models $$TARGETDIR/release/. # MAC OS X -macx { +macx { - COMPILER_VERSION = system(gcc -v) + COMPILER_VERSION = $$system(gcc -v) message(Using compiler $$COMPILER_VERSION) HARDWARE_PLATFORM = $$system(uname -a) @@ -73,7 +74,7 @@ macx { -framework CoreFoundation \ -framework ApplicationServices \ -lm - + ICON = $$BASEDIR/images/icons/macx.icns # Copy audio files if needed @@ -153,12 +154,13 @@ linux-g++ { release { DESTDIR = $$TARGETDIR/release + DEFINES += QT_NO_DEBUG } QMAKE_POST_LINK += cp -rf $$BASEDIR/audio $$DESTDIR/. INCLUDEPATH += /usr/include \ - /usr/local/include \ + /usr/local/include \ /usr/include/qt4/phonon # $$BASEDIR/lib/flite/include \ # $$BASEDIR/lib/flite/lang @@ -222,6 +224,7 @@ linux-g++-64 { release { DESTDIR = $$TARGETDIR/release + DEFINES += QT_NO_DEBUG } QMAKE_POST_LINK += cp -rf $$BASEDIR/audio $$DESTDIR/. @@ -289,6 +292,14 @@ win32-msvc2008 { CONFIG += qaxcontainer + # QWebkit is not needed on MS-Windows compilation environment + CONFIG -= webkit + + release { + CONFIG -= console + DEFINES += QT_NO_DEBUG + } + # Special settings for debug #CONFIG += CONSOLE @@ -310,9 +321,9 @@ INCLUDEPATH += $$BASEDIR/lib/osgEarth/win32/include \ LIBS += -L$$BASEDIR/lib/osgEarth_3rdparty/win32/OpenSceneGraph-2.8.2/lib \ -losg \ -losgViewer \ - -losgGA \ - -losgDB \ - -losgText \ + -losgGA \ + -losgDB \ + -losgText \ -lOpenThreads DEFINES += QGC_OSG_ENABLED exists($$BASEDIR/lib/osgEarth123) { @@ -331,18 +342,18 @@ exists($$BASEDIR/lib/osgEarth123) { BASEDIR_WIN = $$replace(BASEDIR,"/","\\") TARGETDIR_WIN = $$replace(TARGETDIR,"/","\\") - debug { + exists($$TARGETDIR/debug) { QMAKE_POST_LINK += && copy /Y \"$$BASEDIR_WIN\\lib\\sdl\\win32\\SDL.dll\" \"$$TARGETDIR_WIN\\debug\\SDL.dll\" - QMAKE_POST_LINK += && xcopy \"$$BASEDIR_WIN\audio\" \"$$TARGETDIR_WIN\debug\audio\" /S /E /Y - QMAKE_POST_LINK += && xcopy \"$$BASEDIR_WIN\models\" \"$$TARGETDIR_WIN\debug\models\" /S /E /Y - QMAKE_POST_LINK += && copy /Y \"$$BASEDIR/images/earth.html $$TARGETDIR_WIN\debug\" + QMAKE_POST_LINK += && xcopy \"$$BASEDIR_WIN\\audio\" \"$$TARGETDIR_WIN\\debug\\audio\\\" /S /E /Y + QMAKE_POST_LINK += && xcopy \"$$BASEDIR_WIN\\models\" \"$$TARGETDIR_WIN\\debug\\models\\\" /S /E /Y + QMAKE_POST_LINK += && copy /Y \"$$BASEDIR_WIN\\images\\earth.html\" \"$$TARGETDIR_WIN\\debug\\earth.html\" } - release { - QMAKE_POST_LINK += && copy /Y \"$$BASEDIR_WIN\lib\sdl\win32\SDL.dll\" \"$$TARGETDIR_WIN\release\SDL.dll\" - QMAKE_POST_LINK += && xcopy \"$$BASEDIR_WIN\audio\" \"$$TARGETDIR_WIN\release\audio\" /S /E /Y - QMAKE_POST_LINK += && xcopy \"$$BASEDIR_WIN\models\" \"$$TARGETDIR_WIN\release\models\" /S /E /Y - QMAKE_POST_LINK += && copy /Y \"$$BASEDIR/images/earth.html $$TARGETDIR_WIN\release\" + exists($$TARGETDIR/release) { + QMAKE_POST_LINK += && copy /Y \"$$BASEDIR_WIN\\lib\\sdl\\win32\\SDL.dll\" \"$$TARGETDIR_WIN\\release\\SDL.dll\" + QMAKE_POST_LINK += && xcopy \"$$BASEDIR_WIN\\audio\" \"$$TARGETDIR_WIN\\release\\audio\\\" /S /E /Y + QMAKE_POST_LINK += && xcopy \"$$BASEDIR_WIN\\models\" \"$$TARGETDIR_WIN\\release\\models\\\" /S /E /Y + QMAKE_POST_LINK += && copy /Y \"$$BASEDIR_WIN\\images\\earth.html\" \"$$TARGETDIR_WIN\\release\\earth.html\" } } @@ -351,7 +362,7 @@ exists($$BASEDIR/lib/osgEarth123) { win32-g++ { message(Building for Windows Platform (32bit)) - + # Special settings for debug #CONFIG += CONSOLE @@ -362,6 +373,8 @@ win32-g++ { LIBS += -L$$BASEDIR/lib/sdl/win32 \ -lmingw32 -lSDLmain -lSDL -mwindows + CONFIG += windows + debug { @@ -369,13 +382,18 @@ win32-g++ { } release { + CONFIG -= console + DEFINES += QT_NO_DEBUG #DESTDIR = $$BUILDDIR/release } - + RC_FILE = $$BASEDIR/qgroundcontrol.rc # Copy dependencies + system(cp): { + # CP command is available, use it instead of copy / xcopy + message("Using cp to copy image and audio files to executable") debug { QMAKE_POST_LINK += && cp $$BASEDIR/lib/sdl/win32/SDL.dll $$TARGETDIR/debug/SDL.dll QMAKE_POST_LINK += && cp -r $$BASEDIR/audio $$TARGETDIR/debug/audio @@ -388,6 +406,28 @@ win32-g++ { QMAKE_POST_LINK += && cp -r $$BASEDIR/models $$TARGETDIR/release/models } + } else { + # No cp command available, go for copy / xcopy + # Copy dependencies + BASEDIR_WIN = $$replace(BASEDIR,"/","\\") + TARGETDIR_WIN = $$replace(TARGETDIR,"/","\\") + + exists($$TARGETDIR/debug) { + QMAKE_POST_LINK += && copy /Y \"$$BASEDIR_WIN\\lib\\sdl\\win32\\SDL.dll\" \"$$TARGETDIR_WIN\\debug\\SDL.dll\" + QMAKE_POST_LINK += && xcopy \"$$BASEDIR_WIN\\audio\" \"$$TARGETDIR_WIN\\debug\\audio\\\" /S /E /Y + QMAKE_POST_LINK += && xcopy \"$$BASEDIR_WIN\\models\" \"$$TARGETDIR_WIN\\debug\\models\\\" /S /E /Y + QMAKE_POST_LINK += && copy /Y \"$$BASEDIR_WIN\\images\\earth.html\" \"$$TARGETDIR_WIN\\debug\\earth.html\" + } + + exists($$TARGETDIR/release) { + QMAKE_POST_LINK += && copy /Y \"$$BASEDIR_WIN\\lib\\sdl\\win32\\SDL.dll\" \"$$TARGETDIR_WIN\\release\\SDL.dll\" + QMAKE_POST_LINK += && xcopy \"$$BASEDIR_WIN\\audio\" \"$$TARGETDIR_WIN\\release\\audio\\\" /S /E /Y + QMAKE_POST_LINK += && xcopy \"$$BASEDIR_WIN\\models\" \"$$TARGETDIR_WIN\\release\\models\\\" /S /E /Y + QMAKE_POST_LINK += && copy /Y \"$$BASEDIR_WIN\\images\\earth.html\" \"$$TARGETDIR_WIN\\release\\earth.html\" + } + +} + # osg/osgEarth dynamic casts might fail without this compiler option. # see http://osgearth.org/wiki/FAQ for details. QMAKE_CXXFLAGS += -Wl,-E diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index d526a260786c7e5630279d8ecf31dd2d196de17e..eadd028c0d79f9b5d678f5f7621b9e993b4faae3 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -150,7 +150,8 @@ FORMS += src/ui/MainWindow.ui \ src/ui/SlugsHilSim.ui \ src/ui/SlugsPIDControl.ui \ src/ui/SlugsVideoCamControl.ui \ - src/ui/SlugsPadCameraControl.ui + src/ui/SlugsPadCameraControl.ui \ + src/ui/uas/QGCUnconnectedInfoWidget.ui INCLUDEPATH += src \ src/ui \ @@ -244,13 +245,19 @@ HEADERS += src/MG.h \ src/comm/QGCMAVLink.h \ src/ui/QGCWebView.h \ src/ui/map3D/QGCWebPage.h \ - src/ui/map3D/QGCGoogleEarthView.h\ src/ui/SlugsDataSensorView.h \ src/ui/SlugsHilSim.h \ src/ui/SlugsPIDControl.h \ src/ui/SlugsVideoCamControl.h \ src/ui/SlugsPadCameraControl.h \ - src/ui/QGCMainWindowAPConfigurator.h + src/ui/QGCMainWindowAPConfigurator.h \ + src/comm/MAVLinkSwarmSimulationLink.h \ + src/ui/uas/QGCUnconnectedInfoWidget.h + +# Google Earth is only supported on Mac OS and Windows with Visual Studio Compiler +macx|win32-msvc2008: { + HEADERS += src/ui/map3D/QGCGoogleEarthView.h +} contains(DEPENDENCIES_PRESENT, osg) { message("Including headers for OpenSceneGraph") @@ -354,14 +361,20 @@ SOURCES += src/main.cc \ src/ui/RadioCalibration/AbstractCalibrator.cc \ src/ui/RadioCalibration/RadioCalibrationData.cc \ src/ui/QGCWebView.cc \ - src/ui/map3D/QGCGoogleEarthView.cc \ src/ui/map3D/QGCWebPage.cc \ src/ui/SlugsDataSensorView.cc \ src/ui/SlugsHilSim.cc \ src/ui/SlugsPIDControl.cpp \ src/ui/SlugsVideoCamControl.cpp \ src/ui/SlugsPadCameraControl.cpp \ - src/ui/QGCMainWindowAPConfigurator.cc + src/ui/QGCMainWindowAPConfigurator.cc \ + src/comm/MAVLinkSwarmSimulationLink.cc \ + src/ui/uas/QGCUnconnectedInfoWidget.cc + +macx|win32-msvc2008: { + SOURCES += src/ui/map3D/QGCGoogleEarthView.cc +} + contains(DEPENDENCIES_PRESENT, osg) { message("Including sources for OpenSceneGraph") diff --git a/src/Core.cc b/src/Core.cc index e49a06c53cd57ec73c143533a3e849f4acff607d..2beaf58764a4d5b5ba6ef97789db9a19e0434ea3 100644 --- a/src/Core.cc +++ b/src/Core.cc @@ -66,7 +66,7 @@ Core::Core(int &argc, char* argv[]) : QApplication(argc, argv) { this->setApplicationName(QGC_APPLICATION_NAME); this->setApplicationVersion(QGC_APPLICATION_VERSION); - this->setOrganizationName(QLatin1String("PIXHAWK Association Zurich")); + this->setOrganizationName(QLatin1String("OPENMAV")); this->setOrganizationDomain("http://qgroundcontrol.org"); // Show splash screen @@ -105,10 +105,6 @@ Core::Core(int &argc, char* argv[]) : QApplication(argc, argv) // Start the user interface splashScreen->showMessage(tr("Starting User Interface"), Qt::AlignLeft | Qt::AlignBottom, QColor(62, 93, 141)); // Start UI - mainWindow = new MainWindow(); - - // Remove splash screen - splashScreen->finish(mainWindow); // Connect links // to make sure that all components are initialized when the @@ -117,7 +113,21 @@ Core::Core(int &argc, char* argv[]) : QApplication(argc, argv) // Listen on Multicast-Address 239.255.77.77, Port 14550 //QHostAddress * multicast_udp = new QHostAddress("239.255.77.77"); //UDPLink* udpLink = new UDPLink(*multicast_udp, 14550); - mainWindow->addLink(udpLink); + //mainWindow->addLink(udpLink); + +#ifdef OPAL_RT + // Add OpalRT Link, but do not connect + OpalLink* opalLink = new OpalLink(); + //mainWindow->addLink(opalLink); +#endif + // MAVLinkSimulationLink* simulationLink = new MAVLinkSimulationLink(MG::DIR::getSupportFilesDirectory() + "/demo-log.txt"); + MAVLinkSimulationLink* simulationLink = new MAVLinkSimulationLink(":/demo-log.txt"); + //mainWindow->addLink(simulationLink); + + mainWindow = MainWindow::instance(); + + // Remove splash screen + splashScreen->finish(mainWindow); // Check if link could be connected if (!udpLink->connect()) @@ -140,15 +150,6 @@ Core::Core(int &argc, char* argv[]) : QApplication(argc, argv) QTimer::singleShot(200, mainWindow, SLOT(close())); } } - -#ifdef OPAL_RT - // Add OpalRT Link, but do not connect - OpalLink* opalLink = new OpalLink(); - mainWindow->addLink(opalLink); -#endif - // MAVLinkSimulationLink* simulationLink = new MAVLinkSimulationLink(MG::DIR::getSupportFilesDirectory() + "/demo-log.txt"); - MAVLinkSimulationLink* simulationLink = new MAVLinkSimulationLink(":/demo-log.txt"); - mainWindow->addLink(simulationLink); } /** diff --git a/src/QGC.h b/src/QGC.h index 4751cdda47404bea2c0d454f84ad560f51984ea7..c4e4fed4b4c0b33d2dd61fd6cde677b51bee80bc 100644 --- a/src/QGC.h +++ b/src/QGC.h @@ -19,4 +19,6 @@ namespace QGC const QString COMPANYNAME = "OPENMAV"; } +#define QGC_EVENTLOOP_DEBUG 0 + #endif // QGC_H diff --git a/src/comm/LinkManager.cc b/src/comm/LinkManager.cc index 802cf1741f7642efe92cfea223405ce301f7f88d..c2cf73b94c9f5ac82a391f8cebdea6beb7c1d3fa 100644 --- a/src/comm/LinkManager.cc +++ b/src/comm/LinkManager.cc @@ -67,6 +67,7 @@ LinkManager::~LinkManager() void LinkManager::add(LinkInterface* link) { if(!link) return; + connect(link, SIGNAL(destroyed(QObject*)), this, SLOT(removeLink(QObject*))); links.append(link); emit newLink(link); } @@ -127,6 +128,17 @@ bool LinkManager::disconnectLink(LinkInterface* link) return link->disconnect(); } +void LinkManager::removeLink(QObject* link) +{ + LinkInterface* linkInterface = dynamic_cast(link); + // Add link to link list + if (links.contains(linkInterface)) + { + int linkIndex = links.indexOf(linkInterface); + links.removeAt(linkIndex); + } +} + bool LinkManager::removeLink(LinkInterface* link) { if(link) diff --git a/src/comm/LinkManager.h b/src/comm/LinkManager.h index 8d283c3de0d3f1273bf41afdabdccd8724812d7a..36ef64ceea4071d7e32f8c17e52e98e31bab2f58 100644 --- a/src/comm/LinkManager.h +++ b/src/comm/LinkManager.h @@ -67,6 +67,7 @@ public slots: void add(LinkInterface* link); void addProtocol(LinkInterface* link, ProtocolInterface* protocol); + void removeLink(QObject* link); bool removeLink(LinkInterface* link); bool connectAll(); diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc index f35a129467e2f05efbd0efbb0080ff234f6c49aa..ed67251bfed6263c07566ba55c8a4511ad77bac7 100644 --- a/src/comm/MAVLinkSimulationLink.cc +++ b/src/comm/MAVLinkSimulationLink.cc @@ -192,6 +192,7 @@ void MAVLinkSimulationLink::mainloop() static unsigned int rate1hzCounter = 1; static unsigned int rate10hzCounter = 1; static unsigned int rate50hzCounter = 1; + static unsigned int circleCounter = 0; // Vary values @@ -374,17 +375,20 @@ void MAVLinkSimulationLink::mainloop() // Move X Position - x = x*0.93f + 0.07f*(x+sin(static_cast(QGC::groundTimeUsecs()) * 0.08f)); - y = y*0.93f + 0.07f*(y+sin(static_cast(QGC::groundTimeUsecs()) * 0.5f)); - z = z*0.93f + 0.07f*(z+sin(static_cast(QGC::groundTimeUsecs()*0.001f)) * 0.1f); + x = 8.0*sin((double)circleCounter/50.0); + y = 3.0*cos((double)circleCounter/40.0); + z = 1.8 + 1.2*sin((double)circleCounter/60.0); - x = (x > 5.0f) ? 5.0f : x; - y = (y > 5.0f) ? 5.0f : y; - z = (z > 3.0f) ? 3.0f : z; + circleCounter++; - x = (x < -5.0f) ? -5.0f : x; - y = (y < -5.0f) ? -5.0f : y; - z = (z < -3.0f) ? -3.0f : z; + +// x = (x > 5.0f) ? 5.0f : x; +// y = (y > 5.0f) ? 5.0f : y; +// z = (z > 3.0f) ? 3.0f : z; + +// x = (x < -5.0f) ? -5.0f : x; +// y = (y < -5.0f) ? -5.0f : y; +// z = (z < -3.0f) ? -3.0f : z; // Send back new setpoint mavlink_message_t ret; @@ -401,15 +405,15 @@ void MAVLinkSimulationLink::mainloop() memcpy(stream+streampointer,buffer, bufferlength); streampointer += bufferlength; - // GPS RAW - mavlink_msg_gps_raw_pack(systemId, componentId, &ret, 0, 3, 47.376417+(x*0.00001), 8.548103+(y*0.00001), z, 0, 0, 2.5f, 0.1f); - bufferlength = mavlink_msg_to_send_buffer(buffer, &ret); - //add data into datastream - memcpy(stream+streampointer,buffer, bufferlength); - streampointer += bufferlength; +// // GPS RAW +// mavlink_msg_gps_raw_pack(systemId, componentId, &ret, 0, 3, 47.376417+(x*0.00001), 8.548103+(y*0.00001), z, 0, 0, 2.5f, 0.1f); +// bufferlength = mavlink_msg_to_send_buffer(buffer, &ret); +// //add data into datastream +// memcpy(stream+streampointer,buffer, bufferlength); +// streampointer += bufferlength; // GLOBAL POSITION - mavlink_msg_global_position_pack(systemId, componentId, &ret, 0, 47.378028137103+(x*0.00001), 8.54899892510421+(y*0.00001), z, 0, 0, 0); + mavlink_msg_global_position_pack(systemId, componentId, &ret, 0, 47.378028137103+(x*0.001), 8.54899892510421+(y*0.001), z+25.0, 0, 0, 0); bufferlength = mavlink_msg_to_send_buffer(buffer, &ret); //add data into datastream memcpy(stream+streampointer,buffer, bufferlength); @@ -508,6 +512,7 @@ void MAVLinkSimulationLink::mainloop() detectionCounter++; status.vbat = voltage * 1000; // millivolts + status.load = 33 * detectionCounter % 1000; // Pack message and get size of encoded byte string messageSize = mavlink_msg_sys_status_encode(systemId, componentId, &msg, &status); @@ -520,7 +525,7 @@ void MAVLinkSimulationLink::mainloop() // Pack debug text message mavlink_statustext_t text; text.severity = 0; - strcpy((char*)(text.text), "DEBUG MESSAGE TEXT"); + strcpy((char*)(text.text), "Text message from system 32"); mavlink_msg_statustext_encode(systemId, componentId, &msg, &text); bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); memcpy(stream+streampointer, buffer, bufferlength); @@ -578,11 +583,11 @@ void MAVLinkSimulationLink::mainloop() streampointer += bufferlength; - /* + // HEARTBEAT VEHICLE 2 // Pack message and get size of encoded byte string - messageSize = mavlink_msg_heartbeat_pack(42, componentId, &msg, MAV_FIXED_WING); + messageSize = mavlink_msg_heartbeat_pack(54, componentId, &msg, MAV_FIXED_WING, MAV_AUTOPILOT_ARDUPILOTMEGA); // Allocate buffer with packet data bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); //add data into datastream @@ -590,25 +595,19 @@ void MAVLinkSimulationLink::mainloop() streampointer += bufferlength; // STATUS VEHICLE 2 - sys_status_t status2; + mavlink_sys_status_t status2; status2.mode = MAV_MODE_LOCKED; status2.vbat = voltage; + status2.load = 120; status2.status = MAV_STATE_STANDBY; // Pack message and get size of encoded byte string - messageSize = mavlink_msg_sys_status_encode(systemId, componentId, &msg, &status); + messageSize = mavlink_msg_sys_status_encode(54, componentId, &msg, &status); // Allocate buffer with packet data bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); //add data into datastream memcpy(stream+streampointer,buffer, bufferlength); streampointer += bufferlength; - */ - //qDebug() << "BOOT" << "BUF LEN" << bufferlength << "POINTER" << streampointer; - - // AUX STATUS - #ifdef MAVLINK_ENABLED_PIXHAWK - rawAuxValues.vbat = voltage; -#endif rate1hzCounter = 1; } @@ -780,31 +779,6 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size) j++; } - /* - // Pack message and get size of encoded byte string - mavlink_msg_param_value_pack(systemId, componentId, &msg, (int8_t*)"ROLL_K_P", 0.5f); - // Allocate buffer with packet data - bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); - //add data into datastream - memcpy(stream+streampointer,buffer, bufferlength); - streampointer+=bufferlength; - - // Pack message and get size of encoded byte string - mavlink_msg_param_value_pack(systemId, componentId, &msg, (int8_t*)"PITCH_K_P", 0.6f); - // Allocate buffer with packet data - bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); - //add data into datastream - memcpy(stream+streampointer,buffer, bufferlength); - streampointer+=bufferlength; - - // Pack message and get size of encoded byte string - mavlink_msg_param_value_pack(systemId, componentId, &msg, (int8_t*)"YAW_K_P", 0.8f); - // Allocate buffer with packet data - bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); - //add data into datastream - memcpy(stream+streampointer,buffer, bufferlength); - streampointer+=bufferlength;*/ - qDebug() << "SIMULATION SENT PARAMETERS TO GCS"; } } @@ -837,7 +811,7 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size) } - unsigned char v=data[i]; + //unsigned char v=data[i]; //fprintf(stderr,"%02x ", v); } //fprintf(stderr,"\n"); @@ -927,6 +901,18 @@ bool MAVLinkSimulationLink::connect() return true; } +/** + * Connect the link. + * + * @param connect true connects the link, false disconnects it + * @return True if connection has been established, false if connection + * couldn't be established. + **/ +void MAVLinkSimulationLink::connectLink() +{ + this->connect(); +} + /** * Connect the link. * diff --git a/src/comm/MAVLinkSimulationLink.h b/src/comm/MAVLinkSimulationLink.h index 49cc4d5c028bd36b3a9432a86fc43cb3b719c96c..25ad3ecb1b8977084909330b60776873a543310d 100644 --- a/src/comm/MAVLinkSimulationLink.h +++ b/src/comm/MAVLinkSimulationLink.h @@ -85,16 +85,17 @@ public: public slots: void writeBytes(const char* data, qint64 size); void readBytes(); - void mainloop(); + virtual void mainloop(); bool connectLink(bool connect); + void connectLink(); protected: // UAS properties float roll, pitch, yaw; - float x, y, z; - float spX, spY, spZ, spYaw; + double x, y, z; + double spX, spY, spZ, spYaw; int battery; QTimer* timer; diff --git a/src/comm/MAVLinkSwarmSimulationLink.cc b/src/comm/MAVLinkSwarmSimulationLink.cc new file mode 100644 index 0000000000000000000000000000000000000000..c94c52896a7cd7a98e3b3d641b0a56e7a2726840 --- /dev/null +++ b/src/comm/MAVLinkSwarmSimulationLink.cc @@ -0,0 +1,12 @@ +#include "MAVLinkSwarmSimulationLink.h" + +MAVLinkSwarmSimulationLink::MAVLinkSwarmSimulationLink(QObject *parent) : + MAVLinkSimulationLink() +{ +} + + +void MAVLinkSwarmSimulationLink::mainloop() +{ + +} diff --git a/src/comm/MAVLinkSwarmSimulationLink.h b/src/comm/MAVLinkSwarmSimulationLink.h new file mode 100644 index 0000000000000000000000000000000000000000..d4b4996d0f9d77fac7d8252dea05a4b356657e5f --- /dev/null +++ b/src/comm/MAVLinkSwarmSimulationLink.h @@ -0,0 +1,19 @@ +#ifndef MAVLINKSWARMSIMULATIONLINK_H +#define MAVLINKSWARMSIMULATIONLINK_H + +#include "MAVLinkSimulationLink.h" + +class MAVLinkSwarmSimulationLink : public MAVLinkSimulationLink +{ + Q_OBJECT +public: + explicit MAVLinkSwarmSimulationLink(QObject *parent = 0); + +signals: + +public slots: + void mainloop(); + +}; + +#endif // MAVLINKSWARMSIMULATIONLINK_H diff --git a/src/comm/SerialLink.cc b/src/comm/SerialLink.cc index 404a8dbfc8049ee7c56afa7191c207c40e58a731..236493000159d1368b71a6d825ca9d8509c46f80 100644 --- a/src/comm/SerialLink.cc +++ b/src/comm/SerialLink.cc @@ -184,18 +184,18 @@ void SerialLink::writeBytes(const char* data, qint64 size) if(port->isOpen()) { int b = port->write(data, size); - qDebug() << "Transmitted " << b << "bytes:"; + qDebug() << "Serial link " << this->getName() << "transmitted" << b << "bytes:"; // Increase write counter bitsSentTotal += size * 8; - int i; - for (i=0; i @@ -10,15 +9,15 @@ This file is part of the QGROUNDCONTROL project it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. - + QGROUNDCONTROL is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. - + You should have received a copy of the GNU General Public License along with QGROUNDCONTROL. If not, see . - + ======================================================================*/ /** @@ -47,17 +46,20 @@ void ArduPilotMegaMAV::receiveMessage(LinkInterface* link, mavlink_message_t mes { // Let UAS handle the default message set UAS::receiveMessage(link, message); - - // Handle your special messages - switch (message.msgid) + + if (message.sysid == uasId) { - case MAVLINK_MSG_ID_HEARTBEAT: + // Handle your special messages + switch (message.msgid) { - qDebug() << "ARDUPILOT RECEIVED HEARTBEAT"; + case MAVLINK_MSG_ID_HEARTBEAT: + { + qDebug() << "ARDUPILOT RECEIVED HEARTBEAT"; + break; + } + default: + qDebug() << "\nARDUPILOT RECEIVED MESSAGE WITH ID" << message.msgid; break; } - default: - qDebug() << "\nARDUPILOT RECEIVED MESSAGE WITH ID" << message.msgid; - break; } } diff --git a/src/uas/SlugsMAV.cc b/src/uas/SlugsMAV.cc index 64fda12cf11c4ae4fc63279313dfbeb8b34234b5..176558c172bb2c6d1c20fb04c925fb37b6ec981e 100644 --- a/src/uas/SlugsMAV.cc +++ b/src/uas/SlugsMAV.cc @@ -62,7 +62,8 @@ void SlugsMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) // Let UAS handle the default message set UAS::receiveMessage(link, message); - + if (message.sysid == uasId) + { // Handle your special messages mavlink_message_t* msg = &message; switch (message.msgid) { @@ -136,7 +137,7 @@ void SlugsMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) case MAVLINK_MSG_ID_CTRL_SRFC_PT: //181 - break; + break; case MAVLINK_MSG_ID_PID: //182 memset(&mlSinglePid,0,sizeof(mavlink_pid_t)); @@ -153,9 +154,10 @@ void SlugsMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) #endif default: -// qDebug() << "\nSLUGS RECEIVED MESSAGE WITH ID" << message.msgid; + // qDebug() << "\nSLUGS RECEIVED MESSAGE WITH ID" << message.msgid; break; } + } } @@ -176,9 +178,9 @@ void SlugsMAV::emitSignals (void){ case 3: emit remoteControlChannelScaledChanged(0,(mlPilotConsoleData.de- 1000.0f)/1000.0f); - 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(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 slugsPWM(uasId, mlPwmCommands); break; diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 95d941a07d6af8c8f1961e78e565d41109947291..87650a671aec587dc2df36047c484c9dc73e8b6e 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: @@ -793,7 +797,7 @@ quint64 UAS::getUnixTime(quint64 time) // 60 seconds // 1000 milliseconds // 1000 microseconds -#ifndef _MSVC_VER +#ifndef _MSC_VER else if (time < 1261440000000000LLU) #else else if (time < 1261440000000000) @@ -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 65b514b239ded2832971f29fd30e0615e95610ec..5a781ccfe887e855074c3a4008b3eb626e1ec1ab 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -211,13 +211,16 @@ public slots: void addLink(LinkInterface* link); /** @brief Receive a message from one of the communication links. */ - void receiveMessage(LinkInterface* link, mavlink_message_t message); + virtual void receiveMessage(LinkInterface* link, mavlink_message_t message); /** @brief Send a message over this link (to this or to all UAS on this link) */ void sendMessage(LinkInterface* link, mavlink_message_t message); /** @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(); diff --git a/src/uas/UASManager.cc b/src/uas/UASManager.cc index fabfec6ba1072d55fbf5101a2a710114d2631710..cb3183cb97f9008f47dddd1bce1e4960ba98b05d 100644 --- a/src/uas/UASManager.cc +++ b/src/uas/UASManager.cc @@ -71,6 +71,18 @@ void UASManager::run() void UASManager::addUAS(UASInterface* uas) { + // WARNING: The active uas is set here + // and then announced below. This is necessary + // to make sure the getActiveUAS() function + // returns the UAS once the UASCreated() signal + // is emitted. The code is thus NOT redundant. + bool firstUAS = false; + if (activeUAS == NULL) + { + firstUAS = true; + activeUAS = uas; + } + // Only execute if there is no UAS at this index if (!systems.contains(uas->getUASID())) { @@ -79,7 +91,7 @@ void UASManager::addUAS(UASInterface* uas) } // If there is no active UAS yet, set the first one as the active UAS - if (activeUAS == NULL) + if (firstUAS) { setActiveUAS(uas); } diff --git a/src/ui/DebugConsole.cc b/src/ui/DebugConsole.cc index d91de4a9ecf47762afaa758e9d5fb84ea0cde28d..36629e67db572a008d6b646bf14a06c61928f9d9 100644 --- a/src/ui/DebugConsole.cc +++ b/src/ui/DebugConsole.cc @@ -125,12 +125,28 @@ void DebugConsole::addLink(LinkInterface* link) // Register for name changes connect(link, SIGNAL(nameChanged(QString)), this, SLOT(updateLinkName(QString))); + connect(link, SIGNAL(destroyed(QObject*)), this, SLOT(removeLink(QObject*))); +} + +void DebugConsole::removeLink(QObject* link) +{ + LinkInterface* linkInterface = dynamic_cast(link); + // Add link to link list + if (links.contains(linkInterface)) + { + int linkIndex = links.indexOf(linkInterface); + + links.removeAt(linkIndex); + + m_ui->linkComboBox->removeItem(linkIndex); + } + if (link == currLink) currLink = NULL; } void DebugConsole::linkSelected(int linkId) { // Disconnect - if (currLink != NULL) + if (currLink) { disconnect(currLink, SIGNAL(bytesReceived(LinkInterface*,QByteArray)), this, SLOT(receiveBytes(LinkInterface*, QByteArray))); } @@ -205,30 +221,30 @@ void DebugConsole::paintEvent(QPaintEvent *event) { Q_UNUSED(event); // Update bandwidth - if (holdOn) - { - //qDebug() << "Data rate:" << dataRate/1000.0f << "kB/s"; - QString rate("data rate: %1"); - rate.arg(dataRate); - QPainter painter(this); - painter.setRenderHint(QPainter::HighQualityAntialiasing); - painter.translate(width()/5.0f, height()/5.0f); - - - - //QFont font("Bitstream Vera Sans"); - QFont font = painter.font(); - font.setPixelSize((int)(60.0f)); - - QFontMetrics metrics = QFontMetrics(font); - int border = qMax(4, metrics.leading()); - QRect rect = metrics.boundingRect(0, 0, width() - 2*border, int(height()*0.125), - Qt::AlignLeft | Qt::TextWordWrap, rate); - painter.setPen(QColor(255, 50, 50)); - painter.setRenderHint(QPainter::TextAntialiasing); - painter.drawText(QRect(QPoint(static_cast(width()/5.0f), static_cast(height()/5.0f)), QPoint(static_cast(width() - width()/5.0f), static_cast(height() - height()/5.0f))), rate); - //Qt::AlignRight | Qt::TextWordWrap - } +// if (holdOn) +// { +// //qDebug() << "Data rate:" << dataRate/1000.0f << "kB/s"; +// QString rate("data rate: %1"); +// rate.arg(dataRate); +// QPainter painter(this); +// painter.setRenderHint(QPainter::HighQualityAntialiasing); +// painter.translate(width()/5.0f, height()/5.0f); + + + +// //QFont font("Bitstream Vera Sans"); +// QFont font = painter.font(); +// font.setPixelSize((int)(60.0f)); + +// QFontMetrics metrics = QFontMetrics(font); +// int border = qMax(4, metrics.leading()); +// QRect rect = metrics.boundingRect(0, 0, width() - 2*border, int(height()*0.125), +// Qt::AlignLeft | Qt::TextWordWrap, rate); +// painter.setPen(QColor(255, 50, 50)); +// painter.setRenderHint(QPainter::TextAntialiasing); +// painter.drawText(QRect(QPoint(static_cast(width()/5.0f), static_cast(height()/5.0f)), QPoint(static_cast(width() - width()/5.0f), static_cast(height() - height()/5.0f))), rate); +// //Qt::AlignRight | Qt::TextWordWrap +// } } void DebugConsole::receiveBytes(LinkInterface* link, QByteArray bytes) diff --git a/src/ui/DebugConsole.h b/src/ui/DebugConsole.h index ffc97c45b7dce72ddb960f438473f7ad76d7d9cf..b68fa7c114358444da1345f23765afe54d992169 100644 --- a/src/ui/DebugConsole.h +++ b/src/ui/DebugConsole.h @@ -59,6 +59,8 @@ public: public slots: /** @brief Add a link to the list of monitored links */ void addLink(LinkInterface* link); + /** @brief Remove a link from the list */ + void removeLink(QObject* link); /** @brief Update a link name */ void updateLinkName(QString name); /** @brief Select a link for the active view */ diff --git a/src/ui/HDDisplay.cc b/src/ui/HDDisplay.cc index 1f8f11208dc81f470b2dd070a03e365e6f162be4..1192494028880a965092f5a44ac8942f679eeae5 100644 --- a/src/ui/HDDisplay.cc +++ b/src/ui/HDDisplay.cc @@ -38,6 +38,7 @@ This file is part of the PIXHAWK project #include "HDDisplay.h" #include "ui_HDDisplay.h" #include "MG.h" +#include "QGC.h" #ifndef M_PI #define M_PI 3.14159265358979323846 #endif @@ -134,7 +135,7 @@ HDDisplay::HDDisplay(QStringList* plotList, QWidget *parent) : if(!QFile::exists(fontFileName)) qDebug() << "ERROR! font file: " << fontFileName << " DOES NOT EXIST!"; fontDatabase.addApplicationFont(fontFileName); - font = fontDatabase.font(fontFamilyName, "Roman", (int)(10*scalingFactor*1.2f+0.5f)); + font = fontDatabase.font(fontFamilyName, "Roman", qMax(5, (int)(10*scalingFactor*1.2f+0.5f))); if (font.family() != fontFamilyName) qDebug() << "ERROR! Font not loaded: " << fontFamilyName; // Connect with UAS @@ -151,7 +152,7 @@ HDDisplay::~HDDisplay() void HDDisplay::enableGLRendering(bool enable) { - + Q_UNUSED(enable) } void HDDisplay::triggerUpdate() @@ -171,6 +172,9 @@ void HDDisplay::paintEvent(QPaintEvent * event) void HDDisplay::renderOverlay() { +#if (QGC_EVENTLOOP_DEBUG) + qDebug() << "EVENTLOOP:" << __FILE__ << __LINE__; +#endif quint64 refreshInterval = 100; quint64 currTime = MG::TIME::getGroundTimeNow(); if (currTime - lastPaintTime < refreshInterval) @@ -220,16 +224,6 @@ void HDDisplay::renderOverlay() } } -void HDDisplay::start() -{ - refreshTimer->start(); -} - -void HDDisplay::stop() -{ - refreshTimer->stop(); -} - /** * * @param uas the UAS/MAV to monitor/display with the HUD @@ -237,20 +231,17 @@ void HDDisplay::stop() void HDDisplay::setActiveUAS(UASInterface* uas) { //qDebug() << "ATTEMPTING TO SET UAS"; - if (this->uas != NULL && this->uas != uas) + if (this->uas != NULL) { // Disconnect any previously connected active MAV - disconnect(uas, SIGNAL(valueChanged(UASInterface*,QString,double,quint64)), this, SLOT(updateValue(UASInterface*,QString,double,quint64))); + disconnect(this->uas, SIGNAL(valueChanged(UASInterface*,QString,double,quint64)), this, SLOT(updateValue(UASInterface*,QString,double,quint64))); } // Now connect the new UAS - //if (this->uas != uas) - // { //qDebug() << "UAS SET!" << "ID:" << uas->getUASID(); // Setup communication connect(uas, SIGNAL(valueChanged(UASInterface*,QString,double,quint64)), this, SLOT(updateValue(UASInterface*,QString,double,quint64))); - //} this->uas = uas; } @@ -669,7 +660,25 @@ void HDDisplay::changeEvent(QEvent *e) } +void HDDisplay::showEvent(QShowEvent* event) +{ + // React only to internal (pre-display) + // events + Q_UNUSED(event) + { + refreshTimer->start(updateInterval); + } +} +void HDDisplay::hideEvent(QHideEvent* event) +{ + // React only to internal (pre-display) + // events + Q_UNUSED(event) + { + refreshTimer->stop(); + } +} ///** diff --git a/src/ui/HDDisplay.h b/src/ui/HDDisplay.h index 0461c4cc1e85180a535e49430f0e64874456f1c3..35121b560365c233ad47d6693e04f7cc6ffd05ff 100644 --- a/src/ui/HDDisplay.h +++ b/src/ui/HDDisplay.h @@ -64,8 +64,6 @@ public: public slots: /** @brief Update a HDD value */ void updateValue(UASInterface* uas, QString name, double value, quint64 msec); - void start(); - void stop(); void setActiveUAS(UASInterface* uas); protected slots: @@ -77,6 +75,8 @@ protected slots: protected: void changeEvent(QEvent *e); void paintEvent(QPaintEvent * event); + void showEvent(QShowEvent* event); + void hideEvent(QHideEvent* event); float refLineWidthToPen(float line); float refToScreenX(float x); float refToScreenY(float y); @@ -142,6 +142,7 @@ protected: int warningBlinkRate; ///< Blink rate of warning messages, will be rounded to the refresh rate QTimer* refreshTimer; ///< The main timer, controls the update rate + static const int updateInterval = 120; ///< Update interval in milliseconds QPainter* hudPainter; QFont font; ///< The HUD font, per default the free Bitstream Vera SANS, which is very close to actual HUD fonts QFontDatabase fontDatabase;///< Font database, only used to load the TrueType font file (the HUD font is directly loaded from file rather than from the system) diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc index a9a993a02dc1a2a65fbc84cddf1f1594a97df0bb..6b547aced32ae751a8193d127903251051848479 100644 --- a/src/ui/HSIDisplay.cc +++ b/src/ui/HSIDisplay.cc @@ -100,10 +100,10 @@ HSIDisplay::HSIDisplay(QWidget *parent) : topMargin(3.0f) { connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*))); - refreshTimer->setInterval(120); + refreshTimer->setInterval(updateInterval); -// this->setScene(new QGraphicsScene(-metricWidth/2.0f, -metricWidth/2.0f, metricWidth, metricWidth, this)); + // this->setScene(new QGraphicsScene(-metricWidth/2.0f, -metricWidth/2.0f, metricWidth, metricWidth, this)); xCenterPos = vwidth/2.0f; yCenterPos = vheight/2.0f + topMargin - bottomMargin; @@ -138,6 +138,9 @@ void HSIDisplay::paintEvent(QPaintEvent * event) void HSIDisplay::renderOverlay() { +#if (QGC_EVENTLOOP_DEBUG) + qDebug() << "EVENTLOOP:" << __FILE__ << __LINE__; +#endif // Center location of the HSI gauge items float bottomMargin = 3.0f; @@ -432,43 +435,46 @@ void HSIDisplay::setMetricWidth(double width) */ void HSIDisplay::setActiveUAS(UASInterface* uas) { - if (this->uas != NULL && this->uas != uas) - { - // Disconnect any previously connected active MAV - //disconnect(uas, SIGNAL(valueChanged(UASInterface*,QString,double,quint64)), this, SLOT(updateValue(UASInterface*,QString,double,quint64))); - } - - - HDDisplay::setActiveUAS(uas); - //qDebug() << "ATTEMPTING TO SET UAS"; - - - connect(uas, SIGNAL(gpsSatelliteStatusChanged(int,int,float,float,float,bool)), this, SLOT(updateSatellite(int,int,float,float,float,bool))); - connect(uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64))); - connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64))); - connect(uas, SIGNAL(attitudeThrustSetPointChanged(UASInterface*,double,double,double,double,quint64)), this, SLOT(updateAttitudeSetpoints(UASInterface*,double,double,double,double,quint64))); - connect(uas, SIGNAL(positionSetPointsChanged(int,float,float,float,float,quint64)), this, SLOT(updatePositionSetpoints(int,float,float,float,float,quint64))); - connect(uas, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64))); - connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,double,double,double,quint64))); + if (this->uas != NULL) + { + disconnect(this->uas, SIGNAL(gpsSatelliteStatusChanged(int,int,float,float,float,bool)), this, SLOT(updateSatellite(int,int,float,float,float,bool))); + disconnect(this->uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64))); + disconnect(this->uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64))); + disconnect(this->uas, SIGNAL(attitudeThrustSetPointChanged(UASInterface*,double,double,double,double,quint64)), this, SLOT(updateAttitudeSetpoints(UASInterface*,double,double,double,double,quint64))); + disconnect(this->uas, SIGNAL(positionSetPointsChanged(int,float,float,float,float,quint64)), this, SLOT(updatePositionSetpoints(int,float,float,float,float,quint64))); + disconnect(this->uas, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64))); + disconnect(this->uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,double,double,double,quint64))); + + disconnect(this->uas, SIGNAL(attitudeControlEnabled(bool)), this, SLOT(updateAttitudeControllerEnabled(bool))); + disconnect(this->uas, SIGNAL(positionXYControlEnabled(bool)), this, SLOT(updatePositionXYControllerEnabled(bool))); + disconnect(this->uas, SIGNAL(positionZControlEnabled(bool)), this, SLOT(updatePositionZControllerEnabled(bool))); + disconnect(this->uas, SIGNAL(positionYawControlEnabled(bool)), this, SLOT(updatePositionYawControllerEnabled(bool))); + + disconnect(this->uas, SIGNAL(localizationChanged(UASInterface*,int)), this, SLOT(updateLocalization(UASInterface*,int))); + disconnect(this->uas, SIGNAL(visionLocalizationChanged(UASInterface*,int)), this, SLOT(updateVisionLocalization(UASInterface*,int))); + disconnect(this->uas, SIGNAL(gpsLocalizationChanged(UASInterface*,int)), this, SLOT(updateGpsLocalization(UASInterface*,int))); + disconnect(this->uas, SIGNAL(irUltraSoundLocalizationChanged(UASInterface*,int)), this, SLOT(updateInfraredUltrasoundLocalization(UASInterface*,int))); + } - connect(uas, SIGNAL(attitudeControlEnabled(bool)), this, SLOT(updateAttitudeControllerEnabled(bool))); - connect(uas, SIGNAL(positionXYControlEnabled(bool)), this, SLOT(updatePositionXYControllerEnabled(bool))); - connect(uas, SIGNAL(positionZControlEnabled(bool)), this, SLOT(updatePositionZControllerEnabled(bool))); - connect(uas, SIGNAL(positionYawControlEnabled(bool)), this, SLOT(updatePositionYawControllerEnabled(bool))); + connect(uas, SIGNAL(gpsSatelliteStatusChanged(int,int,float,float,float,bool)), this, SLOT(updateSatellite(int,int,float,float,float,bool))); + connect(uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64))); + connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64))); + connect(uas, SIGNAL(attitudeThrustSetPointChanged(UASInterface*,double,double,double,double,quint64)), this, SLOT(updateAttitudeSetpoints(UASInterface*,double,double,double,double,quint64))); + connect(uas, SIGNAL(positionSetPointsChanged(int,float,float,float,float,quint64)), this, SLOT(updatePositionSetpoints(int,float,float,float,float,quint64))); + connect(uas, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64))); + connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,double,double,double,quint64))); - connect(uas, SIGNAL(localizationChanged(UASInterface*,int)), this, SLOT(updateLocalization(UASInterface*,int))); - connect(uas, SIGNAL(visionLocalizationChanged(UASInterface*,int)), this, SLOT(updateVisionLocalization(UASInterface*,int))); - connect(uas, SIGNAL(gpsLocalizationChanged(UASInterface*,int)), this, SLOT(updateGpsLocalization(UASInterface*,int))); - connect(uas, SIGNAL(irUltraSoundLocalizationChanged(UASInterface*,int)), this, SLOT(updateInfraredUltrasoundLocalization(UASInterface*,int))); + connect(uas, SIGNAL(attitudeControlEnabled(bool)), this, SLOT(updateAttitudeControllerEnabled(bool))); + connect(uas, SIGNAL(positionXYControlEnabled(bool)), this, SLOT(updatePositionXYControllerEnabled(bool))); + connect(uas, SIGNAL(positionZControlEnabled(bool)), this, SLOT(updatePositionZControllerEnabled(bool))); + connect(uas, SIGNAL(positionYawControlEnabled(bool)), this, SLOT(updatePositionYawControllerEnabled(bool))); - // Now connect the new UAS + connect(uas, SIGNAL(localizationChanged(UASInterface*,int)), this, SLOT(updateLocalization(UASInterface*,int))); + connect(uas, SIGNAL(visionLocalizationChanged(UASInterface*,int)), this, SLOT(updateVisionLocalization(UASInterface*,int))); + connect(uas, SIGNAL(gpsLocalizationChanged(UASInterface*,int)), this, SLOT(updateGpsLocalization(UASInterface*,int))); + connect(uas, SIGNAL(irUltraSoundLocalizationChanged(UASInterface*,int)), this, SLOT(updateInfraredUltrasoundLocalization(UASInterface*,int))); - //if (this->uas != uas) - // { - //qDebug() << "UAS SET!" << "ID:" << uas->getUASID(); - // Setup communication - //connect(uas, SIGNAL(valueChanged(UASInterface*,QString,double,quint64)), this, SLOT(updateValue(UASInterface*,QString,double,quint64))); - //} + this->uas = uas; } void HSIDisplay::updateSpeed(UASInterface* uas, double vx, double vy, double vz, quint64 time) @@ -667,18 +673,18 @@ void HSIDisplay::drawWaypoints(QPainter& painter) if (uas) { const QVector& list = uas->getWaypointManager().getWaypointList(); -// for (int i = 0; i < list.size(); i++) -// { -// QPointF in(list.at(i)->getX(), list.at(i)->getY()); -// // Transform from world to body coordinates -// in = metricWorldToBody(in); -// // Scale from metric to screen reference coordinates -// QPointF p = metricBodyToRef(in); -// Waypoint2DIcon* wp = new Waypoint2DIcon(); -// wp->setLocalPosition(list.at(i)->getX(), list.at(i)->getY()); -// wp->setPos(0, 0); -// scene()->addItem(wp); -// } + // for (int i = 0; i < list.size(); i++) + // { + // QPointF in(list.at(i)->getX(), list.at(i)->getY()); + // // Transform from world to body coordinates + // in = metricWorldToBody(in); + // // Scale from metric to screen reference coordinates + // QPointF p = metricBodyToRef(in); + // Waypoint2DIcon* wp = new Waypoint2DIcon(); + // wp->setLocalPosition(list.at(i)->getX(), list.at(i)->getY()); + // wp->setPos(0, 0); + // scene()->addItem(wp); + // } QColor color; painter.setBrush(Qt::NoBrush); @@ -925,6 +931,26 @@ void HSIDisplay::wheelEvent(QWheelEvent* event) emit metricWidthChanged(metricWidth); } +void HSIDisplay::showEvent(QShowEvent* event) +{ + // React only to internal (pre-display) + // events + Q_UNUSED(event) + { + refreshTimer->start(updateInterval); + } +} + +void HSIDisplay::hideEvent(QHideEvent* event) +{ + // React only to internal (post-display) + // events + Q_UNUSED(event) + { + refreshTimer->stop(); + } +} + void HSIDisplay::updateJoystick(double roll, double pitch, double yaw, double thrust, int xHat, int yHat) { Q_UNUSED(roll); diff --git a/src/ui/HSIDisplay.h b/src/ui/HSIDisplay.h index 2fb873866c9218c7177306c833edbdeca6562882..c70598e41daccd19dfc9f2142ff2afb3179ae76f 100644 --- a/src/ui/HSIDisplay.h +++ b/src/ui/HSIDisplay.h @@ -111,6 +111,8 @@ protected slots: protected: + void showEvent(QShowEvent* event); + void hideEvent(QHideEvent* event); /** @brief Get color from GPS signal-to-noise colormap */ static QColor getColorForSNR(float snr); /** @brief Metric world coordinates to metric body coordinates */ diff --git a/src/ui/HUD.cc b/src/ui/HUD.cc index c95107ea10545701aa35b8e4ef550167f9809258..ad090a1c7530b1b00d950f7a524657ea40ac9a2a 100644 --- a/src/ui/HUD.cc +++ b/src/ui/HUD.cc @@ -29,6 +29,8 @@ This file is part of the QGROUNDCONTROL project * */ +#include + #include #include #ifndef M_PI @@ -39,6 +41,7 @@ This file is part of the QGROUNDCONTROL project #include "UASManager.h" #include "HUD.h" #include "MG.h" +#include "QGC.h" // Fix for some platforms, e.g. windows #ifndef GL_MULTISAMPLE @@ -129,7 +132,7 @@ HUD::HUD(int width, int height, QWidget* parent) glImage = QGLWidget::convertToGLFormat(fill); // Refresh timer - refreshTimer->setInterval(50); // 20 Hz + refreshTimer->setInterval(updateInterval); //connect(refreshTimer, SIGNAL(timeout()), this, SLOT(update())); connect(refreshTimer, SIGNAL(timeout()), this, SLOT(paintHUD())); @@ -150,8 +153,16 @@ HUD::HUD(int width, int height, QWidget* parent) if(!QFile::exists(fontFileName)) qDebug() << "ERROR! font file: " << fontFileName << " DOES NOT EXIST!"; fontDatabase.addApplicationFont(fontFileName); - font = fontDatabase.font(fontFamilyName, "Roman", (int)(10*scalingFactor*1.2f+0.5f)); - if (font.family() != fontFamilyName) qDebug() << "ERROR! Font not loaded: " << fontFamilyName; + font = fontDatabase.font(fontFamilyName, "Roman", qMax(5,(int)(10.0f*scalingFactor*1.2f+0.5f))); + QFont* fontPtr = &font; + if (!fontPtr) + { + qDebug() << "ERROR! FONT NOT LOADED!"; + } + else + { + if (font.family() != fontFamilyName) qDebug() << "ERROR! WRONG FONT LOADED: " << fontFamilyName; + } // Connect with UAS UASManager* manager = UASManager::instance(); @@ -167,25 +178,22 @@ HUD::~HUD() void HUD::showEvent(QShowEvent* event) { - Q_UNUSED(event); - if (isVisible()) + // React only to internal (pre-display) + // events + Q_UNUSED(event) { - refreshTimer->start(); - } - else - { - refreshTimer->stop(); + refreshTimer->start(updateInterval); } } -void HUD::start() +void HUD::hideEvent(QHideEvent* event) { - refreshTimer->start(); -} - -void HUD::stop() -{ - refreshTimer->stop(); + // React only to internal (pre-display) + // events + Q_UNUSED(event) + { + refreshTimer->stop(); + } } void HUD::updateValue(UASInterface* uas, QString name, double value, quint64 msec) @@ -553,9 +561,13 @@ void HUD::paintEvent(QPaintEvent *event) void HUD::paintHUD() { -// static quint64 interval = 0; -// qDebug() << "INTERVAL:" << MG::TIME::getGroundTimeNow() - interval << __FILE__ << __LINE__; -// interval = MG::TIME::getGroundTimeNow(); + // static quint64 interval = 0; + // qDebug() << "INTERVAL:" << MG::TIME::getGroundTimeNow() - interval << __FILE__ << __LINE__; + // interval = MG::TIME::getGroundTimeNow(); + +#if (QGC_EVENTLOOP_DEBUG) + qDebug() << "EVENTLOOP:" << __FILE__ << __LINE__; +#endif // Read out most important values to limit hash table lookups static float roll = 0.0f; diff --git a/src/ui/HUD.h b/src/ui/HUD.h index 658054de3ce8a67bb096da831e8db85925c5565a..f5f4695f28b490316526bef6aa5b90e56a186277 100644 --- a/src/ui/HUD.h +++ b/src/ui/HUD.h @@ -60,11 +60,6 @@ public slots: void initializeGL(); //void paintGL(); - /** @brief Start updating the view at 30Hz */ - void start(); - /** @brief Stop updating the view */ - void stop(); - /** @brief Set the currently monitored UAS */ void setActiveUAS(UASInterface* uas); @@ -123,8 +118,12 @@ protected: float refLineWidthToPen(float line); /** @brief Rotate a polygon around a point clockwise */ void rotatePolygonClockWiseRad(QPolygonF& p, float angle, QPointF origin); - /** @brief Override base class show */ - virtual void showEvent(QShowEvent* event); + /** @brief Start updating widget */ + void showEvent(QShowEvent* event); + /** @brief Stop updating widget */ + void hideEvent(QHideEvent* event); + + static const int updateInterval = 50; QImage* image; ///< Double buffer image QImage glImage; ///< The background / camera image diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index a57150e1b89a4b9c85fa4889d6c08333250a3323..c1371fc68dfdf3f08c5abec30099db2625988a07 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -38,6 +38,20 @@ #include "LogCompressor.h" +MainWindow* MainWindow::instance() +{ + static MainWindow* _instance = 0; + if(_instance == 0) + { + _instance = new MainWindow(); + + /* Set the application as parent to ensure that this object + * will be destroyed when the main application exits */ + //_instance->setParent(qApp); + } + return _instance; +} + /** * Create new mainwindow. The constructor instantiates all parts of the user * interface. It does NOT show the mainwindow. To display it, call the show() @@ -48,11 +62,27 @@ MainWindow::MainWindow(QWidget *parent): QMainWindow(parent), toolsMenuActions(), - currentView(VIEW_MAVLINK), + currentView(VIEW_OPERATOR), + aboutToCloseFlag(false), settings() { - this->hide(); - this->setVisible(false); + // Get current settings + settings.sync(); + + // Check if the settings exist, instantiate defaults if necessary + QString centralKey = buildMenuKey(SUB_SECTION_CHECKED, CENTRAL_MAP, currentView); + if (!settings.contains(centralKey)) + { + settings.setValue(centralKey,true); + } + + QString listKey = buildMenuKey(SUB_SECTION_CHECKED, MENU_UAS_LIST, currentView); + if (!settings.contains(listKey)) + { + settings.setValue(listKey, true); + } + + settings.sync(); // Setup user interface ui.setupUi(this); @@ -78,38 +108,32 @@ MainWindow::MainWindow(QWidget *parent): // Create actions connectCommonActions(); + // Set dock options + setDockOptions(AnimatedDocks | AllowTabbedDocks | AllowNestedDocks); + // Load mavlink view as default widget set - loadMAVLinkView(); + //loadMAVLinkView(); + if (settings.contains("geometry")) + { + // Restore the window geometry + restoreGeometry(settings.value("geometry").toByteArray()); + } + else + { // Adjust the size adjustSize(); + } - // Load previous widget setup - - // FIXME WORK IN PROGRESS - QSettings settings(QGC::COMPANYNAME, QGC::APPNAME); - - QList dockwidgets = qFindChildren(this); - if (dockwidgets.size()) + // Populate link menu + QList links = LinkManager::instance()->getLinks(); + foreach(LinkInterface* link, links) { - settings.beginGroup("mainwindow/dockwidgets"); - for (int i = 0; i < dockwidgets.size(); ++i) - { - QDockWidget *dockWidget = dockwidgets.at(i); - if (dockWidget->parentWidget() == this) - { - if (settings.contains(dockWidget->windowTitle())) - { - dockWidget->setVisible(settings.value(dockWidget->windowTitle(), dockWidget->isVisible()).toBool()); - } + this->addLink(link); } - } - settings.endGroup(); - } - - - this->show(); + // Enable and update view + presentView(); } MainWindow::~MainWindow() @@ -124,141 +148,55 @@ void MainWindow::buildCommonWidgets() mavlink = new MAVLinkProtocol(); // Dock widgets + if (!controlDockWidget) + { controlDockWidget = new QDockWidget(tr("Control"), this); controlDockWidget->setWidget( new UASControlWidget(this) ); - addToToolsMenu (controlDockWidget, tr("UAS Control"), SLOT(showToolWidget()), MENU_UAS_CONTROL, Qt::LeftDockWidgetArea); + addToToolsMenu (controlDockWidget, tr("Control"), SLOT(showToolWidget()), MENU_UAS_CONTROL, Qt::LeftDockWidgetArea); + } + if (!listDockWidget) + { listDockWidget = new QDockWidget(tr("Unmanned Systems"), this); listDockWidget->setWidget( new UASListWidget(this) ); - addToToolsMenu (listDockWidget, tr("UAS List"), SLOT(showToolWidget()), MENU_UAS_LIST, Qt::RightDockWidgetArea); + addToToolsMenu (listDockWidget, tr("Unmanned Systems"), SLOT(showToolWidget()), MENU_UAS_LIST, Qt::RightDockWidgetArea); + } + if (!waypointsDockWidget) + { waypointsDockWidget = new QDockWidget(tr("Waypoint List"), this); waypointsDockWidget->setWidget( new WaypointList(this, NULL) ); addToToolsMenu (waypointsDockWidget, tr("Waypoints List"), SLOT(showToolWidget()), MENU_WAYPOINTS, Qt::BottomDockWidgetArea); + } + if (!infoDockWidget) + { infoDockWidget = new QDockWidget(tr("Status Details"), this); infoDockWidget->setWidget( new UASInfoWidget(this) ); addToToolsMenu (infoDockWidget, tr("Status Details"), SLOT(showToolWidget()), MENU_STATUS, Qt::RightDockWidgetArea); + } - + if (!debugConsoleDockWidget) + { 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 + if (!mapWidget) + { mapWidget = new MapWidget(this); addToCentralWidgetsMenu (mapWidget, "Maps", SLOT(showCentralWidget()),CENTRAL_MAP); + } + if (!protocolWidget) + { protocolWidget = new XMLCommProtocolWidget(this); addToCentralWidgetsMenu (protocolWidget, "Mavlink Generator", SLOT(showCentralWidget()),CENTRAL_PROTOCOL); - - + } } -//======= -//void MainWindow::storeSettings() -//{ -// QSettings settings(QGC::COMPANYNAME, QGC::APPNAME); - -// QList dockwidgets = qFindChildren(this); -// if (dockwidgets.size()) -// { -// settings.beginGroup("mainwindow/dockwidgets"); -// for (int i = 0; i < dockwidgets.size(); ++i) -// { -// QDockWidget *dockWidget = dockwidgets.at(i); -// if (dockWidget->parentWidget() == this) -// { -// settings.setValue(dockWidget->windowTitle(), QVariant(dockWidget->isVisible())); -// } -// } -// settings.endGroup(); -// } -// settings.sync(); -//} - -//QMenu* MainWindow::createCenterWidgetMenu() -//{ -// QMenu* menu = NULL; -// QStackedWidget* centerStack = dynamic_cast(centralWidget()); -// if (centerStack) -// { -// if (centerStack->count() > 0) -// { -// menu = new QMenu(this); -// for (int i = 0; i < centerStack->count(); ++i) -// { -// //menu->addAction(centerStack->widget(i)->actions()) -// } -// } -// } -// return menu; -//} - -//QMenu* MainWindow::createDockWidgetMenu() -//{ -// QMenu *menu = 0; -//#ifndef QT_NO_DOCKWIDGET -// QList dockwidgets = qFindChildren(this); -// if (dockwidgets.size()) -// { -// menu = new QMenu(this); -// for (int i = 0; i < dockwidgets.size(); ++i) -// { -// QDockWidget *dockWidget = dockwidgets.at(i); -// if (dockWidget->parentWidget() == this) -// { -// menu->addAction(dockwidgets.at(i)->toggleViewAction()); -// } -// } -// menu->addSeparator(); -// } -//#endif -// return menu; -//} - -////QList* MainWindow::getMainWidgets() -////{ - -////} - -////QMenu* QMainWindow::getDockWidgetMenu() -////{ -//// Q_D(QMainWindow); -//// QMenu *menu = 0; -////#ifndef QT_NO_DOCKWIDGET -//// QList dockwidgets = qFindChildren(this); -//// if (dockwidgets.size()) { -//// menu = new QMenu(this); -//// for (int i = 0; i < dockwidgets.size(); ++i) { -//// QDockWidget *dockWidget = dockwidgets.at(i); -//// if (dockWidget->parentWidget() == this -//// && d->layout->contains(dockWidget)) { -//// menu->addAction(dockwidgets.at(i)->toggleViewAction()); -//// } -//// } -//// menu->addSeparator(); -//// } -////#endif // QT_NO_DOCKWIDGET -////#ifndef QT_NO_TOOLBAR -//// QList toolbars = qFindChildren(this); -//// if (toolbars.size()) { -//// if (!menu) -//// menu = new QMenu(this); -//// for (int i = 0; i < toolbars.size(); ++i) { -//// QToolBar *toolBar = toolbars.at(i); -//// if (toolBar->parentWidget() == this -//// && d->layout->contains(toolBar)) { -//// menu->addAction(toolbars.at(i)->toggleViewAction()); -//// } -//// } -//// } -////#endif -//// Q_UNUSED(d); -//// return menu; -////} -//>>>>>>> master - void MainWindow::buildPxWidgets() { //FIXME: memory of acceptList will never be freed again @@ -275,179 +213,183 @@ void MainWindow::buildPxWidgets() acceptList2->append("Battery"); acceptList2->append("Pressure"); + if (!linechartWidget) + { // Center widgets linechartWidget = new Linecharts(this); addToCentralWidgetsMenu(linechartWidget, "Line Plots", SLOT(showCentralWidget()), CENTRAL_LINECHART); + } + if (!hudWidget) + { hudWidget = new HUD(320, 240, this); addToCentralWidgetsMenu(hudWidget, "HUD", SLOT(showCentralWidget()), CENTRAL_HUD); + } + if (!dataplotWidget) + { dataplotWidget = new QGCDataPlot2D(this); addToCentralWidgetsMenu(dataplotWidget, "Data Plots", SLOT(showCentralWidget()), CENTRAL_DATA_PLOT); + } #ifdef QGC_OSG_ENABLED + if (!_3DWidget) + { _3DWidget = Q3DWidgetFactory::get("PIXHAWK"); addToCentralWidgetsMenu(_3DWidget, "Local 3D", SLOT(showCentralWidget()), CENTRAL_3D_LOCAL); - + } #endif #ifdef QGC_OSGEARTH_ENABLED + if (!_3DMapWidget) + { _3DMapWidget = Q3DWidgetFactory::get("MAP3D"); addToCentralWidgetsMenu(_3DMapWidget, "OSG Earth 3D", SLOT(showCentralWidget()), CENTRAL_OSGEARTH); - + } #endif -#if (defined Q_OS_WIN) | (defined Q_OS_MAC) + +#if (defined _MSC_VER) | (defined Q_OS_MAC) + if (!gEarthWidget) + { gEarthWidget = new QGCGoogleEarthView(this); addToCentralWidgetsMenu(gEarthWidget, "Google Earth", SLOT(showCentralWidget()), CENTRAL_GOOGLE_EARTH); + } #endif // Dock widgets + if (!detectionDockWidget) + { 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); + } - + if (!parametersDockWidget) + { parametersDockWidget = new QDockWidget(tr("Onboard Parameters"), this); parametersDockWidget->setWidget( new ParameterInterface(this) ); addToToolsMenu (parametersDockWidget, tr("Onboard Parameters"), SLOT(showToolWidget()), MENU_PARAMETERS, Qt::RightDockWidgetArea); + } + if (!watchdogControlDockWidget) + { watchdogControlDockWidget = new QDockWidget(tr("Process Control"), this); watchdogControlDockWidget->setWidget( new WatchdogControl(this) ); addToToolsMenu (watchdogControlDockWidget, tr("Process Control"), SLOT(showToolWidget()), MENU_WATCHDOG, Qt::BottomDockWidgetArea); + } - + if (!hsiDockWidget) + { hsiDockWidget = new QDockWidget(tr("Horizontal Situation Indicator"), this); hsiDockWidget->setWidget( new HSIDisplay(this) ); addToToolsMenu (hsiDockWidget, tr("HSI"), SLOT(showToolWidget()), MENU_HSI, Qt::BottomDockWidgetArea); -//======= -// controlDockWidget = new QDockWidget(tr("Control"), this); -// controlDockWidget->setWidget( new UASControlWidget(this) ); -// addDockWidget(Qt::LeftDockWidgetArea, controlDockWidget); -// controlDockWidget->hide(); - -// infoDockWidget = new QDockWidget(tr("Status Details"), this); -// infoDockWidget->setWidget( new UASInfoWidget(this) ); -// addDockWidget(Qt::LeftDockWidgetArea, infoDockWidget); -// //infoDockWidget->hide(); - -// listDockWidget = new QDockWidget(tr("Unmanned Systems"), this); -// listDockWidget->setWidget( new UASListWidget(this) ); -// addDockWidget(Qt::BottomDockWidgetArea, listDockWidget); -// listDockWidget->hide(); - -// waypointsDockWidget = new QDockWidget(tr("Waypoint List"), this); -// waypointsDockWidget->setWidget( new WaypointList(this, NULL) ); -// addDockWidget(Qt::BottomDockWidgetArea, waypointsDockWidget); -// waypointsDockWidget->hide(); - -// detectionDockWidget = new QDockWidget(tr("Object Recognition"), this); -// detectionDockWidget->setWidget( new ObjectDetectionView("images/patterns", this) ); -// addDockWidget(Qt::RightDockWidgetArea, detectionDockWidget); -// detectionDockWidget->hide(); - -// debugConsoleDockWidget = new QDockWidget(tr("Communication Console"), this); -// debugConsoleDockWidget->setWidget( new DebugConsole(this) ); -// addDockWidget(Qt::BottomDockWidgetArea, debugConsoleDockWidget); - -// parametersDockWidget = new QDockWidget(tr("Onboard Parameters"), this); -// parametersDockWidget->setWidget( new ParameterInterface(this) ); -// addDockWidget(Qt::RightDockWidgetArea, parametersDockWidget); - -// watchdogControlDockWidget = new QDockWidget(tr("Process Control"), this); -// watchdogControlDockWidget->setWidget( new WatchdogControl(this) ); -// addDockWidget(Qt::RightDockWidgetArea, watchdogControlDockWidget); -// watchdogControlDockWidget->hide(); - -// hsiDockWidget = new QDockWidget(tr("Horizontal Situation Indicator"), this); -// hsiDockWidget->setWidget( new HSIDisplay(this) ); -// addDockWidget(Qt::LeftDockWidgetArea, hsiDockWidget); -//>>>>>>> master + } + if (!headDown1DockWidget) + { headDown1DockWidget = new QDockWidget(tr("System Stats"), this); headDown1DockWidget->setWidget( new HDDisplay(acceptList, this) ); - addToToolsMenu (headDown1DockWidget, tr("Flight Display"), SLOT(showToolWidget()), MENU_HDD_1, Qt::RightDockWidgetArea); + } + if (!headDown2DockWidget) + { 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); + } + if (!rcViewDockWidget) + { rcViewDockWidget = new QDockWidget(tr("Radio Control"), this); rcViewDockWidget->setWidget( new QGCRemoteControlView(this) ); addToToolsMenu (rcViewDockWidget, tr("Radio Control"), SLOT(showToolWidget()), MENU_RC_VIEW, Qt::BottomDockWidgetArea); -//======= -// addDockWidget(Qt::RightDockWidgetArea, headDown1DockWidget); - -// headDown2DockWidget = new QDockWidget(tr("Payload Status"), this); -// headDown2DockWidget->setWidget( new HDDisplay(acceptList2, this) ); -// addDockWidget(Qt::RightDockWidgetArea, headDown2DockWidget); - -// rcViewDockWidget = new QDockWidget(tr("Radio Control"), this); -// rcViewDockWidget->setWidget( new QGCRemoteControlView(this) ); -// addDockWidget(Qt::BottomDockWidgetArea, rcViewDockWidget); -// rcViewDockWidget->hide(); -//>>>>>>> master + } + if (!headUpDockWidget) + { 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 + if (!joystick) + { joystick = new JoystickInput(); + } } void MainWindow::buildSlugsWidgets() { + if (!linechartWidget) + { // Center widgets -// linechartWidget = new Linecharts(this); -// addToCentralWidgetsMenu(linechartWidget, "Line Plots", SLOT(showCentralWidget()), CENTRAL_LINECHART); - -// // 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); + linechartWidget = new Linecharts(this); + } + if (!headUpDockWidget) + { + // 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); + } -// // 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); + if (!rcViewDockWidget) + { + rcViewDockWidget = new QDockWidget(tr("Radio Control"), this); + rcViewDockWidget->setWidget( new QGCRemoteControlView(this) ); + addToToolsMenu (rcViewDockWidget, tr("Radio Control"), SLOT(showToolWidget()), MENU_RC_VIEW, Qt::BottomDockWidgetArea); + } + if (!slugsDataWidget) + { + // 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); + } + if (!slugsPIDControlWidget) + { slugsPIDControlWidget = new QDockWidget(tr("Slugs PID Control"), this); slugsPIDControlWidget->setWidget(new SlugsPIDControl(this)); addToToolsMenu (slugsPIDControlWidget, tr("PID Configuration"), SLOT(showToolWidget()), MENU_SLUGS_PID, Qt::LeftDockWidgetArea); + } + if (!slugsHilSimWidget) + { 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); + } + if (!slugsCamControlWidget) + { slugsCamControlWidget = new QDockWidget(tr("Slugs Video Camera Control"), this); slugsCamControlWidget->setWidget(new SlugsVideoCamControl(this)); addToToolsMenu (slugsCamControlWidget, tr("Camera Control"), SLOT(showToolWidget()), MENU_SLUGS_CAMERA, Qt::BottomDockWidgetArea); - + } } void MainWindow::addToCentralWidgetsMenu ( QWidget* widget, const QString title, const char * slotName, - TOOLS_WIDGET_NAMES centralWidget){ + TOOLS_WIDGET_NAMES centralWidget) +{ QAction* tempAction; // Add the separator that will separate tools from central Widgets - if (!toolsMenuActions[CENTRAL_SEPARATOR]){ + if (!toolsMenuActions[CENTRAL_SEPARATOR]) + { tempAction = ui.menuTools->addSeparator(); toolsMenuActions[CENTRAL_SEPARATOR] = tempAction; tempAction->setData(CENTRAL_SEPARATOR); @@ -464,28 +406,31 @@ void MainWindow::addToCentralWidgetsMenu ( QWidget* widget, QString chKey = buildMenuKey(SUB_SECTION_CHECKED, centralWidget, currentView); - if (!settings.contains(chKey)){ + 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); - } -void MainWindow::showCentralWidget(){ +void MainWindow::showCentralWidget() +{ QAction* senderAction = qobject_cast(sender()); int tool = senderAction->data().toInt(); QString chKey; // check the current action - if (senderAction && dockWidgets[tool]){ + if (senderAction && dockWidgets[tool]) + { // uncheck all central widget actions QHashIterator i(toolsMenuActions); @@ -511,25 +456,34 @@ void MainWindow::showCentralWidget(){ // store the selected central widget chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(tool), currentView); settings.setValue(chKey,true); + settings.sync(); presentView(); } } +/** + * Adds a widget to the tools menu and sets it visible if it was + * enabled last time. + */ void MainWindow::addToToolsMenu ( QWidget* widget, const QString title, const char * slotName, TOOLS_WIDGET_NAMES tool, - Qt::DockWidgetArea location){ + Qt::DockWidgetArea location) +{ QAction* tempAction; QString posKey, chKey; - if (toolsMenuActions[CENTRAL_SEPARATOR]){ + if (toolsMenuActions[CENTRAL_SEPARATOR]) + { tempAction = new QAction(title, this); ui.menuTools->insertAction(toolsMenuActions[CENTRAL_SEPARATOR], tempAction); - } else { + } + else + { tempAction = ui.menuTools->addAction(title); } @@ -543,20 +497,28 @@ void MainWindow::addToToolsMenu ( QWidget* widget, posKey = buildMenuKey (SUB_SECTION_LOCATION,tool, currentView); - if (!settings.contains(posKey)){ + if (!settings.contains(posKey)) + { settings.setValue(posKey,location); dockWidgetLocations[tool] = location; - } else { - dockWidgetLocations[tool] = static_cast (settings.value(posKey).toInt()); } + else + { + dockWidgetLocations[tool] = static_cast (settings.value(posKey, Qt::RightDockWidgetArea).toInt()); + } chKey = buildMenuKey(SUB_SECTION_CHECKED,tool, currentView); - if (!settings.contains(chKey)){ + if (!settings.contains(chKey)) + { settings.setValue(chKey,false); tempAction->setChecked(false); - } else { + widget->setVisible(false); + } + else + { tempAction->setChecked(settings.value(chKey).toBool()); + widget->setVisible(settings.value(chKey, false).toBool()); } // connect the action @@ -570,48 +532,75 @@ void MainWindow::addToToolsMenu ( QWidget* widget, } -void MainWindow::showToolWidget(){ +void MainWindow::showToolWidget() +{ QAction* temp = qobject_cast(sender()); int tool = temp->data().toInt(); - if (temp && dockWidgets[tool]){ - if (temp->isChecked()){ + if (temp && dockWidgets[tool]) + { + if (temp->isChecked()) + { addDockWidget(dockWidgetLocations[tool], qobject_cast (dockWidgets[tool])); qobject_cast(dockWidgets[tool])->show(); - } else { + } + else + { removeDockWidget(qobject_cast(dockWidgets[tool])); } QString chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(tool), currentView); settings.setValue(chKey,temp->isChecked()); + settings.sync(); } } -void MainWindow::showTheWidget (TOOLS_WIDGET_NAMES widget, VIEW_SECTIONS view){ +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(); + tempVisible = settings.value(buildMenuKey (SUB_SECTION_CHECKED,widget,view), false).toBool(); + + // Some widgets are per default visible. Overwrite the settings value if not present. + if (widget == MainWindow::MENU_UAS_LIST) + { + if (!settings.contains(buildMenuKey (SUB_SECTION_CHECKED,widget,view))) + { + tempVisible = true; + } + } - if (tempWidget){ + 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()); + tempLocation = static_cast (settings.value(buildMenuKey (SUB_SECTION_LOCATION,widget, view), QVariant(Qt::RightDockWidgetArea)).toInt()); - if (tempWidget && tempVisible){ + // if (widget == MainWindow::MENU_UAS_LIST) + // { + // if (!settings.contains(buildMenuKey (SUB_SECTION_LOCATION,widget, view))) + // { + // tempLocation = Qt::RightDockWidgetArea; + // } + // } + + if ((tempWidget != NULL) && tempVisible) + { addDockWidget(tempLocation, tempWidget); tempWidget->show(); } } -QString MainWindow::buildMenuKey(SETTINGS_SECTIONS section, TOOLS_WIDGET_NAMES tool, VIEW_SECTIONS view){ +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; @@ -626,51 +615,62 @@ QString MainWindow::buildMenuKey(SETTINGS_SECTIONS section, TOOLS_WIDGET_NAMES t QString::number(section) + "/" ); } -void MainWindow::updateVisibilitySettings (bool vis){ - Q_UNUSED(vis); - - // This is commented since when the application closes - // sets the visibility to false. +void MainWindow::closeEvent(QCloseEvent *event) +{ + settings.setValue("geometry", saveGeometry()); + //settings.setValue("windowState", saveState()); + aboutToCloseFlag = true; + settings.setValue("VIEW_ON_APPLICATION_CLOSE", currentView); + settings.sync(); + QMainWindow::closeEvent(event); +} - // TODO: A workaround is needed. The QApplication::aboutToQuit - // did not work +/** + * Stores the visibility setting of each widget. This method + * will only change the settings if the application is not + * about to close. + */ +void MainWindow::updateVisibilitySettings (bool vis) +{ + if (!aboutToCloseFlag) + { - /* QDockWidget* temp = qobject_cast(sender()); + if (temp) + { QHashIterator i(dockWidgets); - while (i.hasNext()) { + 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; + if ((static_cast (dockWidgets[i.key()])) == temp) + { + QString chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(i.key()), currentView); settings.setValue(chKey,vis); + settings.sync(); toolsMenuActions[i.key()]->setChecked(vis); break; } } -*/ + } + } } -void MainWindow::updateLocationSettings (Qt::DockWidgetArea location){ +void MainWindow::updateLocationSettings (Qt::DockWidgetArea location) +{ QDockWidget* temp = qobject_cast(sender()); QHashIterator i(dockWidgets); - while (i.hasNext()) { + while (i.hasNext()) + { i.next(); - if ((static_cast (dockWidgets[i.key()])) == temp){ + if ((static_cast (dockWidgets[i.key()])) == temp) + { QString posKey = buildMenuKey (SUB_SECTION_LOCATION,static_cast(i.key()), currentView); settings.setValue(posKey,location); break; } } -//======= -// addDockWidget(Qt::BottomDockWidgetArea, slugsCamControlWidget); -// slugsCamControlWidget->hide(); - -// //FIXME: free memory in destructor -// joystick = new JoystickInput(); -//>>>>>>> master } /** @@ -699,26 +699,11 @@ void MainWindow::connectCommonWidgets() void MainWindow::connectPxWidgets() { - if (linechartWidget) - { - connect(linechartWidget, SIGNAL(logfileWritten(QString)), - this, SLOT(loadDataView(QString))); - } - + // No special connections necessary at this point } 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()){ connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), slugsHilSimWidget->widget(), SLOT(activeUasSet(UASInterface*))); @@ -738,8 +723,8 @@ void MainWindow::arrangeCommonCenterStack() if (!centerStack) return; - if (mapWidget) centerStack->addWidget(mapWidget); - if (protocolWidget) centerStack->addWidget(protocolWidget); + if (mapWidget && (centerStack->indexOf(mapWidget) == -1)) centerStack->addWidget(mapWidget); + if (protocolWidget && (centerStack->indexOf(protocolWidget) == -1)) centerStack->addWidget(protocolWidget); setCentralWidget(centerStack); } @@ -752,20 +737,20 @@ void MainWindow::arrangePxCenterStack() return; } - if (linechartWidget) centerStack->addWidget(linechartWidget); + + if (linechartWidget && (centerStack->indexOf(linechartWidget) == -1)) centerStack->addWidget(linechartWidget); #ifdef QGC_OSG_ENABLED - if (_3DWidget) centerStack->addWidget(_3DWidget); + if (_3DWidget && (centerStack->indexOf(_3DWidget) == -1)) centerStack->addWidget(_3DWidget); #endif #ifdef QGC_OSGEARTH_ENABLED - if (_3DMapWidget) centerStack->addWidget(_3DMapWidget); + if (_3DMapWidget && (centerStack->indexOf(_3DMapWidget) == -1)) centerStack->addWidget(_3DMapWidget); #endif -#if (defined Q_OS_WIN) | (defined Q_OS_MAC) - if (gEarthWidget) centerStack->addWidget(gEarthWidget); +#if (defined _MSC_VER) | (defined Q_OS_MAC) + if (gEarthWidget && (centerStack->indexOf(gEarthWidget) == -1)) centerStack->addWidget(gEarthWidget); #endif - if (hudWidget) centerStack->addWidget(hudWidget); - if (dataplotWidget) centerStack->addWidget(dataplotWidget); - + if (hudWidget && (centerStack->indexOf(hudWidget) == -1)) centerStack->addWidget(hudWidget); + if (dataplotWidget && (centerStack->indexOf(dataplotWidget) == -1)) centerStack->addWidget(dataplotWidget); } void MainWindow::arrangeSlugsCenterStack() @@ -776,10 +761,8 @@ void MainWindow::arrangeSlugsCenterStack() return; } - if (linechartWidget) centerStack->addWidget(linechartWidget); - - - if (hudWidget) centerStack->addWidget(hudWidget); + if (linechartWidget && (centerStack->indexOf(linechartWidget) == -1)) centerStack->addWidget(linechartWidget); + if (hudWidget && (centerStack->indexOf(hudWidget) == -1)) centerStack->addWidget(hudWidget); } @@ -901,7 +884,6 @@ void MainWindow::showStatusMessage(const QString& status) **/ void MainWindow::connectCommonActions() { - // Connect actions from ui connect(ui.actionAdd_Link, SIGNAL(triggered()), this, SLOT(addLink())); @@ -928,12 +910,10 @@ void MainWindow::connectCommonActions() 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())); - } void MainWindow::connectPxActions() { - ui.actionJoystickSettings->setVisible(true); // Joystick configuration @@ -1005,8 +985,6 @@ void MainWindow::addLink() ui.menuNetwork->addAction(commWidget->getAction()); commWidget->show(); - - // TODO Implement the link removal! } void MainWindow::addLink(LinkInterface *link) @@ -1097,29 +1075,8 @@ void MainWindow::UASCreated(UASInterface* uas) // TODO Stylesheet reloading should in theory not be necessary reloadStylesheet(); - switch (uas->getAutopilotType()){ - case (MAV_AUTOPILOT_GENERIC): - case (MAV_AUTOPILOT_ARDUPILOTMEGA): - case (MAV_AUTOPILOT_PIXHAWK): + switch (uas->getAutopilotType()) { - // 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 @@ -1135,58 +1092,70 @@ void MainWindow::UASCreated(UASInterface* uas) connectSlugsActions(); // FIXME: This type checking might be redundant - if (slugsDataWidget) { - SlugsDataSensorView* dataWidget = dynamic_cast(slugsDataWidget->widget()); - if (dataWidget) { + // 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; + break; + default: + 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(); - loadEngineerView(); } + break; } -} -/** - * Clears the current view completely - */ -void MainWindow::clearView() -{ - // Halt HUD central widget - if (hudWidget) hudWidget->stop(); + // Change the view only if this is the first UAS - // Disable linechart - if (linechartWidget) linechartWidget->setActive(false); + // If this is the first connected UAS, it is both created as well as + // the currently active UAS + if (UASManager::instance()->getActiveUAS() == uas) + { + qDebug() << "UPDATING THE VIEW SINCE THIS IS THE FIRST CONNECTED SYSTEM"; - // Halt HDDs - if (headDown1DockWidget) + // Load last view if setting is present + if (settings.contains("VIEW_ON_APPLICATION_CLOSE")) { - HDDisplay* hddWidget = dynamic_cast( headDown1DockWidget->widget() ); - if (hddWidget) hddWidget->stop(); + int view = settings.value("VIEW_ON_APPLICATION_CLOSE").toInt(); + currentView = (VIEW_SECTIONS) view; + presentView(); } - - if (headDown2DockWidget) + else { - HDDisplay* hddWidget = dynamic_cast( headDown2DockWidget->widget() ); - if (hddWidget) hddWidget->stop(); + loadEngineerView(); } - - // Halt HSI - if (hsiDockWidget) - { - HSIDisplay* hsi = dynamic_cast( hsiDockWidget->widget() ); - if (hsi) hsi->stop(); } - // Halt HUD if in docked widget mode - if (headUpDockWidget) - { - HUD* hud = dynamic_cast( headUpDockWidget->widget() ); - if (hud) hud->stop(); } +} +/** + * Clears the current view completely + */ +void MainWindow::clearView() +{ // Remove all dock widgets from main window QObjectList childList( this->children() ); @@ -1241,47 +1210,12 @@ void MainWindow::loadMAVLinkView() currentView = VIEW_MAVLINK; presentView(); -//======= -// // Slugs Data View -// if (slugsHilSimWidget) -// { -// addDockWidget(Qt::LeftDockWidgetArea, slugsHilSimWidget); -// slugsHilSimWidget->show(); -// } -//>>>>>>> master } -void MainWindow::presentView() { - - -#ifdef QGC_OSG_ENABLED - // 3D map - if (_3DWidget) - { - if (centerStack) - { - //map3DWidget->setActive(true); - centerStack->setCurrentWidget(_3DWidget); - } - } -#endif - +void MainWindow::presentView() +{ 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() ; - if (settings.value(buildMenuKey (SUB_SECTION_CHECKED,CENTRAL_LINECHART,currentView)).toBool()){ - if (centerStack) { - linechartWidget->setActive(true); - centerStack->setCurrentWidget(linechartWidget); - } - } else { - linechartWidget->setActive(false); - } - } - - // MAP qDebug() << "MAP"; @@ -1293,19 +1227,17 @@ void MainWindow::presentView() { // HEAD UP DISPLAY showTheCentralWidget(CENTRAL_HUD, currentView); - 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(); - } - } + + // GOOGLE EARTH + showTheCentralWidget(CENTRAL_GOOGLE_EARTH, currentView); + + // LOCAL 3D VIEW + showTheCentralWidget(CENTRAL_3D_LOCAL, currentView); + + // GLOBAL 3D VIEW + showTheCentralWidget(CENTRAL_3D_MAP, currentView); + + // Show docked widgets based on current view and autopilot type @@ -1340,12 +1272,10 @@ void MainWindow::presentView() { 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()), headUpDockWidget); headUpDockWidget->show(); } else { - tmpHud->stop(); headUpDockWidget->hide(); } } @@ -1369,234 +1299,89 @@ void MainWindow::presentView() { // HORIZONTAL SITUATION INDICATOR showTheWidget(MENU_HSI, currentView); - if (hsiDockWidget) - { - HSIDisplay* hsi = dynamic_cast( hsiDockWidget->widget() ); - 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(); - } - } - } + // if (hsiDockWidget) + // { + // HSIDisplay* hsi = dynamic_cast( hsiDockWidget->widget() ); + // if (hsi){ + // if (settings.value(buildMenuKey (SUB_SECTION_CHECKED,MENU_HSI,currentView)).toBool()){ + // addDockWidget(static_cast (settings.value(buildMenuKey (SUB_SECTION_LOCATION,MENU_HSI, currentView)).toInt()), + // hsiDockWidget); + // } + // } + // } // HEAD DOWN 1 showTheWidget(MENU_HDD_1, currentView); - if (headDown1DockWidget) - { - HDDisplay *hdd = dynamic_cast(headDown1DockWidget->widget()); - 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(); - } - } - } + // if (headDown1DockWidget) + // { + // HDDisplay *hdd = dynamic_cast(headDown1DockWidget->widget()); + // 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){ - 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(); - } - } - } + // if (headDown2DockWidget) + // { + // HDDisplay *hdd = dynamic_cast(headDown2DockWidget->widget()); + // 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(); + // } + // } + // } this->show(); } -void MainWindow::showTheCentralWidget (TOOLS_WIDGET_NAMES centralWidget, VIEW_SECTIONS view){ +void MainWindow::showTheCentralWidget (TOOLS_WIDGET_NAMES centralWidget, VIEW_SECTIONS view) +{ bool tempVisible; QWidget* tempWidget = dockWidgets[centralWidget]; -//======= -// // ONBOARD PARAMETERS -// if (parametersDockWidget) -// { -// addDockWidget(Qt::RightDockWidgetArea, parametersDockWidget); -// parametersDockWidget->show(); -// } -//} -//>>>>>>> master tempVisible = settings.value(buildMenuKey (SUB_SECTION_CHECKED,centralWidget,view)).toBool(); qDebug() << buildMenuKey (SUB_SECTION_CHECKED,centralWidget,view) << tempVisible; - if (toolsMenuActions[centralWidget]){ + if (toolsMenuActions[centralWidget]) + { toolsMenuActions[centralWidget]->setChecked(tempVisible); } - if (centerStack && tempWidget && tempVisible){ + if (centerStack && tempWidget && tempVisible) + { centerStack->setCurrentWidget(tempWidget); } } - - -/* -========================================================== - Potentially Deprecated -========================================================== -*/ - -void MainWindow::loadPixhawkEngineerView() +void MainWindow::loadWidgets() { - + loadOperatorView(); + //loadMAVLinkView(); + //loadPilotView(); } - -void MainWindow::loadAllView() +void MainWindow::loadDataView(QString fileName) { - clearView(); - - if (headDown1DockWidget) + // DATAPLOT + if (dataplotWidget) { - 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(); - } - } -//<<<<<<< HEAD -//======= -//} - -//void MainWindow::loadOperatorView() -//{ -// clearView(); - -// // MAP -// if (mapWidget) -// { -// QStackedWidget *centerStack = dynamic_cast(centralWidget()); -// if (centerStack) -// { -// centerStack->setCurrentWidget(mapWidget); -// } -// } -//>>>>>>> master - - // 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(); - } - - // OBJECT DETECTION - if (detectionDockWidget) - { - addDockWidget(Qt::RightDockWidgetArea, detectionDockWidget); - detectionDockWidget->show(); - } - - // LINE CHART - if (linechartWidget) - { - QStackedWidget *centerStack = dynamic_cast(centralWidget()); - if (centerStack) - { - linechartWidget->setActive(true); - centerStack->setCurrentWidget(linechartWidget); - } - } - - // ONBOARD PARAMETERS - if (parametersDockWidget) - { - addDockWidget(Qt::RightDockWidgetArea, parametersDockWidget); - parametersDockWidget->show(); - } -} - -void MainWindow::loadWidgets() -{ - //loadOperatorView(); - loadMAVLinkView(); - //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) + QStackedWidget *centerStack = dynamic_cast(centralWidget()); + if (centerStack) { centerStack->setCurrentWidget(dataplotWidget); dataplotWidget->loadFile(fileName); @@ -1619,45 +1404,12 @@ void MainWindow::load3DMapView() 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 } void MainWindow::loadGoogleEarthView() { - #if (defined Q_OS_WIN) | (defined Q_OS_MAC) +#if (defined _MSC_VER) | (defined Q_OS_MAC) clearView(); // 3D map @@ -1669,39 +1421,6 @@ void MainWindow::loadGoogleEarthView() 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(); - } - } #endif } @@ -1721,300 +1440,6 @@ void MainWindow::load3DView() 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 -} - -/* - ================================== - ========== ATTIC ================= - ================================== - -void MainWindow::buildWidgets() -{ - //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) ); - -<<<<<<< HEAD - 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) ); -======= - // RADIO CONTROL VIEW - if (rcViewDockWidget) - { - addDockWidget(Qt::BottomDockWidgetArea, rcViewDockWidget); - rcViewDockWidget->show(); - } -} ->>>>>>> master - - detectionDockWidget = new QDockWidget(tr("Object Recognition"), this); - detectionDockWidget->setWidget( new ObjectDetectionView("images/patterns", this) ); - -<<<<<<< HEAD - 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)); - -======= - if (protocolWidget) - { - QStackedWidget *centerStack = dynamic_cast(centralWidget()); - if (centerStack) - { - centerStack->setCurrentWidget(protocolWidget); - } - } ->>>>>>> master -} - -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::arrangeCenterStack() -{ - - 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); -======= - // ONBOARD PARAMETERS - if (parametersDockWidget) - { - addDockWidget(Qt::RightDockWidgetArea, parametersDockWidget); - parametersDockWidget->show(); - } ->>>>>>> master -} - -void MainWindow::connectActions() -{ -<<<<<<< HEAD - // 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()) - -======= - //loadEngineerView(); ->>>>>>> master } -*/ diff --git a/src/ui/MainWindow.h b/src/ui/MainWindow.h index be8b3184f1fdfa90a9ad7bc1458d150cee57c5fc..f866e9a1c6339a393d75d920c39ac639f6a996af 100644 --- a/src/ui/MainWindow.h +++ b/src/ui/MainWindow.h @@ -63,7 +63,9 @@ This file is part of the QGROUNDCONTROL project #include "HSIDisplay.h" #include "QGCDataPlot2D.h" #include "QGCRemoteControlView.h" +#if (defined Q_OS_MAC) | (defined _MSC_VER) #include "QGCGoogleEarthView.h" +#endif //#include "QMap3DWidget.h" #include "SlugsDataSensorView.h" #include "LogCompressor.h" @@ -83,7 +85,7 @@ class MainWindow : public QMainWindow { Q_OBJECT public: - MainWindow(QWidget *parent = 0); + static MainWindow* instance(); ~MainWindow(); public slots: @@ -138,21 +140,16 @@ public slots: /** @brief Reload the CSS style sheet */ void reloadStylesheet(); + void closeEvent(QCloseEvent* event); + /* ========================================================== 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); @@ -191,6 +188,8 @@ public slots: protected: + MainWindow(QWidget *parent = 0); + // 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 @@ -228,13 +227,15 @@ protected: }TOOLS_WIDGET_NAMES; - typedef enum _SETTINGS_SECTIONS { + typedef enum _SETTINGS_SECTIONS + { SECTION_MENU, SUB_SECTION_CHECKED, SUB_SECTION_LOCATION, } SETTINGS_SECTIONS; - typedef enum _VIEW_SECTIONS { + typedef enum _VIEW_SECTIONS + { VIEW_ENGINEER, VIEW_OPERATOR, VIEW_PILOT, @@ -259,7 +260,7 @@ protected: * @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); + void addToToolsMenu (QWidget* widget, const QString title, const char * slotName, TOOLS_WIDGET_NAMES tool, Qt::DockWidgetArea location=Qt::RightDockWidgetArea); /** * @brief Determines if a QDockWidget needs to be show and if so, shows it @@ -301,6 +302,7 @@ protected: /** @brief Keeps track of the current view */ VIEW_SECTIONS currentView; + bool aboutToCloseFlag; QStatusBar* statusBar; QStatusBar* createStatusBar(); @@ -351,7 +353,9 @@ protected: #ifdef QGC_OSGEARTH_ENABLED QPointer _3DMapWidget; #endif +#if (defined _MSC_VER) || (defined Q_OS_MAC) QPointer gEarthWidget; +#endif // Dock widgets QPointer controlDockWidget; QPointer infoDockWidget; diff --git a/src/ui/ObjectDetectionView.cc b/src/ui/ObjectDetectionView.cc index 03d8c007e8a2f6db0c168721ffb3f2d9a0fccd3b..b070795f01ab598471a564030fab3807e60848ea 100644 --- a/src/ui/ObjectDetectionView.cc +++ b/src/ui/ObjectDetectionView.cc @@ -40,8 +40,6 @@ This file is part of the PIXHAWK project #include #include -#include "MG.h" - ObjectDetectionView::ObjectDetectionView(QString folder, QWidget *parent) : QWidget(parent), patternList(), @@ -79,12 +77,15 @@ void ObjectDetectionView::changeEvent(QEvent *e) void ObjectDetectionView::setUAS(UASInterface* uas) { - //if (this->uas == NULL && uas != NULL) - //{ + if (this->uas != NULL) + { + disconnect(this->uas, SIGNAL(patternDetected(int, QString, float, bool)), this, SLOT(newPattern(int, QString, float, bool))); + disconnect(this->uas, SIGNAL(letterDetected(int, QString, float, bool)), this, SLOT(newLetter(int, QString, float, bool))); + } + this->uas = uas; connect(uas, SIGNAL(patternDetected(int, QString, float, bool)), this, SLOT(newPattern(int, QString, float, bool))); connect(uas, SIGNAL(letterDetected(int, QString, float, bool)), this, SLOT(newLetter(int, QString, float, bool))); - //} } void ObjectDetectionView::newPattern(int uasId, QString patternPath, float confidence, bool detected) @@ -117,7 +118,7 @@ void ObjectDetectionView::newPattern(int uasId, QString patternPath, float confi m_ui->listWidget->addItem(pattern.name + separator + "(" + QString::number(pattern.count) + ")" + separator + QString::number(pattern.confidence)); // load image - QString filePath = MG::DIR::getSupportFilesDirectory() + "/" + patternFolder + "/" + patternPath.split("/", QString::SkipEmptyParts).last(); + QString filePath = patternFolder + "/" + patternPath.split("/", QString::SkipEmptyParts).last(); QPixmap image = QPixmap(filePath); if (image.width() > image.height()) image = image.scaledToWidth(m_ui->imageLabel->width()); diff --git a/src/ui/QGCDataPlot2D.cc b/src/ui/QGCDataPlot2D.cc index afaf40dae0f1dd67b489f584861e5bfb0775116c..6f71b0ba4bffcef163e9765ea4584919162f5386 100644 --- a/src/ui/QGCDataPlot2D.cc +++ b/src/ui/QGCDataPlot2D.cc @@ -126,6 +126,13 @@ void QGCDataPlot2D::savePlot() fileName = QFileDialog::getSaveFileName( this, "Export File Name", QDesktopServices::storageLocation(QDesktopServices::DesktopLocation), "PDF Documents (*.pdf);;SVG Images (*.svg)"); + + if (!fileName.contains(".")) + { + // .csv is default extension + fileName.append(".pdf"); + } + while(!(fileName.endsWith(".svg") || fileName.endsWith(".pdf"))) { QMessageBox msgBox; @@ -262,7 +269,7 @@ void QGCDataPlot2D::selectFile() // Let user select the log file name //QDate date(QDate::currentDate()); // QString("./pixhawk-log-" + date.toString("yyyy-MM-dd") + "-" + QString::number(logindex) + ".log") - fileName = QFileDialog::getOpenFileName(this, tr("Specify log file name"), tr("."), tr("Logfile (*.txt)")); + fileName = QFileDialog::getOpenFileName(this, tr("Specify log file name"), QString(), "Logfile (*.csv *.txt *.log)"); // Store reference to file QFileInfo fileInfo(fileName); diff --git a/src/ui/QGCParamWidget.cc b/src/ui/QGCParamWidget.cc index ef4ad51cf4dbb1d4b464d9d7dd2882ab7098cc63..7fef09c2b92995fc31440a23bfcccb920769cbfe 100644 --- a/src/ui/QGCParamWidget.cc +++ b/src/ui/QGCParamWidget.cc @@ -62,26 +62,38 @@ QGCParamWidget::QGCParamWidget(UASInterface* uas, QWidget *parent) : horizontalLayout->addWidget(tree, 0, 0, 1, 3); QPushButton* refreshButton = new QPushButton(tr("Refresh")); + refreshButton->setToolTip(tr("Load parameters currently in non-permanent memory of aircraft.")); + refreshButton->setWhatsThis(tr("Load parameters currently in non-permanent memory of aircraft.")); connect(refreshButton, SIGNAL(clicked()), this, SLOT(requestParameterList())); horizontalLayout->addWidget(refreshButton, 1, 0); QPushButton* setButton = new QPushButton(tr("Transmit")); + setButton->setToolTip(tr("Set current parameters in non-permanent onboard memory")); + setButton->setWhatsThis(tr("Set current parameters in non-permanent onboard memory")); connect(setButton, SIGNAL(clicked()), this, SLOT(setParameters())); horizontalLayout->addWidget(setButton, 1, 1); QPushButton* writeButton = new QPushButton(tr("Write (ROM)")); + writeButton->setToolTip(tr("Copy current parameters in non-permanent memory of the aircraft to permanent memory. Transmit your parameters first to write these.")); + writeButton->setWhatsThis(tr("Copy current parameters in non-permanent memory of the aircraft to permanent memory. Transmit your parameters first to write these.")); connect(writeButton, SIGNAL(clicked()), this, SLOT(writeParameters())); horizontalLayout->addWidget(writeButton, 1, 2); QPushButton* readButton = new QPushButton(tr("Read (ROM)")); + readButton->setToolTip(tr("Copy parameters from permanent memory to non-permanent current memory of aircraft. DOES NOT update the parameters in this view, click refresh after copying them to get them.")); + readButton->setWhatsThis(tr("Copy parameters from permanent memory to non-permanent current memory of aircraft. DOES NOT update the parameters in this view, click refresh after copying them to get them.")); connect(readButton, SIGNAL(clicked()), this, SLOT(readParameters())); horizontalLayout->addWidget(readButton, 2, 2); QPushButton* loadFileButton = new QPushButton(tr("Load File")); + loadFileButton->setToolTip(tr("Load parameters from a file on this computer in the view. To write them to the aircraft, use transmit after loading them.")); + loadFileButton->setWhatsThis(tr("Load parameters from a file on this computer in the view. To write them to the aircraft, use transmit after loading them.")); connect(loadFileButton, SIGNAL(clicked()), this, SLOT(loadParameters())); horizontalLayout->addWidget(loadFileButton, 2, 0); QPushButton* saveFileButton = new QPushButton(tr("Save File")); + saveFileButton->setToolTip(tr("Save parameters in this view to a file on this computer.")); + saveFileButton->setWhatsThis(tr("Save parameters in this view to a file on this computer.")); connect(saveFileButton, SIGNAL(clicked()), this, SLOT(saveParameters())); horizontalLayout->addWidget(saveFileButton, 2, 1); diff --git a/src/ui/RadioCalibration/RadioCalibrationWindow.cc b/src/ui/RadioCalibration/RadioCalibrationWindow.cc index effb3b52ea7013457bf32c3974ab238169c85cc7..4bc42ada8613c1a13c9c5cb2e96609e3f954daa1 100644 --- a/src/ui/RadioCalibration/RadioCalibrationWindow.cc +++ b/src/ui/RadioCalibration/RadioCalibrationWindow.cc @@ -52,6 +52,66 @@ RadioCalibrationWindow::RadioCalibrationWindow(QWidget *parent) : setUASId(0); } +void RadioCalibrationWindow::setChannelRaw(int ch, float raw) +{ + /** this expects a particular channel to function mapping + \todo allow run-time channel mapping + */ + switch (ch) + { + case 0: + aileron->channelChanged(raw); + break; + case 1: + elevator->channelChanged(raw); + break; + case 2: + throttle->channelChanged(raw); + break; + case 3: + rudder->channelChanged(raw); + break; + case 4: + gyro->channelChanged(raw); + break; + case 5: + pitch->channelChanged(raw); + break; + + + } +} + +void RadioCalibrationWindow::setChannelScaled(int ch, float normalized) +{ +// /** this expects a particular channel to function mapping +// \todo allow run-time channel mapping +// */ +// switch (ch) +// { +// case 0: +// aileron->channelChanged(raw); +// break; +// case 1: +// elevator->channelChanged(raw); +// break; +// case 2: +// throttle->channelChanged(raw); +// break; +// case 3: +// rudder->channelChanged(raw); +// break; +// case 4: +// gyro->channelChanged(raw); +// break; +// case 5: +// pitch->channelChanged(raw); +// break; + + +// } +} + void RadioCalibrationWindow::setChannel(int ch, float raw, float normalized) { /** this expects a particular channel to function mapping diff --git a/src/ui/RadioCalibration/RadioCalibrationWindow.h b/src/ui/RadioCalibration/RadioCalibrationWindow.h index 1ec038c754c062314ec22d29baf0fd228cab6df4..3a3f55310632d49c71a7b1b9b883390a74a1e109 100644 --- a/src/ui/RadioCalibration/RadioCalibrationWindow.h +++ b/src/ui/RadioCalibration/RadioCalibrationWindow.h @@ -67,6 +67,8 @@ public: public slots: void setChannel(int ch, float raw, float normalized); + void setChannelRaw(int ch, float raw); + void setChannelScaled(int ch, float normalized); void loadFile(); void saveFile(); void send(); diff --git a/src/ui/SerialConfigurationWindow.h b/src/ui/SerialConfigurationWindow.h index abfa5c138c0ea3ba8ffb5e7c118ff26ab2ce86c5..69b020182a8fd20df7108d74731c27327b69ba04 100644 --- a/src/ui/SerialConfigurationWindow.h +++ b/src/ui/SerialConfigurationWindow.h @@ -62,11 +62,9 @@ public slots: void setLinkName(QString name); void setupPortList(); -protected slots: +protected: void showEvent(QShowEvent* event); void hideEvent(QHideEvent* event); - -protected: bool userConfigured; ///< Switch to detect if current values are user-selected and shouldn't be overriden private: diff --git a/src/ui/SlugsPIDControl.h b/src/ui/SlugsPIDControl.h index d820cee362831457c9ee21baca5d6372f3fcdd48..3ced5eab8bf61088c67850341eefc7ce402081c0 100644 --- a/src/ui/SlugsPIDControl.h +++ b/src/ui/SlugsPIDControl.h @@ -2,7 +2,7 @@ #define SLUGSPIDCONTROL_H #include -#include +#include #include "UASInterface.h" #include "QGCMAVLink.h" #include "SlugsMAV.h" diff --git a/src/ui/WaypointView.cc b/src/ui/WaypointView.cc index b291fc8f24acc884c19c25c00dcdfb2105f4dd11..ca7f981a067b8a8ba2bd41551adce7edc30df990 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(); diff --git a/src/ui/linechart/LinechartPlot.cc b/src/ui/linechart/LinechartPlot.cc index 3a7cb0183279d73dc87ff51d98e2f9bea52e1f3c..9532b1ce970f1dd06f00fb034207bdb26ea2d7e0 100644 --- a/src/ui/linechart/LinechartPlot.cc +++ b/src/ui/linechart/LinechartPlot.cc @@ -43,6 +43,8 @@ This file is part of the PIXHAWK project #include #include +#include "QGC.h" + /** * @brief The default constructor * @@ -55,7 +57,7 @@ maxTime(QUINT64_MIN), maxInterval(MAX_STORAGE_INTERVAL), timeScaleStep(DEFAULT_SCALE_INTERVAL), // 10 seconds automaticScrollActive(false), -m_active(true), +m_active(false), m_groundTime(true), d_data(NULL), d_curve(NULL) @@ -142,7 +144,7 @@ d_curve(NULL) // Start QTimer for plot update updateTimer = new QTimer(this); connect(updateTimer, SIGNAL(timeout()), this, SLOT(paintRealtime())); - updateTimer->start(DEFAULT_REFRESH_RATE); + //updateTimer->start(DEFAULT_REFRESH_RATE); // QwtPlot::setAutoReplot(); @@ -155,6 +157,18 @@ LinechartPlot::~LinechartPlot() removeAllData(); } +void LinechartPlot::showEvent(QShowEvent* event) +{ + Q_UNUSED(event); + updateTimer->start(DEFAULT_REFRESH_RATE); +} + +void LinechartPlot::hideEvent(QHideEvent* event) +{ + Q_UNUSED(event); + updateTimer->stop(); +} + int LinechartPlot::getPlotId() { return this->plotid; @@ -184,6 +198,14 @@ double LinechartPlot::getMedian(QString id) return data.value(id)->getMedian(); } +/** + * @param id curve identifier + */ +double LinechartPlot::getVariance(QString id) +{ + return data.value(id)->getVariance(); +} + int LinechartPlot::getAverageWindow() { return averageWindowSize; @@ -272,6 +294,14 @@ void LinechartPlot::enforceGroundTime(bool enforce) m_groundTime = enforce; } +/** + * @return True if the data points are stamped with the packet receive time + */ +bool LinechartPlot::groundTime() +{ + return m_groundTime; +} + void LinechartPlot::addCurve(QString id) { QColor currentColor = getNextColor(); @@ -454,6 +484,23 @@ bool LinechartPlot::isVisible(QString id) return curves.value(id)->isVisible(); } +/** + * @return The visibility, true if it is visible, false otherwise + **/ +bool LinechartPlot::anyCurveVisible() +{ + bool visible = false; + foreach (QString key, curves.keys()) + { + if (curves.value(key)->isVisible()) + { + visible = true; + } + } + + return visible; +} + /** * @brief Allows to block interference of the automatic scrolling with user interaction * When the plot is updated very fast (at 1 ms for example) with new data, it might @@ -580,6 +627,9 @@ void LinechartPlot::paintRealtime() { if (m_active) { +#if (QGC_EVENTLOOP_DEBUG) + qDebug() << "EVENTLOOP:" << __FILE__ << __LINE__; +#endif // Update plot window value to new max time if the last time was also the max time windowLock.lock(); if (automaticScrollActive) @@ -701,8 +751,9 @@ TimeSeriesData::TimeSeriesData(QwtPlot* plot, QString friendlyName, quint64 plot maxValue(DBL_MIN), zeroValue(0), count(0), - mean(0.00), - median(0.00), + mean(0.0), + median(0.0), + variance(0.0), averageWindow(50) { this->plot = plot; @@ -760,6 +811,14 @@ void TimeSeriesData::append(quint64 ms, double value) medianList.append(this->value[count-i]); } mean = mean / static_cast(qMin(averageWindow,static_cast(count))); + + this->variance = 0; + for (unsigned int i = 0; (i < averageWindow) && (((int)count - (int)i) >= 0); ++i) + { + this->variance += (this->value[count-i] - mean) * (this->value[count-i] - mean); + } + this->variance = this->variance / static_cast(qMin(averageWindow,static_cast(count))); + qSort(medianList); if (medianList.count() > 2) @@ -859,6 +918,14 @@ double TimeSeriesData::getMedian() return median; } +/** + * @return the variance + */ +double TimeSeriesData::getVariance() +{ + return variance; +} + double TimeSeriesData::getCurrentValue() { return lastValue; diff --git a/src/ui/linechart/LinechartPlot.h b/src/ui/linechart/LinechartPlot.h index 138df19195ddcf693e6cf351b4a10a8a45f59264..a180135219ec40c4dc732889340b74cad98089bc 100644 --- a/src/ui/linechart/LinechartPlot.h +++ b/src/ui/linechart/LinechartPlot.h @@ -132,6 +132,8 @@ public: double getMean(); /** @brief Get the short-term median */ double getMedian(); + /** @brief Get the short-term variance */ + double getVariance(); /** @brief Get the current value */ double getCurrentValue(); void setZeroValue(double zeroValue); @@ -166,6 +168,7 @@ private: QwtArray value; double mean; double median; + double variance; unsigned int averageWindow; QwtArray outputMs; QwtArray outputValue; @@ -190,6 +193,8 @@ public: QList getCurves(); bool isVisible(QString id); + /** @brief Check if any curve is visible */ + bool anyCurveVisible(); int getPlotId(); /** @brief Get the number of values to average over */ @@ -206,6 +211,8 @@ public: double getMean(QString id); /** @brief Get the short-term median of a curve */ double getMedian(QString id); + /** @brief Get the short-term variance of a curve */ + double getVariance(QString id); /** @brief Get the last inserted value */ double getCurrentValue(QString id); @@ -214,8 +221,8 @@ public: static const int SCALE_LOGARITHMIC = 2; static const int DEFAULT_REFRESH_RATE = 50; ///< The default refresh rate is 25 Hz / every 100 ms - static const int DEFAULT_PLOT_INTERVAL = 1000 * 12; ///< The default plot interval is 15 seconds - static const int DEFAULT_SCALE_INTERVAL = 1000 * 5; + static const int DEFAULT_PLOT_INTERVAL = 1000 * 8; ///< The default plot interval is 15 seconds + static const int DEFAULT_SCALE_INTERVAL = 1000 * 8; public slots: void setRefreshRate(int ms); @@ -243,6 +250,8 @@ public slots: /** @brief Enforce the use of the receive timestamp */ void enforceGroundTime(bool enforce); + /** @brief Check if the receive timestamp is enforced */ + bool groundTime(); // General interaction void setWindowPosition(quint64 end); @@ -301,6 +310,8 @@ protected: // Methods void addCurve(QString id); QColor getNextColor(); + void showEvent(QShowEvent* event); + void hideEvent(QHideEvent* event); private: TimeSeriesData* d_data; diff --git a/src/ui/linechart/LinechartWidget.cc b/src/ui/linechart/LinechartWidget.cc index 03153e9acb6537b96bf75b740521664f92022058..d1703573e3fc178fa9c7db3b8276e4281aeb21e6 100644 --- a/src/ui/linechart/LinechartWidget.cc +++ b/src/ui/linechart/LinechartWidget.cc @@ -48,6 +48,7 @@ This file is part of the PIXHAWK project #include "LinechartWidget.h" #include "LinechartPlot.h" #include "LogCompressor.h" +#include "QGC.h" #include "MG.h" @@ -62,6 +63,7 @@ listedCurves(new QList()), curveLabels(new QMap()), curveMeans(new QMap()), curveMedians(new QMap()), +curveVariances(new QMap()), curveMenu(new QMenu(this)), logFile(new QFile()), logindex(1), @@ -79,16 +81,52 @@ updateTimer(new QTimer()) curvesWidgetLayout->setMargin(2); curvesWidgetLayout->setSpacing(4); curvesWidgetLayout->setSizeConstraint(QLayout::SetMinimumSize); + curvesWidgetLayout->setAlignment(Qt::AlignTop); curvesWidget->setLayout(curvesWidgetLayout); + // Create curve list headings + QWidget* form = new QWidget(this); + QHBoxLayout *horizontalLayout; + QLabel* label; + QLabel* value; + QLabel* mean; + QLabel* variance; + form->setAutoFillBackground(false); + horizontalLayout = new QHBoxLayout(form); + horizontalLayout->setSpacing(5); + horizontalLayout->setMargin(0); + horizontalLayout->setSizeConstraint(QLayout::SetMinimumSize); + + //horizontalLayout->addWidget(checkBox); + + label = new QLabel(form); + label->setText("Name"); + horizontalLayout->addWidget(label); + + // Value + value = new QLabel(form); + value->setText("Val"); + horizontalLayout->addWidget(value); + + // Mean + mean = new QLabel(form); + mean->setText("Mean"); + horizontalLayout->addWidget(mean); + + // Variance + variance = new QLabel(form); + variance->setText("Variance"); + horizontalLayout->addWidget(variance); + curvesWidgetLayout->addWidget(form); + // Add and customize plot elements (right side) // Create the layout createLayout(); // Add the last actions - connect(this, SIGNAL(plotWindowPositionUpdated(int)), scrollbar, SLOT(setValue(int))); - connect(scrollbar, SIGNAL(sliderMoved(int)), this, SLOT(setPlotWindowPosition(int))); + //connect(this, SIGNAL(plotWindowPositionUpdated(int)), scrollbar, SLOT(setValue(int))); + //connect(scrollbar, SIGNAL(sliderMoved(int)), this, SLOT(setPlotWindowPosition(int))); updateTimer->setInterval(300); connect(updateTimer, SIGNAL(timeout()), this, SLOT(refresh())); @@ -129,6 +167,8 @@ void LinechartWidget::createLayout() scalingLinearButton = createButton(this); scalingLinearButton->setDefaultAction(setScalingLinear); scalingLinearButton->setCheckable(true); + scalingLinearButton->setToolTip(tr("Set linear scale for Y axis")); + scalingLinearButton->setWhatsThis(tr("Set linear scale for Y axis")); layout->addWidget(scalingLinearButton, 1, 0); layout->setColumnStretch(0, 0); @@ -136,13 +176,18 @@ void LinechartWidget::createLayout() scalingLogButton = createButton(this); scalingLogButton->setDefaultAction(setScalingLogarithmic); scalingLogButton->setCheckable(true); + scalingLogButton->setToolTip(tr("Set logarithmic scale for Y axis")); + scalingLogButton->setWhatsThis(tr("Set logarithmic scale for Y axis")); layout->addWidget(scalingLogButton, 1, 1); layout->setColumnStretch(1, 0); // Averaging spin box averageSpinBox = new QSpinBox(this); - averageSpinBox->setValue(200); + averageSpinBox->setToolTip(tr("Sliding window size to calculate mean and variance")); + averageSpinBox->setWhatsThis(tr("Sliding window size to calculate mean and variance")); averageSpinBox->setMinimum(2); + averageSpinBox->setValue(200); + setAverageWindow(200); averageSpinBox->setMaximum(9999); layout->addWidget(averageSpinBox, 1, 2); layout->setColumnStretch(2, 0); @@ -150,6 +195,8 @@ void LinechartWidget::createLayout() // Log Button logButton = new QToolButton(this); + logButton->setToolTip(tr("Start to log curve data into a CSV or TXT file")); + logButton->setWhatsThis(tr("Start to log curve data into a CSV or TXT file")); logButton->setText(tr("Start Logging")); layout->addWidget(logButton, 1, 3); layout->setColumnStretch(3, 0); @@ -159,6 +206,8 @@ void LinechartWidget::createLayout() QToolButton* timeButton = new QToolButton(this); timeButton->setText(tr("Ground Time")); timeButton->setCheckable(true); + timeButton->setToolTip(tr("Overwrite timestamp of data from vehicle with ground receive time. Helps if the plots are not visible because of missing or invalid onboard time.")); + timeButton->setWhatsThis(tr("Overwrite timestamp of data from vehicle with ground receive time. Helps if the plots are not visible because of missing or invalid onboard time.")); bool gTimeDefault = true; if (activePlot) activePlot->enforceGroundTime(gTimeDefault); timeButton->setChecked(gTimeDefault); @@ -167,18 +216,18 @@ void LinechartWidget::createLayout() connect(timeButton, SIGNAL(clicked(bool)), activePlot, SLOT(enforceGroundTime(bool))); // Create the scroll bar - scrollbar = new QScrollBar(Qt::Horizontal, ui.diagramGroupBox); - scrollbar->setMinimum(MIN_TIME_SCROLLBAR_VALUE); - scrollbar->setMaximum(MAX_TIME_SCROLLBAR_VALUE); - scrollbar->setPageStep(PAGESTEP_TIME_SCROLLBAR_VALUE); + //scrollbar = new QScrollBar(Qt::Horizontal, ui.diagramGroupBox); + //scrollbar->setMinimum(MIN_TIME_SCROLLBAR_VALUE); + //scrollbar->setMaximum(MAX_TIME_SCROLLBAR_VALUE); + //scrollbar->setPageStep(PAGESTEP_TIME_SCROLLBAR_VALUE); // Set scrollbar to maximum and disable it - scrollbar->setValue(MIN_TIME_SCROLLBAR_VALUE); - scrollbar->setDisabled(true); + //scrollbar->setValue(MIN_TIME_SCROLLBAR_VALUE); + //scrollbar->setDisabled(true); // scrollbar->setFixedHeight(20); // Add scroll bar to layout and make sure it gets all available space - layout->addWidget(scrollbar, 1, 5); + //layout->addWidget(scrollbar, 1, 5); layout->setColumnStretch(5, 10); ui.diagramGroupBox->setLayout(layout); @@ -225,7 +274,18 @@ void LinechartWidget::appendData(int uasId, QString curve, double value, quint64 { if (activePlot->isVisible(curve)) { - logFile->write(QString(QString::number(usec) + "\t" + QString::number(uasId) + "\t" + curve + "\t" + QString::number(value) + "\n").toLatin1()); + quint64 time = 0; + // Adjust time + if (activePlot->groundTime()) + { + time = QGC::groundTimeUsecs() - logStartTime; + } + else + { + time = usec - logStartTime; + } + + logFile->write(QString(QString::number(time) + "\t" + QString::number(uasId) + "\t" + curve + "\t" + QString::number(value) + "\n").toLatin1()); logFile->flush(); } } @@ -256,19 +316,47 @@ void LinechartWidget::refresh() // str.sprintf("%+.2f", activePlot->getMedian(k.key())); // k.value()->setText(str); // } + QMap::iterator l; + for (l = curveVariances->begin(); l != curveVariances->end(); ++l) + { + // Variance + str.sprintf("%+.5f", activePlot->getVariance(l.key())); + l.value()->setText(str); + } } void LinechartWidget::startLogging() { - // Let user select the log file name - QDate date(QDate::currentDate()); - // QString("./pixhawk-log-" + date.toString("yyyy-MM-dd") + "-" + QString::number(logindex) + ".log") - QString fileName = QFileDialog::getSaveFileName(this, tr("Specify log file name"), QDesktopServices::storageLocation(QDesktopServices::DesktopLocation), tr("Logfile (*.txt, *.csv);;")); // Store reference to file // Append correct file ending if needed bool abort = false; - while (!(fileName.endsWith(".txt") || fileName.endsWith(".csv"))) + + // Check if any curve is enabled + if (!activePlot->anyCurveVisible()) + { + QMessageBox msgBox; + msgBox.setIcon(QMessageBox::Critical); + msgBox.setText("No curves selected for logging."); + msgBox.setInformativeText("Please check all curves you want to log. Currently no data would be logged, aborting the logging."); + msgBox.setStandardButtons(QMessageBox::Ok); + msgBox.setDefaultButton(QMessageBox::Ok); + msgBox.exec(); + return; + } + + // Let user select the log file name + QDate date(QDate::currentDate()); + // QString("./pixhawk-log-" + date.toString("yyyy-MM-dd") + "-" + QString::number(logindex) + ".log") + QString fileName = QFileDialog::getSaveFileName(this, tr("Specify log file name"), QDesktopServices::storageLocation(QDesktopServices::DesktopLocation), tr("Logfile (*.csv, *.txt);;")); + + if (!fileName.contains(".")) + { + // .csv is default extension + fileName.append(".csv"); + } + + while (!(fileName.endsWith(".txt") || fileName.endsWith(".csv")) && !abort) { QMessageBox msgBox; msgBox.setIcon(QMessageBox::Critical); @@ -292,6 +380,7 @@ void LinechartWidget::startLogging() if (logFile->open(QIODevice::WriteOnly | QIODevice::Text)) { logging = true; + logStartTime = QGC::groundTimeUsecs(); logindex++; logButton->setText(tr("Stop logging")); disconnect(logButton, SIGNAL(clicked()), this, SLOT(startLogging())); @@ -354,6 +443,7 @@ QWidget* LinechartWidget::createCurveItem(QString curve) QLabel* label; QLabel* value; QLabel* mean; + QLabel* variance; form->setAutoFillBackground(false); horizontalLayout = new QHBoxLayout(form); horizontalLayout->setSpacing(5); @@ -363,6 +453,8 @@ QWidget* LinechartWidget::createCurveItem(QString curve) checkBox = new QCheckBox(form); checkBox->setCheckable(true); checkBox->setObjectName(curve); + checkBox->setToolTip(tr("Enable the curve in the graph window")); + checkBox->setWhatsThis(tr("Enable the curve in the graph window")); horizontalLayout->addWidget(checkBox); @@ -388,12 +480,16 @@ QWidget* LinechartWidget::createCurveItem(QString curve) // Value value = new QLabel(form); value->setNum(0.00); + value->setToolTip(tr("Current value of ") + curve); + value->setWhatsThis(tr("Current value of ") + curve); curveLabels->insert(curve, value); horizontalLayout->addWidget(value); // Mean mean = new QLabel(form); mean->setNum(0.00); + mean->setToolTip(tr("Arithmetic mean of ") + curve); + mean->setWhatsThis(tr("Arithmetic mean of ") + curve); curveMeans->insert(curve, mean); horizontalLayout->addWidget(mean); @@ -403,6 +499,14 @@ QWidget* LinechartWidget::createCurveItem(QString curve) // curveMedians->insert(curve, median); // horizontalLayout->addWidget(median); + // Variance + variance = new QLabel(form); + variance->setNum(0.00); + variance->setToolTip(tr("Variance of ") + curve); + variance->setWhatsThis(tr("Variance of ") + curve); + curveVariances->insert(curve, variance); + horizontalLayout->addWidget(variance); + /* Color picker QColor color = QColorDialog::getColor(Qt::green, this); if (color.isValid()) { @@ -419,6 +523,7 @@ QWidget* LinechartWidget::createCurveItem(QString curve) horizontalLayout->setStretchFactor(value, 50); horizontalLayout->setStretchFactor(mean, 50); // horizontalLayout->setStretchFactor(median, 50); + horizontalLayout->setStretchFactor(variance, 50); // Connect actions QObject::connect(checkBox, SIGNAL(clicked(bool)), this, SLOT(takeButtonClick(bool))); @@ -447,7 +552,13 @@ void LinechartWidget::removeCurve(QString curve) void LinechartWidget::showEvent(QShowEvent* event) { Q_UNUSED(event); - setActive(isVisible()); + setActive(true); +} + +void LinechartWidget::hideEvent(QHideEvent* event) +{ + Q_UNUSED(event); + setActive(false); } void LinechartWidget::setActive(bool active) @@ -528,14 +639,14 @@ void LinechartWidget::setPlotWindowPosition(quint64 position) { // A relative position makes only sense if the plot is filled if(activePlot->getDataInterval() > activePlot->getPlotInterval()) { //TODO @todo Implement the scrollbar enabling in a more elegant way - scrollbar->setDisabled(false); + //scrollbar->setDisabled(false); quint64 scrollInterval = position - activePlot->getMinTime() - activePlot->getPlotInterval(); pos = (static_cast(scrollInterval) / (activePlot->getDataInterval() - activePlot->getPlotInterval())); } else { - scrollbar->setDisabled(true); + //scrollbar->setDisabled(true); pos = 1; } plotWindowLock.unlock(); @@ -551,7 +662,8 @@ void LinechartWidget::setPlotWindowPosition(quint64 position) { * * @param interval The time interval to plot **/ -void LinechartWidget::setPlotInterval(quint64 interval) { +void LinechartWidget::setPlotInterval(quint64 interval) +{ activePlot->setPlotInterval(interval); } @@ -562,7 +674,8 @@ void LinechartWidget::setPlotInterval(quint64 interval) { * * @param checked The visibility of the curve: true to display the curve, false otherwise **/ -void LinechartWidget::takeButtonClick(bool checked) { +void LinechartWidget::takeButtonClick(bool checked) +{ QCheckBox* button = qobject_cast(QObject::sender()); @@ -579,7 +692,8 @@ void LinechartWidget::takeButtonClick(bool checked) { * @param text The button text * @param parent The parent object (to ensure that the memory is freed after the deletion of the button) **/ -QToolButton* LinechartWidget::createButton(QWidget* parent) { +QToolButton* LinechartWidget::createButton(QWidget* parent) +{ QToolButton* button = new QToolButton(parent); button->setMinimumSize(QSize(20, 20)); button->setMaximumSize(60, 20); diff --git a/src/ui/linechart/LinechartWidget.h b/src/ui/linechart/LinechartWidget.h index 850b0aaa9f64caedc05d234b8799b110000b74a6..d14269c6dd472779d1dc287904f6a566fb756f76 100644 --- a/src/ui/linechart/LinechartWidget.h +++ b/src/ui/linechart/LinechartWidget.h @@ -77,8 +77,10 @@ public slots: void setPlotWindowPosition(int scrollBarValue); void setPlotWindowPosition(quint64 position); void setPlotInterval(quint64 interval); - /** @brief Override base class show */ - virtual void showEvent(QShowEvent* event); + /** @brief Start automatic updates once visible */ + void showEvent(QShowEvent* event); + /** @brief Stop automatic updates once hidden */ + void hideEvent(QHideEvent* event); void setActive(bool active); /** @brief Set the number of values to average over */ void setAverageWindow(int windowSize); @@ -90,7 +92,6 @@ public slots: void refresh(); protected: - void addCurveToList(QString curve); void removeCurveFromList(QString curve); QToolButton* createButton(QWidget* parent); @@ -108,6 +109,7 @@ protected: QMap* curveLabels; ///< References to the curve labels QMap* curveMeans; ///< References to the curve means QMap* curveMedians; ///< References to the curve medians + QMap* curveVariances; ///< References to the curve variances QWidget* curvesWidget; ///< The QWidget containing the curve selection button QVBoxLayout* curvesWidgetLayout; ///< The layout for the curvesWidget QWidget @@ -130,6 +132,7 @@ protected: bool logging; QTimer* updateTimer; LogCompressor* compressor; + quint64 logStartTime; static const int MAX_CURVE_MENUITEM_NUMBER = 8; static const int PAGESTEP_TIME_SCROLLBAR_VALUE = (MAX_TIME_SCROLLBAR_VALUE - MIN_TIME_SCROLLBAR_VALUE) / 10; diff --git a/src/ui/linechart/Linecharts.cc b/src/ui/linechart/Linecharts.cc index b47ea71a7f5a32b0d2281b57f2a0a166d8378ee5..aba663f58159953e6962d899abe421c7defad9bc 100644 --- a/src/ui/linechart/Linecharts.cc +++ b/src/ui/linechart/Linecharts.cc @@ -1,41 +1,69 @@ +#include + #include "Linecharts.h" #include "UASManager.h" +#include "MainWindow.h" + Linecharts::Linecharts(QWidget *parent) : QStackedWidget(parent), plots(), active(true) { - this->setVisible(false); - // Get current MAV list - QList systems = UASManager::instance()->getUASList(); + 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))); + // 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))); + connect(this, SIGNAL(logfileWritten(QString)), + MainWindow::instance(), SLOT(loadDataView(QString))); } - -void Linecharts::setActive(bool active) +void Linecharts::showEvent(QShowEvent* event) { - this->active = active; - QWidget* prevWidget = currentWidget(); - if (prevWidget) + // React only to internal (pre-display) + // events + Q_UNUSED(event) { - LinechartWidget* chart = dynamic_cast(prevWidget); - if (chart) + QWidget* prevWidget = currentWidget(); + if (prevWidget) { - chart->setActive(active); + LinechartWidget* chart = dynamic_cast(prevWidget); + if (chart) + { + this->active = true; + chart->setActive(true); + } } } } +void Linecharts::hideEvent(QHideEvent* event) +{ + // React only to internal (pre-display) + // events + Q_UNUSED(event) + { + QWidget* prevWidget = currentWidget(); + if (prevWidget) + { + LinechartWidget* chart = dynamic_cast(prevWidget); + if (chart) + { + this->active = false; + chart->setActive(false); + } + } + } +} void Linecharts::selectSystem(int systemid) { diff --git a/src/ui/linechart/Linecharts.h b/src/ui/linechart/Linecharts.h index 8bf9d602e8d705d9f03a3ddbd3460e935ee75489..99fb44e9f71630b398162fec025c872baf2e0da0 100644 --- a/src/ui/linechart/Linecharts.h +++ b/src/ui/linechart/Linecharts.h @@ -18,8 +18,6 @@ signals: void logfileWritten(QString fileName); public slots: - /** @brief Set all plots active/inactive */ - void setActive(bool active); /** @brief Select plot for one system */ void selectSystem(int systemid); /** @brief Add a new system to the list of plots */ @@ -28,6 +26,11 @@ public slots: protected: QMap plots; bool active; + /** @brief Start updating widget */ + void showEvent(QShowEvent* event); + /** @brief Stop updating widget */ + void hideEvent(QHideEvent* event); + }; #endif // LINECHARTS_H diff --git a/src/ui/map3D/Pixhawk3DWidget.cc b/src/ui/map3D/Pixhawk3DWidget.cc index d0a57960ba097791fc07acef4e1e24e40038a719..fb18f3365899cf6632fa80a4457b312cf9f89a6f 100644 --- a/src/ui/map3D/Pixhawk3DWidget.cc +++ b/src/ui/map3D/Pixhawk3DWidget.cc @@ -393,6 +393,15 @@ Pixhawk3DWidget::findVehicleModels(void) // add Pixhawk Bravo model nodes.push_back(PixhawkCheetahGeode::instance()); + // add sphere of 0.05m radius + osg::ref_ptr sphere = new osg::Sphere(osg::Vec3f(0.0f, 0.0f, 0.0f), 0.05f); + osg::ref_ptr sphereDrawable = new osg::ShapeDrawable(sphere); + sphereDrawable->setColor(osg::Vec4f(0.5f, 0.0f, 0.5f, 1.0f)); + osg::ref_ptr sphereGeode = new osg::Geode; + sphereGeode->addDrawable(sphereDrawable); + sphereGeode->setName("Sphere (0.1m)"); + nodes.push_back(sphereGeode); + // add all other models in folder for (int i = 0; i < files.size(); ++i) { diff --git a/src/ui/map3D/Q3DWidget.cc b/src/ui/map3D/Q3DWidget.cc index 91e1ad093f29ff235f904dbabefed8a71aa0021d..8e3f3b4988e86b75c8c21ed388cd133f2f68251a 100644 --- a/src/ui/map3D/Q3DWidget.cc +++ b/src/ui/map3D/Q3DWidget.cc @@ -30,6 +30,7 @@ This file is part of the QGROUNDCONTROL project */ #include "Q3DWidget.h" +#include "QGC.h" #include #include @@ -48,6 +49,7 @@ Q3DWidget::Q3DWidget(QWidget* parent) , robotAttitude(new osg::PositionAttitudeTransform()) , hudGroup(new osg::Switch()) , hudProjectionMatrix(new osg::Projection) + , fps(30.0f) { // set initial camera parameters cameraParams.minZoomRange = 2.0f; @@ -70,6 +72,8 @@ Q3DWidget::~Q3DWidget() void Q3DWidget::init(float fps) { + this->fps = fps; + getCamera()->setGraphicsContext(osgGW); // manually specify near and far clip planes @@ -102,9 +106,25 @@ Q3DWidget::init(float fps) cameraManipulator->setDistance(cameraParams.minZoomRange * 2.0); connect(&timer, SIGNAL(timeout()), this, SLOT(redraw())); + // DO NOT START TIMER IN INITIALIZATION! IT IS STARTED IN THE SHOW EVENT +} + +void Q3DWidget::showEvent(QShowEvent* event) +{ + // React only to internal (pre/post-display) + // events + Q_UNUSED(event) timer.start(static_cast(floorf(1000.0f / fps))); } +void Q3DWidget::hideEvent(QHideEvent* event) +{ + // React only to internal (pre/post-display) + // events + Q_UNUSED(event) + timer.stop(); +} + osg::ref_ptr Q3DWidget::createRobot(void) { @@ -243,6 +263,9 @@ Q3DWidget::getGlobalCursorPosition(int32_t cursorX, int32_t cursorY, double z) void Q3DWidget::redraw(void) { +#if (QGC_EVENTLOOP_DEBUG) + qDebug() << "EVENTLOOP:" << __FILE__ << __LINE__; +#endif updateGL(); } diff --git a/src/ui/map3D/Q3DWidget.h b/src/ui/map3D/Q3DWidget.h index 62639508b94ad9eb2ccce225ef2a535f61bdfbe7..3ba2d6f233dd345240c0bd8b8035d0e67cca14fc 100644 --- a/src/ui/map3D/Q3DWidget.h +++ b/src/ui/map3D/Q3DWidget.h @@ -121,6 +121,12 @@ protected slots: void redraw(void); protected: + + /** @brief Start widget updating */ + void showEvent(QShowEvent* event); + /** @brief Stop widget updating */ + void hideEvent(QHideEvent* event); + /** * @brief Get base robot geode. * @return Smart pointer to the geode. @@ -253,6 +259,7 @@ protected: }; CameraParams cameraParams; /**< Struct representing camera parameters. */ + float fps; }; #endif // Q3DWIDGET_H diff --git a/src/ui/map3D/QGCGoogleEarthView.cc b/src/ui/map3D/QGCGoogleEarthView.cc index 5762082ea545f1722c2ed6a7cbaf9534fdadd7a9..72816511b51073b68689845313c6ecd11cf29f83 100644 --- a/src/ui/map3D/QGCGoogleEarthView.cc +++ b/src/ui/map3D/QGCGoogleEarthView.cc @@ -1,50 +1,66 @@ -#include -#include +#include +#include +#include +#include #include +#include "UASManager.h" -#include "QGCGoogleEarthView.h" +#ifdef Q_OS_MAC +#include +#include #include "QGCWebPage.h" -#include "UASManager.h" -#include "ui_QGCGoogleEarthControls.h" -#if (defined Q_OS_WIN) && !(defined __MINGW32__) -#include "ui_QGCGoogleEarthViewWin.h" -#else -#include "ui_QGCGoogleEarthView.h" #endif +#include "QGC.h" +#include "ui_QGCGoogleEarthView.h" +#include "QGCGoogleEarthView.h" + +#define QGCGOOGLEEARTHVIEWSETTINGS QString("GoogleEarthViewSettings_") + QGCGoogleEarthView::QGCGoogleEarthView(QWidget *parent) : QWidget(parent), updateTimer(new QTimer(this)), - refreshRateMs(200), + refreshRateMs(80), mav(NULL), followCamera(true), trailEnabled(true), webViewInitialized(false), + gEarthInitialized(false), #if (defined Q_OS_MAC) webViewMac(new QWebView(this)), #endif -#if (defined Q_OS_WIN) & !(defined __MINGW32__) +#ifdef _MSC_VER webViewWin(new QGCWebAxWidget(this)), +#endif +#if (defined _MSC_VER) + ui(new Ui::QGCGoogleEarthView) #else ui(new Ui::QGCGoogleEarthView) #endif { -#if (defined Q_OS_WIN) & !(defined __MINGW32__) +#ifdef _MSC_VER // Create layout and attach webViewWin #else #endif + // Load settings + QSettings settings; + followCamera = settings.value(QGCGOOGLEEARTHVIEWSETTINGS + "follow", followCamera).toBool(); + trailEnabled = settings.value(QGCGOOGLEEARTHVIEWSETTINGS + "trail", trailEnabled).toBool(); + ui->setupUi(this); #if (defined Q_OS_MAC) ui->webViewLayout->addWidget(webViewMac); + connect(webViewMac, SIGNAL(loadFinished(bool)), this, SLOT(initializeGoogleEarth(bool))); +#endif + +#ifdef _MSC_VER + ui->webViewLayout->addWidget(webViewWin); #endif -#if ((defined Q_OS_MAC) | ((defined Q_OS_WIN) & !(defined __MINGW32__))) connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*))); connect(updateTimer, SIGNAL(timeout()), this, SLOT(updateState())); - updateTimer->start(refreshRateMs); -#endif // Follow checkbox ui->followAirplaneCheckbox->setChecked(followCamera); @@ -54,28 +70,67 @@ QGCGoogleEarthView::QGCGoogleEarthView(QWidget *parent) : ui->trailCheckbox->setChecked(trailEnabled); connect(ui->trailCheckbox, SIGNAL(toggled(bool)), this, SLOT(showTrail(bool))); - // Get list of available 3D models - - // Load HTML file - - // Parse for model links - - // Populate model list + // Go home + connect(ui->goHomeButton, SIGNAL(clicked()), this, SLOT(goHome())); } QGCGoogleEarthView::~QGCGoogleEarthView() { + QSettings settings; + settings.setValue(QGCGOOGLEEARTHVIEWSETTINGS + "follow", followCamera); + settings.setValue(QGCGOOGLEEARTHVIEWSETTINGS + "trail", trailEnabled); + settings.sync(); delete ui; } +void QGCGoogleEarthView::addUAS(UASInterface* uas) +{ +#ifdef Q_OS_MAC + // uasid, type, color (in aarrbbgg format) + webViewMac->page()->currentFrame()->evaluateJavaScript(QString("createAircraft(%1, %2, %3);").arg(uas->getUASID()).arg(uas->getSystemType()).arg(uas->getColor().name().remove(0, 1).prepend("50"))); +#endif +#ifdef _MSC_VER + //if (webViewMac->page()->currentFrame()->evaluateJavaScript("isInitialized();").toBool()) +#endif + + // Automatically receive further position updates + connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64))); +} + void QGCGoogleEarthView::setActiveUAS(UASInterface* uas) { - mav = uas; + if (uas) + { + mav = uas; +#ifdef Q_OS_MAC + if (webViewMac->page()->currentFrame()->evaluateJavaScript("isInitialized();").toBool()) + { + webViewMac->page()->currentFrame()->evaluateJavaScript(QString("setCurrAircraft(%1);").arg(uas->getUASID())); + } +#endif +#ifdef _MSC_VER + //if (webViewMac->page()->currentFrame()->evaluateJavaScript("isInitialized();").toBool()) +#endif + } } -void QGCGoogleEarthView::showTrail(bool state) +void QGCGoogleEarthView::updateGlobalPosition(UASInterface* uas, double lat, double lon, double alt, quint64 usec) { + Q_UNUSED(usec); +#ifdef Q_OS_MAC + webViewMac->page()->currentFrame()->evaluateJavaScript(QString("addTrailPosition(%1, %2, %3, %4);").arg(uas->getUASID()).arg(lat, 0, 'f', 15).arg(lon, 0, 'f', 15).arg(alt, 0, 'f', 15)); + + //qDebug() << QString("addTrailPosition(%1, %2, %3, %4);").arg(uas->getUASID()).arg(lat, 0, 'f', 15).arg(lon, 0, 'f', 15).arg(alt, 0, 'f', 15); + +#endif +#ifdef _MSC_VER + //if (webViewMac->page()->currentFrame()->evaluateJavaScript("isInitialized();").toBool()) +#endif +} +void QGCGoogleEarthView::showTrail(bool state) +{ + ui->trailCheckbox->setChecked(state); } void QGCGoogleEarthView::showWaypoints(bool state) @@ -85,66 +140,137 @@ void QGCGoogleEarthView::showWaypoints(bool state) void QGCGoogleEarthView::follow(bool follow) { + ui->followAirplaneCheckbox->setChecked(follow); followCamera = follow; } -void QGCGoogleEarthView::hide() +void QGCGoogleEarthView::goHome() +{ + // Disable follow and update + follow(false); + updateState(); + // Go to home location +#ifdef Q_OS_MAC + webViewMac->page()->currentFrame()->evaluateJavaScript("goHome();"); +#endif +#ifdef _MSC_VER + webViewWin.dynamicCall("InvokeScript(\"goHome\");"); +#endif +} + +void QGCGoogleEarthView::setHome(double lat, double lon, double alt) { - updateTimer->stop(); - QWidget::hide(); +#ifdef Q_OS_MAC + webViewMac->page()->currentFrame()->evaluateJavaScript(QString("setGCSHome(%1,%2,%3);").arg(lat, 0, 'f', 15).arg(lon, 0, 'f', 15).arg(alt, 0, 'f', 15)); +#endif +#ifdef _MSC_VER + webViewWin.dynamicCall(QString("InvokeScript(\"setGCSHome\", %1, %2, %3);").arg(lat, 0, 'f', 15).arg(lon, 0, 'f', 15).arg(alt, 0, 'f', 15)); +#endif } -void QGCGoogleEarthView::show() +void QGCGoogleEarthView::hideEvent(QHideEvent* event) { - if (!webViewInitialized) + Q_UNUSED(event) updateTimer->stop(); +} + +void QGCGoogleEarthView::showEvent(QShowEvent* event) +{ + // React only to internal (pre-display) + // events + Q_UNUSED(event) { + // Enable widget, initialize on first run + if (!webViewInitialized) + { #if (defined Q_OS_MAC) - webViewMac->setPage(new QGCWebPage(webViewMac)); - webViewMac->settings()->setAttribute(QWebSettings::PluginsEnabled, true); - webViewMac->load(QUrl("earth.html")); + webViewMac->setPage(new QGCWebPage(webViewMac)); + webViewMac->settings()->setAttribute(QWebSettings::PluginsEnabled, true); + webViewMac->load(QUrl("earth.html")); #endif -#if (defined Q_OS_WIN) & !(defined __MINGW32__) - webViewWin->load(QUrl("earth.html")); +#ifdef _MSC_VER + //webViewWin->dynamicCall("GoHome()"); + webViewWin->dynamicCall("Navigate(const QString&)", QApplication::applicationDirPath() + "/earth.html"); #endif - webViewInitialized = true; + + webViewInitialized = true; + // Reloading the webpage, this resets Google Earth + gEarthInitialized = false; + + QTimer::singleShot(1000, this, SLOT(initializeGoogleEarth())); + updateTimer->start(refreshRateMs); + } } - updateTimer->start(); - QWidget::show(); } -void QGCGoogleEarthView::updateState() +void QGCGoogleEarthView::initializeGoogleEarth() { -#ifdef Q_OS_MAC - if (isVisible()) + if (!gEarthInitialized) { - if (webViewMac->page()->currentFrame()->evaluateJavaScript("isInitialized();").toBool()) +#ifdef Q_OS_MAC + if (!webViewMac->page()->currentFrame()->evaluateJavaScript("isInitialized();").toBool()) +#endif +#ifdef _MSC_VER + //if (!webViewMac->page()->currentFrame()->evaluateJavaScript("isInitialized();").toBool()) +#endif { - static bool initialized = false; - if (!initialized) - { - webViewMac->page()->currentFrame()->evaluateJavaScript("setGCSHome(22.679833,8.549444, 470);"); - initialized = true; - } - int uasId = 0; - double lat = 22.679833; - double lon = 8.549444; - double alt = 470.0; + QTimer::singleShot(200, this, SLOT(initializeGoogleEarth())); + } + else + { + // Set home location + setHome(47.3769, 8.549444, 500); + + // Move to home location + goHome(); - float roll = 0.0f; - float pitch = 0.0f; - float yaw = 0.0f; + // Set current UAS + setActiveUAS(mav); - if (mav) + // Add all MAVs + QList mavs = UASManager::instance()->getUASList(); + foreach (UASInterface* mav, mavs) { - uasId = mav->getUASID(); - lat = mav->getLatitude(); - lon = mav->getLongitude(); - alt = mav->getAltitude(); - roll = mav->getRoll(); - pitch = mav->getPitch(); - yaw = mav->getYaw(); + addUAS(mav); } + + // Add any further MAV automatically + connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)), this, SLOT(addUAS(UASInterface*))); + + gEarthInitialized = true; + } + } +} + +void QGCGoogleEarthView::updateState() +{ +#if (QGC_EVENTLOOP_DEBUG) + qDebug() << "EVENTLOOP:" << __FILE__ << __LINE__; +#endif + if (gEarthInitialized) + { + int uasId = 0; + double lat = 47.3769; + double lon = 8.549444; + double alt = 470.0; + + float roll = 0.0f; + float pitch = 0.0f; + float yaw = 0.0f; + + // Update all MAVs + QList mavs = UASManager::instance()->getUASList(); + foreach (UASInterface* mav, mavs) + { + uasId = mav->getUASID(); + lat = mav->getLatitude(); + lon = mav->getLongitude(); + alt = mav->getAltitude(); + roll = mav->getRoll(); + pitch = mav->getPitch(); + yaw = mav->getYaw(); + +#ifdef Q_OS_MAC webViewMac->page()->currentFrame()->evaluateJavaScript(QString("setAircraftPositionAttitude(%1, %2, %3, %4, %6, %7, %8);") .arg(uasId) .arg(lat) @@ -153,14 +279,21 @@ void QGCGoogleEarthView::updateState() .arg(roll) .arg(pitch) .arg(yaw)); +#endif +#ifdef _MSC_VER - if (followCamera) - { - webViewMac->page()->currentFrame()->evaluateJavaScript(QString("updateFollowAircraft()")); - } +#endif } - } + + if (followCamera) + { +#ifdef Q_OS_MAC + webViewMac->page()->currentFrame()->evaluateJavaScript(QString("updateFollowAircraft()")); #endif +#ifdef _MSC_VER +#endif + } + } } diff --git a/src/ui/map3D/QGCGoogleEarthView.h b/src/ui/map3D/QGCGoogleEarthView.h index 8ecf5d8f412c05ef228eef46b649f02169141a38..1366730888b7f521df008fe6be52e33321f9e716 100644 --- a/src/ui/map3D/QGCGoogleEarthView.h +++ b/src/ui/map3D/QGCGoogleEarthView.h @@ -9,18 +9,19 @@ #include #endif -#if (defined Q_OS_WIN) && !(defined __MINGW32__) - QGCWebAxWidget* webViewWin; +#ifdef _MSC_VER #include #include "windows.h" -class WebAxWidget : public QAxWidget +class QGCWebAxWidget : public QAxWidget { public: - WebAxWidget(QWidget* parent = 0, Qt::WindowFlags f = 0) + QGCWebAxWidget(QWidget* parent = 0, Qt::WindowFlags f = 0) : QAxWidget(parent, f) { + // Set web browser control + setControl("{8856F961-340A-11D0-A96B-00C04FD705A2}"); } protected: virtual bool translateKeyEvent(int message, int keycode) const @@ -32,16 +33,15 @@ protected: } }; -#else +#endif + namespace Ui { - class QGCGoogleEarthControls; -#if (defined Q_OS_WIN) && !(defined __MINGW32__) - class QGCGoogleEarthViewWin; +#ifdef _MSC_VER + class QGCGoogleEarthView; #else class QGCGoogleEarthView; #endif } -#endif class QGCGoogleEarthView : public QWidget { @@ -54,18 +54,24 @@ public: public slots: /** @brief Update the internal state. Does not trigger a redraw */ void updateState(); + /** @brief Add a new MAV/UAS to the visualization */ + void addUAS(UASInterface* uas); /** @brief Set the currently selected UAS */ void setActiveUAS(UASInterface* uas); + /** @brief Update the global position */ + void updateGlobalPosition(UASInterface* uas, double lat, double lon, double alt, quint64 usec); /** @brief Show the vehicle trail */ void showTrail(bool state); /** @brief Show the waypoints */ void showWaypoints(bool state); /** @brief Follow the aircraft during flight */ void follow(bool follow); - /** @brief Hide and deactivate */ - void hide(); - /** @brief Show and activate */ - void show(); + /** @brief Go to the home location */ + void goHome(); + /** @brief Set the home location */ + void setHome(double lat, double lon, double alt); + /** @brief Initialize Google Earth */ + void initializeGoogleEarth(); protected: void changeEvent(QEvent *e); @@ -75,17 +81,22 @@ protected: bool followCamera; bool trailEnabled; bool webViewInitialized; -#if (defined Q_OS_WIN) && !(defined __MINGW32__) - WebAxWidget* webViewWin; + bool gEarthInitialized; +#ifdef _MSC_VER + QGCWebAxWidget* webViewWin; #endif #if (defined Q_OS_MAC) QWebView* webViewMac; #endif + /** @brief Start widget updating */ + void showEvent(QShowEvent* event); + /** @brief Stop widget updating */ + void hideEvent(QHideEvent* event); + private: - Ui::QGCGoogleEarthControls* controls; -#if (defined Q_OS_WIN) && !(defined __MINGW32__) - Ui::QGCGoogleEarthViewWin* ui; +#ifdef _MSC_VER + Ui::QGCGoogleEarthView* ui; #else Ui::QGCGoogleEarthView* ui; #endif diff --git a/src/ui/map3D/QGCGoogleEarthViewWin.ui b/src/ui/map3D/QGCGoogleEarthViewWin.ui index 4b7d6a45fb2f7258aed5a05c7df837f1dc0b7b66..6191eaebb2358c8b8934b41f4dd4fc5307842172 100644 --- a/src/ui/map3D/QGCGoogleEarthViewWin.ui +++ b/src/ui/map3D/QGCGoogleEarthViewWin.ui @@ -1,21 +1,19 @@ - - - - - Form - - - - 0 - 0 - 400 - 300 - - - - Form - - - - - + + + QGCGoogleEarthViewWin + + + + 0 + 0 + 400 + 300 + + + + Form + + + + + diff --git a/src/ui/uas/QGCUnconnectedInfoWidget.cc b/src/ui/uas/QGCUnconnectedInfoWidget.cc new file mode 100644 index 0000000000000000000000000000000000000000..dbd9e8e0eb36c0511f14b36adccbe606c0f27706 --- /dev/null +++ b/src/ui/uas/QGCUnconnectedInfoWidget.cc @@ -0,0 +1,46 @@ +#include "QGCUnconnectedInfoWidget.h" +#include "LinkInterface.h" +#include "LinkManager.h" +#include "MAVLinkSimulationLink.h" +#include "MainWindow.h" +#include "ui_QGCUnconnectedInfoWidget.h" + +QGCUnconnectedInfoWidget::QGCUnconnectedInfoWidget(QWidget *parent) : + QWidget(parent), + ui(new Ui::QGCUnconnectedInfoWidget) +{ + ui->setupUi(this); + + connect(ui->simulationButton, SIGNAL(clicked()), this, SLOT(simulate())); + connect(ui->connectButton, SIGNAL(clicked()), this, SLOT(addLink())); +} + +QGCUnconnectedInfoWidget::~QGCUnconnectedInfoWidget() +{ + delete ui; +} + +/** + * @brief Starts the system simulation + */ +void QGCUnconnectedInfoWidget::simulate() +{ + // Try to get reference to MAVLinkSimulationlink + QList links = LinkManager::instance()->getLinks(); + foreach(LinkInterface* link, links) + { + MAVLinkSimulationLink* sim = dynamic_cast(link); + if (sim) + { + sim->connectLink(); + } + } +} + +/** + * @return Opens a "Connect new Link" popup + */ +void QGCUnconnectedInfoWidget::addLink() +{ + MainWindow::instance()->addLink(); +} diff --git a/src/ui/uas/QGCUnconnectedInfoWidget.h b/src/ui/uas/QGCUnconnectedInfoWidget.h new file mode 100644 index 0000000000000000000000000000000000000000..0e758537a1a7c976f4209d3a4e5aa76a6e9018ab --- /dev/null +++ b/src/ui/uas/QGCUnconnectedInfoWidget.h @@ -0,0 +1,37 @@ +#ifndef QGCUNCONNECTEDINFOWIDGET_H +#define QGCUNCONNECTEDINFOWIDGET_H + +#include + +namespace Ui { + class QGCUnconnectedInfoWidget; +} + +/** + * @brief Widget providing initial instructions + * + * This widget provides initial instructions to new users how + * to connect a simulation or a live link to a system to + * QGroundControl. The widget should be automatically be + * unloaded and destroyed once the first MAV/UAS is connected. + * + * @see UASListWidget + */ +class QGCUnconnectedInfoWidget : public QWidget +{ + Q_OBJECT + +public: + explicit QGCUnconnectedInfoWidget(QWidget *parent = 0); + ~QGCUnconnectedInfoWidget(); + + Ui::QGCUnconnectedInfoWidget *ui; + +public slots: + /** @brief Start simulation */ + void simulate(); + /** @brief Add a link */ + void addLink(); +}; + +#endif // QGCUNCONNECTEDINFOWIDGET_H diff --git a/src/ui/uas/QGCUnconnectedInfoWidget.ui b/src/ui/uas/QGCUnconnectedInfoWidget.ui new file mode 100644 index 0000000000000000000000000000000000000000..4eaac71aab3a8d00bbcdcb9fee01101470b6835e --- /dev/null +++ b/src/ui/uas/QGCUnconnectedInfoWidget.ui @@ -0,0 +1,86 @@ + + + QGCUnconnectedInfoWidget + + + + 0 + 0 + 400 + 300 + + + + Form + + + + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + QTextEdit::AutoAll + + + false + + + true + + + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> +<html><head><meta name="qrichtext" content="1" /><style type="text/css"> +p, li { white-space: pre-wrap; } +</style></head><body style=" font-family:'Lucida Grande'; font-size:13pt; font-weight:400; font-style:normal;"> +<table border="0" style="-qt-table-type: root; margin-top:4px; margin-bottom:4px; margin-left:4px; margin-right:4px;"> +<tr> +<td style="border: none;"> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:14pt; font-weight:600;">Unmanned System List</span></p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-family:'Ubuntu'; font-size:6pt; font-weight:600;"></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt; font-weight:600;">No Systems are connected yet.</span><span style=" font-family:'Ubuntu'; font-size:11pt;"> Please either connect a link or use the simulation function to see QGroundControl in action.</span></p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-family:'Ubuntu'; font-size:11pt;"></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt; font-style:italic;">Click on the simulation button below to simulate a micro air vehicle or on the connect link button to connect a serial port link. A UDP link is already open for connections on port </span><span style=" font-family:'Ubuntu'; font-size:11pt; font-weight:600; font-style:italic;">14550</span><span style=" font-family:'Ubuntu'; font-size:11pt; font-style:italic;">.</span></p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-family:'Ubuntu'; font-size:11pt; font-style:italic;"></p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-family:'Ubuntu'; font-size:11pt; font-style:italic;"></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt; font-weight:600;">Communication Link Help:</span></p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-family:'Ubuntu'; font-size:11pt; font-style:italic;"></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">If you encounter communication problems on your link (e.g. no MAV is shown in the list after connecting the link), please check if you receive data on the link using the communication console. Select </span><span style=" font-family:'Ubuntu'; font-size:11pt; font-weight:600;">Tools -&gt; Communication Console</span><span style=" font-family:'Ubuntu'; font-size:11pt;"> to enable it. The console should show incoming traffic and some used bandwidth (e.g. 1.43 kB/s on the indicator).</span></p></td></tr></table></body></html> + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + Simulate MAV + + + + + + + Connect Link + + + + + + + + diff --git a/src/ui/uas/UASListWidget.cc b/src/ui/uas/UASListWidget.cc index e4fb0026fecea3cbf4974d4e12a5fff1e2c7749f..1d6bd7814869004af5a3f511fd7967b6cfadeb67 100644 --- a/src/ui/uas/UASListWidget.cc +++ b/src/ui/uas/UASListWidget.cc @@ -33,12 +33,17 @@ This file is part of the PIXHAWK project #include #include #include +#include #include "MG.h" #include "UASListWidget.h" #include "UASManager.h" #include "UAS.h" #include "UASView.h" +#include "QGCUnconnectedInfoWidget.h" +#include "MainWindow.h" +#include "MAVLinkSimulationLink.h" +#include "LinkManager.h" UASListWidget::UASListWidget(QWidget *parent) : QWidget(parent), m_ui(new Ui::UASList) { @@ -48,6 +53,10 @@ UASListWidget::UASListWidget(QWidget *parent) : QWidget(parent), m_ui(new Ui::UA listLayout->setAlignment(Qt::AlignTop); this->setLayout(listLayout); + // Construct initial widget + uWidget = new QGCUnconnectedInfoWidget(this); + listLayout->addWidget(uWidget); + this->setMinimumWidth(250); uasViews = QMap(); @@ -74,9 +83,11 @@ void UASListWidget::changeEvent(QEvent *e) void UASListWidget::addUAS(UASInterface* uas) { - if (uasViews.size() == 0) + if (uasViews.isEmpty()) { - // TODO Add label when no UAS is present yet. + listLayout->removeWidget(uWidget); + delete uWidget; + uWidget = NULL; } if (!uasViews.contains(uas)) diff --git a/src/ui/uas/UASListWidget.h b/src/ui/uas/UASListWidget.h index 33d0c7b88a88882c28476a1e8e4b627fdee04248..45f0c735edf9a7bd00076f2b667c45a2a9c28d15 100644 --- a/src/ui/uas/UASListWidget.h +++ b/src/ui/uas/UASListWidget.h @@ -36,6 +36,7 @@ This file is part of the QGROUNDCONTROL project #include #include "UASInterface.h" #include "UASView.h" +#include "QGCUnconnectedInfoWidget.h" #include "ui_UASList.h" class UASListWidget : public QWidget @@ -54,6 +55,7 @@ public slots: protected: QMap uasViews; QVBoxLayout* listLayout; + QGCUnconnectedInfoWidget* uWidget; void changeEvent(QEvent *e); private: diff --git a/src/ui/uas/UASView.cc b/src/ui/uas/UASView.cc index dee05898d9eb5aaa05ffaa8dad06e5f3697d6b97..7a3700420b3b9d1c398b8a7cb85c1594b2b2539f 100644 --- a/src/ui/uas/UASView.cc +++ b/src/ui/uas/UASView.cc @@ -1,5 +1,4 @@ /*===================================================================== - PIXHAWK Micro Air Vehicle Flying Robotics Toolkit (c) 2009, 2010 PIXHAWK PROJECT @@ -10,15 +9,15 @@ This file is part of the PIXHAWK project it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. - + PIXHAWK is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. - + You should have received a copy of the GNU General Public License along with PIXHAWK. If not, see . - + ======================================================================*/ /** @@ -33,6 +32,7 @@ This file is part of the PIXHAWK project #include #include +#include "QGC.h" #include "MG.h" #include "UASManager.h" #include "UASView.h" @@ -62,7 +62,7 @@ UASView::UASView(UASInterface* uas, QWidget *parent) : m_ui(new Ui::UASView) { m_ui->setupUi(this); - + // Setup communication //connect(uas, SIGNAL(valueChanged(int,QString,double,quint64)), this, SLOT(receiveValue(int,QString,double,quint64))); connect(uas, SIGNAL(batteryChanged(UASInterface*, double, double, int)), this, SLOT(updateBattery(UASInterface*, double, double, int))); @@ -79,10 +79,10 @@ UASView::UASView(UASInterface* uas, QWidget *parent) : connect(&(uas->getWaypointManager()), SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointUpdated(quint16))); connect(uas, SIGNAL(systemTypeSet(UASInterface*,uint)), this, SLOT(setSystemType(UASInterface*,uint))); connect(UASManager::instance(), SIGNAL(activeUASStatusChanged(UASInterface*,bool)), this, SLOT(updateActiveUAS(UASInterface*,bool))); - + // Setup UAS selection connect(m_ui->uasViewFrame, SIGNAL(clicked(bool)), this, SLOT(setUASasActive(bool))); - + // Setup user interaction connect(m_ui->liftoffButton, SIGNAL(clicked()), uas, SLOT(launch())); connect(m_ui->haltButton, SIGNAL(clicked()), uas, SLOT(halt())); @@ -91,9 +91,9 @@ UASView::UASView(UASInterface* uas, QWidget *parent) : connect(m_ui->abortButton, SIGNAL(clicked()), uas, SLOT(emergencySTOP())); connect(m_ui->killButton, SIGNAL(clicked()), uas, SLOT(emergencyKILL())); connect(m_ui->shutdownButton, SIGNAL(clicked()), uas, SLOT(shutdown())); - + // Set static values - + // Name if (uas->getUASName() == "") { @@ -103,13 +103,12 @@ UASView::UASView(UASInterface* uas, QWidget *parent) : { m_ui->nameLabel->setText(uas->getUASName()); } - + setBackgroundColor(); - + // Heartbeat fade refreshTimer = new QTimer(this); connect(refreshTimer, SIGNAL(timeout()), this, SLOT(refresh())); - refreshTimer->start(updateInterval); } UASView::~UASView() @@ -173,30 +172,55 @@ void UASView::mouseDoubleClickEvent (QMouseEvent * event) void UASView::enterEvent(QEvent* event) { - if (event->MouseMove) emit uasInFocus(uas); + if (event->type() == QEvent::MouseMove) + { + emit uasInFocus(uas); + if (uas != UASManager::instance()->getActiveUAS()) + { + grabMouse(QCursor(Qt::PointingHandCursor)); + } + } qDebug() << __FILE__ << __LINE__ << "IN FOCUS"; + + if (event->type() == QEvent::MouseButtonDblClick) + { + qDebug() << __FILE__ << __LINE__ << "UAS CLICKED!"; + } } void UASView::leaveEvent(QEvent* event) { - if (event->MouseMove) emit uasOutFocus(uas); - qDebug() << __FILE__ << __LINE__ << "OUT OF FOCUS"; + if (event->type() == QEvent::MouseMove) + { + emit uasOutFocus(uas); + releaseMouse(); + } +} + +void UASView::showEvent(QShowEvent* event) +{ + // React only to internal (pre-display) + // events + Q_UNUSED(event); + refreshTimer->start(updateInterval); +} + +void UASView::hideEvent(QHideEvent* event) +{ + // React only to internal (pre-display) + // events + Q_UNUSED(event); + refreshTimer->stop(); } void UASView::receiveHeartbeat(UASInterface* uas) { - if (uas == this->uas) - { - refreshTimer->stop(); - QString colorstyle; - heartbeatColor = QColor(20, 200, 20); - colorstyle = colorstyle.sprintf("QGroupBox { border: 1px solid #EEEEEE; border-radius: 4px; padding: 0px; margin: 0px; background-color: #%02X%02X%02X;}", - heartbeatColor.red(), heartbeatColor.green(), heartbeatColor.blue()); - m_ui->heartbeatIcon->setStyleSheet(colorstyle); - m_ui->heartbeatIcon->setAutoFillBackground(true); - refreshTimer->stop(); - refreshTimer->start(updateInterval); - } + QString colorstyle; + heartbeatColor = QColor(20, 200, 20); + colorstyle = colorstyle.sprintf("QGroupBox { border: 1px solid #EEEEEE; border-radius: 4px; padding: 0px; margin: 0px; background-color: #%02X%02X%02X;}", + heartbeatColor.red(), heartbeatColor.green(), heartbeatColor.blue()); + m_ui->heartbeatIcon->setStyleSheet(colorstyle); + m_ui->heartbeatIcon->setAutoFillBackground(true); } /** @@ -248,8 +272,8 @@ void UASView::setSystemType(UASInterface* uas, unsigned int systemType) m_ui->landButton->hide(); m_ui->shutdownButton->hide(); m_ui->abortButton->hide(); - m_ui->typeButton->setIcon(QIcon(":/images/mavs/groundstation.svg")); - } + m_ui->typeButton->setIcon(QIcon(":/images/mavs/groundstation.svg")); + } break; default: m_ui->typeButton->setIcon(QIcon(":/images/mavs/unknown.svg")); @@ -363,90 +387,93 @@ void UASView::refresh() if (generalUpdateCount == 4) { +#if (QGC_EVENTLOOP_DEBUG) + qDebug() << "EVENTLOOP:" << __FILE__ << __LINE__; +#endif generalUpdateCount = 0; //qDebug() << "UPDATING EVERYTHING"; - // State - m_ui->stateLabel->setText(state); - m_ui->statusTextLabel->setText(stateDesc); - - // Battery - m_ui->batteryBar->setValue(static_cast(this->chargeLevel)); - //m_ui->loadBar->setValue(static_cast(this->load)); - m_ui->thrustBar->setValue(this->thrust); - - // Position - QString position; - position = position.sprintf("%02.2f %02.2f %02.2f m", x, y, z); - m_ui->positionLabel->setText(position); - QString globalPosition; - QString latIndicator; - if (lat > 0) - { - latIndicator = "N"; - } - else - { - latIndicator = "S"; - } - QString lonIndicator; - if (lon > 0) - { - lonIndicator = "E"; - } - else - { - lonIndicator = "W"; - } - globalPosition = globalPosition.sprintf("%02.2f%s %02.2f%s %02.2f m", lon, lonIndicator.toStdString().c_str(), lat, latIndicator.toStdString().c_str(), alt); - m_ui->gpsLabel->setText(globalPosition); + // State + m_ui->stateLabel->setText(state); + m_ui->statusTextLabel->setText(stateDesc); + + // Battery + m_ui->batteryBar->setValue(static_cast(this->chargeLevel)); + //m_ui->loadBar->setValue(static_cast(this->load)); + m_ui->thrustBar->setValue(this->thrust); + + // Position + QString position; + position = position.sprintf("%02.2f %02.2f %02.2f m", x, y, z); + m_ui->positionLabel->setText(position); + QString globalPosition; + QString latIndicator; + if (lat > 0) + { + latIndicator = "N"; + } + else + { + latIndicator = "S"; + } + QString lonIndicator; + if (lon > 0) + { + lonIndicator = "E"; + } + else + { + lonIndicator = "W"; + } + globalPosition = globalPosition.sprintf("%02.2f%s %02.2f%s %02.2f m", lon, lonIndicator.toStdString().c_str(), lat, latIndicator.toStdString().c_str(), alt); + m_ui->gpsLabel->setText(globalPosition); - // Altitude - if (groundDistance == 0 && alt != 0) - { - m_ui->groundDistanceLabel->setText(QString("%1 m").arg(alt)); - } - else - { - m_ui->groundDistanceLabel->setText(QString("%1 m").arg(groundDistance)); - } + // Altitude + if (groundDistance == 0 && alt != 0) + { + m_ui->groundDistanceLabel->setText(QString("%1 m").arg(alt)); + } + else + { + m_ui->groundDistanceLabel->setText(QString("%1 m").arg(groundDistance)); + } - // Speed - QString speed; - speed = speed.sprintf("%02.2f m/s", totalSpeed); - m_ui->speedLabel->setText(speed); + // Speed + QString speed; + speed = speed.sprintf("%02.2f m/s", totalSpeed); + m_ui->speedLabel->setText(speed); - // Thrust - m_ui->thrustBar->setValue(thrust * 100); + // Thrust + m_ui->thrustBar->setValue(thrust * 100); - if(this->timeRemaining > 1 && this->timeRemaining < MG::MAX_FLIGHT_TIME) - { - // Filter output to get a higher stability - static double filterTime = static_cast(this->timeRemaining); - filterTime = 0.8 * filterTime + 0.2 * static_cast(this->timeRemaining); - int sec = static_cast(filterTime - static_cast(filterTime / 60.0f) * 60); + if(this->timeRemaining > 1 && this->timeRemaining < MG::MAX_FLIGHT_TIME) + { + // Filter output to get a higher stability + static double filterTime = static_cast(this->timeRemaining); + filterTime = 0.8 * filterTime + 0.2 * static_cast(this->timeRemaining); + int sec = static_cast(filterTime - static_cast(filterTime / 60.0f) * 60); + int min = static_cast(filterTime / 60); + int hours = static_cast(filterTime - min * 60 - sec); + + QString timeText; + timeText = timeText.sprintf("%02d:%02d:%02d", hours, min, sec); + m_ui->timeRemainingLabel->setText(timeText); + } + else + { + m_ui->timeRemainingLabel->setText(tr("Calculating")); + } + + // Time Elapsed + //QDateTime time = MG::TIME::msecToQDateTime(uas->getUptime()); + + quint64 filterTime = uas->getUptime() / 1000; + int sec = static_cast(filterTime - static_cast(filterTime / 60) * 60); int min = static_cast(filterTime / 60); int hours = static_cast(filterTime - min * 60 - sec); - QString timeText; timeText = timeText.sprintf("%02d:%02d:%02d", hours, min, sec); - m_ui->timeRemainingLabel->setText(timeText); + m_ui->timeElapsedLabel->setText(timeText); } - else - { - m_ui->timeRemainingLabel->setText(tr("Calculating")); - } - - // Time Elapsed - //QDateTime time = MG::TIME::msecToQDateTime(uas->getUptime()); - - quint64 filterTime = uas->getUptime() / 1000; - int sec = static_cast(filterTime - static_cast(filterTime / 60) * 60); - int min = static_cast(filterTime / 60); - int hours = static_cast(filterTime - min * 60 - sec); - QString timeText; - timeText = timeText.sprintf("%02d:%02d:%02d", hours, min, sec); - m_ui->timeElapsedLabel->setText(timeText); -} generalUpdateCount++; // Fade heartbeat icon diff --git a/src/ui/uas/UASView.h b/src/ui/uas/UASView.h index 2d69aa21b37788dc64fa345712bdfdaa9c979a43..74dcc3c963feb5629514d19e682b4acaef5e910f 100644 --- a/src/ui/uas/UASView.h +++ b/src/ui/uas/UASView.h @@ -107,6 +107,10 @@ protected: void enterEvent(QEvent* event); /** @brief Mouse leaves the widget */ void leaveEvent(QEvent* event); + /** @brief Start widget updating */ + void showEvent(QShowEvent* event); + /** @brief Stop widget updating */ + void hideEvent(QHideEvent* event); private: Ui::UASView *m_ui;