diff --git a/data/kinect.cal b/data/kinect.cal index 432c700919205ab5dcd9e5be855272b3898cb628..47c331e938110dde47bbbeb6177dc189d844f14b 100644 --- a/data/kinect.cal +++ b/data/kinect.cal @@ -1,35 +1,37 @@ [rgb] -principal_point\x=313.67245 -principal_point\y=264.06175 -focal_length\x=527.44654 -focal_length\y=527.40652 -distortion\k1=0.20780 -distortion\k2=-0.34320 -distortion\k3=0.00139 -distortion\k4=0.00061 -distortion\k5=0.00000 +principal_point\x=314.70228964 +principal_point\y=264.30478827 +focal_length\x=527.91246131 +focal_length\y=527.91246131 +distortion\k1=0.20496745 +distortion\k2=-0.36341243 +distortion\k3=0.00000000 +distortion\k4=0.00000000 +distortion\k5=0.00000000 [depth] -principal_point\x=313.23400 -principal_point\y=246.13447 -focal_length\x=587.62150 -focal_length\y=587.51184 -distortion\k1=0.01063 -distortion\k2=-0.04479 -distortion\k3=-0.00073 -distortion\k4=0.00081 -distortion\k5=0.00000 +principal_point\x=311.88621344 +principal_point\y=247.63447078 +focal_length\x=593.89813561 +focal_length\y=593.89813561 +distortion\k1=0.00000000 +distortion\k2=0.00000000 +distortion\k3=0.00000000 +distortion\k4=0.00000000 +distortion\k5=0.00000000 [transform] -R11=0.99994 -R12=0.00098102 -R13=0.010900 -R21=-0.00097894 -R22=1.0 -R23=-0.00019534 -R33=-0.010900 -R32=0.00018466 -R33=0.99994 -Tx=-0.02581986 -Ty=-0.0130948 -Tz=-0.0047681 +R11=0.999982 +R12=0.000556 +R13=0.005927 +R21=-0.000563 +R22=0.999999 +R23=0.001235 +R33=-0.005926 +R32=-0.001239 +R33=0.999982 +Tx=-0.024287 +Ty=0.001018 +Tz=-0.015195 +baseline=0.06061 +disparity_offset=1092.3403 diff --git a/images/earth.html b/images/earth.html index 8f8aadf59d0d832d707fcf540d82534dd0487800..6f423cb2604b37f014a83614ee796939fd27aec8 100644 --- a/images/earth.html +++ b/images/earth.html @@ -1,8 +1,9 @@ - + + QGroundControl Google Earth View 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 ddef64e86a1d49d72c92e344725e351078f64c52..a04bca8e02dd8aaa16e11992c34f9e8934aa26d7 100644 --- a/qgroundcontrol.pri +++ b/qgroundcontrol.pri @@ -1,380 +1,395 @@ -#------------------------------------------------- -# -# QGroundControl - Micro Air Vehicle Groundstation -# -# Please see our website at -# -# Author: -# Lorenz Meier -# -# (c) 2009-2010 PIXHAWK Team -# -# This file is part of the mav groundstation project -# QGroundControl is free software: you can redistribute it and/or modify -# 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 . -# -#------------------------------------------------- - -#$$BASEDIR/lib/qextserialport/include -# $$BASEDIR/lib/openjaus/libjaus/include \ -# $$BASEDIR/lib/openjaus/libopenJaus/include - -message(Qt version $$[QT_VERSION]) - -release { -# DEFINES += QT_NO_DEBUG_OUTPUT -# DEFINES += QT_NO_WARNING_OUTPUT -} - -QMAKE_PRE_LINK += echo "Copying files" - -#QMAKE_PRE_LINK += && cp -rf $$BASEDIR/models $$TARGETDIR/debug/. -#QMAKE_PRE_LINK += && cp -rf $$BASEDIR/models $$TARGETDIR/release/. - -# MAC OS X -macx { - - COMPILER_VERSION = system(gcc -v) - message(Using compiler $$COMPILER_VERSION) - - HARDWARE_PLATFORM = $$system(uname -a) - contains( HARDWARE_PLATFORM, 9.6.0 ) || contains( HARDWARE_PLATFORM, 9.7.0 ) || contains( HARDWARE_PLATFORM, 9.8.0 ) || contains( HARDWARE_PLATFORM, 9.9.0 ) { - # x86 Mac OS X Leopard 10.5 and earlier - CONFIG += x86 cocoa phonon - message(Building for Mac OS X 32bit/Leopard 10.5 and earlier) - - # Enable function-profiling with the OS X saturn tool - debug { - #QMAKE_CXXFLAGS += -finstrument-functions - #LIBS += -lSaturn - } - } else { - # x64 Mac OS X Snow Leopard 10.6 and later - CONFIG += x86_64 cocoa - CONFIG -= x86 phonon - message(Building for Mac OS X 64bit/Snow Leopard 10.6 and later) - } - - QMAKE_MACOSX_DEPLOYMENT_TARGET = 10.5 - - #DESTDIR = $$BASEDIR/bin/mac - INCLUDEPATH += -framework SDL - - LIBS += -framework IOKit \ - -framework SDL \ - -framework CoreFoundation \ - -framework ApplicationServices \ - -lm - - ICON = $$BASEDIR/images/icons/macx.icns - - # Copy audio files if needed - QMAKE_PRE_LINK += && cp -rf $$BASEDIR/audio $$TARGETDIR/qgroundcontrol.app/Contents/MacOs/. - # Copy google earth starter file - QMAKE_PRE_LINK += && cp -f $$BASEDIR/images/earth.html $$TARGETDIR/qgroundcontrol.app/Contents/MacOs/. - # Copy model files - #QMAKE_PRE_LINK += && cp -f $$BASEDIR/models/*.dae $$TARGETDIR/qgroundcontrol.app/Contents/MacOs/. - - exists(/Library/Frameworks/osg.framework):exists(/Library/Frameworks/OpenThreads.framework) { - # No check for GLUT.framework since it's a MAC default - message("Building support for OpenSceneGraph") - DEPENDENCIES_PRESENT += osg - DEFINES += QGC_OSG_ENABLED - # Include OpenSceneGraph libraries - INCLUDEPATH += -framework GLUT \ - -framework Carbon \ - -framework OpenThreads \ - -framework osg \ - -framework osgViewer \ - -framework osgGA \ - -framework osgDB \ - -framework osgText \ - -framework osgWidget - - LIBS += -framework GLUT \ - -framework Carbon \ - -framework OpenThreads \ - -framework osg \ - -framework osgViewer \ - -framework osgGA \ - -framework osgDB \ - -framework osgText \ - -framework osgWidget - } - - exists(/usr/include/osgEarth) { - message("Building support for osgEarth") - DEPENDENCIES_PRESENT += osgearth - # Include osgEarth libraries - INCLUDEPATH += -framework GDAL \ - $$IN_PWD/lib/mac32-gcc/include \ - -framework GEOS \ - -framework SQLite3 \ - -framework osgFX \ - -framework osgTerrain - - LIBS += -framework GDAL \ - -framework GEOS \ - -framework SQLite3 \ - -framework osgFX \ - -framework osgTerrain - DEFINES += QGC_OSGEARTH_ENABLED - } - - - exists(/opt/local/include/libfreenect) { - message("Building support for libfreenect") - DEPENDENCIES_PRESENT += libfreenect - # Include libfreenect libraries - LIBS += -lfreenect - DEFINES += QGC_LIBFREENECT_ENABLED - } - - # osg/osgEarth dynamic casts might fail without this compiler option. - # see http://osgearth.org/wiki/FAQ for details. - #QMAKE_CXXFLAGS += -Wl,-E -} - -# GNU/Linux -linux-g++ { - - debug { - #DESTDIR = $$BUILDDIR/debug - CONFIG += debug - } - - release { - #DESTDIR = $$BUILDDIR/release - } - - QMAKE_PRE_LINK += cp -rf $$BASEDIR/audio $$DESTDIR/. - - INCLUDEPATH += /usr/include \ - /usr/include/qt4/phonon - # $$BASEDIR/lib/flite/include \ - # $$BASEDIR/lib/flite/lang - - - message(Building for GNU/Linux 32bit/i386) - - LIBS += \ - -L/usr/lib \ - -lm \ - -lflite_cmu_us_kal \ - -lflite_usenglish \ - -lflite_cmulex \ - -lflite \ - -lSDL \ - -lSDLmain - - exists(/usr/include/osg) { - message("Building support for OpenSceneGraph") - DEPENDENCIES_PRESENT += osg - # Include OpenSceneGraph libraries - LIBS += -losg - DEFINES += QGC_OSG_ENABLED - } - - exists(/usr/include/osgEarth) | exists(/usr/local/include/osgEarth) { - message("Building support for osgEarth") - DEPENDENCIES_PRESENT += osgearth - # Include osgEarth libraries - LIBS += -losgViewer \ - -losgEarth \ - -losgEarthUtil - DEFINES += QGC_OSGEARTH_ENABLED - } - - exists(/usr/local/include/libfreenect/libfreenect.h) { - message("Building support for libfreenect") - DEPENDENCIES_PRESENT += libfreenect - INCLUDEPATH += /usr/include/libusb-1.0 - # Include libfreenect libraries - LIBS += -lfreenect - DEFINES += QGC_LIBFREENECT_ENABLED - } - - QMAKE_PRE_LINK += && cp -rf $$BASEDIR/models $$TARGETDIR/debug/. - QMAKE_PRE_LINK += && cp -rf $$BASEDIR/models $$TARGETDIR/release/. - QMAKE_PRE_LINK += && cp -rf $$BASEDIR/data $$TARGETDIR/debug/. - QMAKE_PRE_LINK += && cp -rf $$BASEDIR/data $$TARGETDIR/release/. - - # osg/osgEarth dynamic casts might fail without this compiler option. - # see http://osgearth.org/wiki/FAQ for details. - QMAKE_CXXFLAGS += -Wl,-E -} - -linux-g++-64 { - - debug { - #DESTDIR = $$BUILDDIR/debug - CONFIG += debug - } - - release { - #DESTDIR = $$BUILDDIR/release - } - - QMAKE_PRE_LINK += cp -rf $$BASEDIR/audio $$DESTDIR/. - - INCLUDEPATH += /usr/include \ - /usr/include/qt4/phonon - # $$BASEDIR/lib/flite/include \ - # $$BASEDIR/lib/flite/lang - - - # 64-bit Linux - message(Building for GNU/Linux 64bit/x64 (g++-64)) - - LIBS += \ - -L/usr/lib \ - -lm \ - -lflite_cmu_us_kal \ - -lflite_usenglish \ - -lflite_cmulex \ - -lflite \ - -lSDL \ - -lSDLmain - - exists(/usr/include/osg) { - message("Building support for OpenSceneGraph") - DEPENDENCIES_PRESENT += osg - # Include OpenSceneGraph libraries - LIBS += -losg - DEFINES += QGC_OSG_ENABLED - } - - exists(/usr/include/osgEarth) { - message("Building support for osgEarth") - DEPENDENCIES_PRESENT += osgearth - # Include osgEarth libraries - LIBS += -losgViewer \ - -losgEarth \ - -losgEarthUtil - DEFINES += QGC_OSGEARTH_ENABLED - } - - exists(/usr/local/include/libfreenect) { - message("Building support for libfreenect") - DEPENDENCIES_PRESENT += libfreenect - INCLUDEPATH += /usr/include/libusb-1.0 - # Include libfreenect libraries - LIBS += -lfreenect - DEFINES += QGC_LIBFREENECT_ENABLED - } - - # osg/osgEarth dynamic casts might fail without this compiler option. - # see http://osgearth.org/wiki/FAQ for details. - QMAKE_CXXFLAGS += -Wl,-E -} - -# Windows (32bit) -win32-msvc2008 { - - message(Building for Windows Visual Studio 2008 (32bit)) - - CONFIG += qaxcontainer - - # Special settings for debug - #CONFIG += CONSOLE - - INCLUDEPATH += $$BASEDIR/lib/sdl/msvc/include \ - $$BASEDIR/lib/opal/include \ - $$BASEDIR/lib/msinttypes - #"C:\Program Files\Microsoft SDKs\Windows\v7.0\Include" - - LIBS += -L$$BASEDIR/lib/sdl/msvc/lib \ - -lSDLmain -lSDL - -exists($$BASEDIR/lib/osg123) { -message("Building support for OSG") -DEPENDENCIES_PRESENT += osg - -# Include OpenSceneGraph and osgEarth libraries -INCLUDEPATH += $$BASEDIR/lib/osgEarth/win32/include \ - $$BASEDIR/lib/osgEarth_3rdparty/win32/OpenSceneGraph-2.8.2/include -LIBS += -L$$BASEDIR/lib/osgEarth_3rdparty/win32/OpenSceneGraph-2.8.2/lib \ - -losg \ - -losgViewer \ - -losgGA \ - -losgDB \ - -losgText \ - -lOpenThreads -DEFINES += QGC_OSG_ENABLED -exists($$BASEDIR/lib/osgEarth123) { - DEPENDENCIES_PRESENT += osgearth - message("Building support for osgEarth") - DEFINES += QGC_OSGEARTH_ENABLED - LIBS += -L$$BASEDIR/lib/osgEarth/win32/lib \ - -losgEarth \ - -losgEarthUtil -} -} - - RC_FILE = $$BASEDIR/qgroundcontrol.rc - - # Copy dependencies - BASEDIR_WIN = $$replace(BASEDIR,"/","\\") - TARGETDIR_WIN = $$replace(TARGETDIR,"/","\\") - - QMAKE_PRE_LINK += && copy /Y \"$$BASEDIR_WIN\\lib\\sdl\\win32\\SDL.dll\" \"$$TARGETDIR_WIN\\debug\\SDL.dll\" - QMAKE_PRE_LINK += && copy /Y \"$$BASEDIR_WIN\lib\sdl\win32\SDL.dll\" \"$$TARGETDIR_WIN\release\SDL.dll\" - QMAKE_PRE_LINK += && xcopy \"$$BASEDIR_WIN\audio\" \"$$TARGETDIR_WIN\debug\audio\" /S /E /Y - QMAKE_PRE_LINK += && xcopy \"$$BASEDIR_WIN\audio\" \"$$TARGETDIR_WIN\release\audio\" /S /E /Y - QMAKE_PRE_LINK += && xcopy \"$$BASEDIR_WIN\models\" \"$$TARGETDIR_WIN\debug\models\" /S /E /Y - QMAKE_PRE_LINK += && xcopy \"$$BASEDIR_WIN\models\" \"$$TARGETDIR_WIN\release\models\" /S /E /Y - - # Copy google earth starter file - QMAKE_PRE_LINK += && copy /Y \"$$BASEDIR/images/earth.html $$TARGETDIR_WIN\release\" - QMAKE_PRE_LINK += && copy /Y \"$$BASEDIR/images/earth.html $$TARGETDIR_WIN\debug\" - -} - -# Windows (32bit) -win32-g++ { - - message(Building for Windows Platform (32bit)) - - # Special settings for debug - #CONFIG += CONSOLE - - INCLUDEPATH += $$BASEDIR/lib/sdl/include \ - $$BASEDIR/lib/opal/include #\ #\ - #"C:\Program Files\Microsoft SDKs\Windows\v7.0\Include" - - LIBS += -L$$BASEDIR/lib/sdl/win32 \ - -lmingw32 -lSDLmain -lSDL -mwindows - - - - debug { - #DESTDIR = $$BUILDDIR/debug - } - - release { - #DESTDIR = $$BUILDDIR/release - } - - RC_FILE = $$BASEDIR/qgroundcontrol.rc - - # Copy dependencies - - QMAKE_PRE_LINK += && cp $$BASEDIR/lib/sdl/win32/SDL.dll $$TARGETDIR/debug/SDL.dll - QMAKE_PRE_LINK += && cp $$BASEDIR/lib/sdl/win32/SDL.dll $$TARGETDIR/release/SDL.dll - QMAKE_PRE_LINK += && cp -r $$BASEDIR/audio $$TARGETDIR/debug/audio - QMAKE_PRE_LINK += && cp -r $$BASEDIR/audio $$TARGETDIR/release/audio - QMAKE_PRE_LINK += && cp -r $$BASEDIR/models $$TARGETDIR/debug/models - QMAKE_PRE_LINK += && cp -r $$BASEDIR/models $$TARGETDIR/release/models - - # osg/osgEarth dynamic casts might fail without this compiler option. - # see http://osgearth.org/wiki/FAQ for details. - QMAKE_CXXFLAGS += -Wl,-E -} +#------------------------------------------------- +# +# QGroundControl - Micro Air Vehicle Groundstation +# +# Please see our website at +# +# Author: +# Lorenz Meier +# +# (c) 2009-2010 PIXHAWK Team +# +# This file is part of the mav groundstation project +# QGroundControl is free software: you can redistribute it and/or modify +# 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 . +# +#------------------------------------------------- + + +#$$BASEDIR/lib/qextserialport/include +# $$BASEDIR/lib/openjaus/libjaus/include \ +# $$BASEDIR/lib/openjaus/libopenJaus/include + +message(Qt version $$[QT_VERSION]) + +release { +# DEFINES += QT_NO_DEBUG_OUTPUT +# DEFINES += QT_NO_WARNING_OUTPUT +} + +QMAKE_POST_LINK += echo "Copying files" + +#QMAKE_POST_LINK += && cp -rf $$BASEDIR/models $$TARGETDIR/debug/. +#QMAKE_POST_LINK += && cp -rf $$BASEDIR/models $$TARGETDIR/release/. + +# MAC OS X +macx { + + COMPILER_VERSION = system(gcc -v) + message(Using compiler $$COMPILER_VERSION) + + HARDWARE_PLATFORM = $$system(uname -a) + contains( HARDWARE_PLATFORM, 9.6.0 ) || contains( HARDWARE_PLATFORM, 9.7.0 ) || contains( HARDWARE_PLATFORM, 9.8.0 ) || contains( HARDWARE_PLATFORM, 9.9.0 ) { + # x86 Mac OS X Leopard 10.5 and earlier + CONFIG += x86 cocoa phonon + message(Building for Mac OS X 32bit/Leopard 10.5 and earlier) + + # Enable function-profiling with the OS X saturn tool + debug { + #QMAKE_CXXFLAGS += -finstrument-functions + #LIBS += -lSaturn + } + } else { + # x64 Mac OS X Snow Leopard 10.6 and later + CONFIG += x86_64 cocoa + CONFIG -= x86 phonon + message(Building for Mac OS X 64bit/Snow Leopard 10.6 and later) + } + + QMAKE_MACOSX_DEPLOYMENT_TARGET = 10.5 + + #DESTDIR = $$BASEDIR/bin/mac + INCLUDEPATH += -framework SDL + + LIBS += -framework IOKit \ + -framework SDL \ + -framework CoreFoundation \ + -framework ApplicationServices \ + -lm + + ICON = $$BASEDIR/images/icons/macx.icns + + # Copy audio files if needed + QMAKE_POST_LINK += && cp -rf $$BASEDIR/audio $$TARGETDIR/qgroundcontrol.app/Contents/MacOs + # Copy google earth starter file + QMAKE_POST_LINK += && cp -f $$BASEDIR/images/earth.html $$TARGETDIR/qgroundcontrol.app/Contents/MacOs + # Copy model files + #QMAKE_POST_LINK += && cp -f $$BASEDIR/models/*.dae $$TARGETDIR/qgroundcontrol.app/Contents/MacOs + + exists(/Library/Frameworks/osg.framework):exists(/Library/Frameworks/OpenThreads.framework) { + # No check for GLUT.framework since it's a MAC default + message("Building support for OpenSceneGraph") + DEPENDENCIES_PRESENT += osg + DEFINES += QGC_OSG_ENABLED + # Include OpenSceneGraph libraries + INCLUDEPATH += -framework GLUT \ + -framework Carbon \ + -framework OpenThreads \ + -framework osg \ + -framework osgViewer \ + -framework osgGA \ + -framework osgDB \ + -framework osgText \ + -framework osgWidget + + LIBS += -framework GLUT \ + -framework Carbon \ + -framework OpenThreads \ + -framework osg \ + -framework osgViewer \ + -framework osgGA \ + -framework osgDB \ + -framework osgText \ + -framework osgWidget + } + + exists(/usr/include/osgEarth) { + message("Building support for osgEarth") + DEPENDENCIES_PRESENT += osgearth + # Include osgEarth libraries + INCLUDEPATH += -framework GDAL \ + $$IN_PWD/lib/mac32-gcc/include \ + -framework GEOS \ + -framework SQLite3 \ + -framework osgFX \ + -framework osgTerrain + + LIBS += -framework GDAL \ + -framework GEOS \ + -framework SQLite3 \ + -framework osgFX \ + -framework osgTerrain + DEFINES += QGC_OSGEARTH_ENABLED + } + + + exists(/opt/local/include/libfreenect) { + message("Building support for libfreenect") + DEPENDENCIES_PRESENT += libfreenect + # Include libfreenect libraries + LIBS += -lfreenect + DEFINES += QGC_LIBFREENECT_ENABLED + } + + # osg/osgEarth dynamic casts might fail without this compiler option. + # see http://osgearth.org/wiki/FAQ for details. + #QMAKE_CXXFLAGS += -Wl,-E +} + +# GNU/Linux +linux-g++ { + + debug { + DESTDIR = $$TARGETDIR/debug + CONFIG += debug + } + + release { + DESTDIR = $$TARGETDIR/release + } + + QMAKE_POST_LINK += cp -rf $$BASEDIR/audio $$DESTDIR/. + + INCLUDEPATH += /usr/include \ + /usr/local/include \ + /usr/include/qt4/phonon + # $$BASEDIR/lib/flite/include \ + # $$BASEDIR/lib/flite/lang + + + message(Building for GNU/Linux 32bit/i386) + + LIBS += \ + -L/usr/lib \ + -lm \ + -lflite_cmu_us_kal \ + -lflite_usenglish \ + -lflite_cmulex \ + -lflite \ + -lSDL \ + -lSDLmain + + exists(/usr/include/osg) { + message("Building support for OpenSceneGraph") + DEPENDENCIES_PRESENT += osg + # Include OpenSceneGraph libraries + LIBS += -losg \ + -losgViewer + DEFINES += QGC_OSG_ENABLED + } + + exists(/usr/include/osgEarth) | exists(/usr/local/include/osgEarth) { + message("Building support for osgEarth") + DEPENDENCIES_PRESENT += osgearth + # Include osgEarth libraries + LIBS += -losgEarth \ + -losgEarthUtil + DEFINES += QGC_OSGEARTH_ENABLED + } + + exists(/usr/local/include/libfreenect/libfreenect.h) { + message("Building support for libfreenect") + DEPENDENCIES_PRESENT += libfreenect + INCLUDEPATH += /usr/include/libusb-1.0 + # Include libfreenect libraries + LIBS += -lfreenect + DEFINES += QGC_LIBFREENECT_ENABLED + } + + QMAKE_POST_LINK += && cp -rf $$BASEDIR/models $$DESTDIR + QMAKE_POST_LINK += && cp -rf $$BASEDIR/data $$DESTDIR + QMAKE_POST_LINK += && mkdir -p $$DESTDIR/images + QMAKE_POST_LINK += && cp -rf $$BASEDIR/images/Vera.ttf $$DESTDIR/images/Vera.ttf + + # osg/osgEarth dynamic casts might fail without this compiler option. + # see http://osgearth.org/wiki/FAQ for details. + QMAKE_CXXFLAGS += -Wl,-E +} + +linux-g++-64 { + + debug { + DESTDIR = $$TARGETDIR/debug + CONFIG += debug + } + + release { + DESTDIR = $$TARGETDIR/release + } + + QMAKE_POST_LINK += cp -rf $$BASEDIR/audio $$DESTDIR/. + + INCLUDEPATH += /usr/include \ + /usr/include/qt4/phonon + # $$BASEDIR/lib/flite/include \ + # $$BASEDIR/lib/flite/lang + + + # 64-bit Linux + message(Building for GNU/Linux 64bit/x64 (g++-64)) + + LIBS += \ + -L/usr/lib \ + -lm \ + -lflite_cmu_us_kal \ + -lflite_usenglish \ + -lflite_cmulex \ + -lflite \ + -lSDL \ + -lSDLmain + + exists(/usr/include/osg) { + message("Building support for OpenSceneGraph") + DEPENDENCIES_PRESENT += osg + # Include OpenSceneGraph libraries + LIBS += -losg \ + -losgViewer + DEFINES += QGC_OSG_ENABLED + } + + exists(/usr/include/osgEarth) { + message("Building support for osgEarth") + DEPENDENCIES_PRESENT += osgearth + # Include osgEarth libraries + LIBS += -losgEarth \ + -losgEarthUtil + DEFINES += QGC_OSGEARTH_ENABLED + } + + exists(/usr/local/include/libfreenect) { + message("Building support for libfreenect") + DEPENDENCIES_PRESENT += libfreenect + INCLUDEPATH += /usr/include/libusb-1.0 + # Include libfreenect libraries + LIBS += -lfreenect + DEFINES += QGC_LIBFREENECT_ENABLED + } + + QMAKE_POST_LINK += && cp -rf $$BASEDIR/models $$DESTDIR + QMAKE_POST_LINK += && cp -rf $$BASEDIR/data $$DESTDIR + QMAKE_POST_LINK += && mkdir -p $$DESTDIR/images + QMAKE_POST_LINK += && cp -rf $$BASEDIR/images/Vera.ttf $$DESTDIR/images/Vera.ttf + + # osg/osgEarth dynamic casts might fail without this compiler option. + # see http://osgearth.org/wiki/FAQ for details. + QMAKE_CXXFLAGS += -Wl,-E +} + +# Windows (32bit) +win32-msvc2008 { + + message(Building for Windows Visual Studio 2008 (32bit)) + + CONFIG += qaxcontainer + + # Special settings for debug + #CONFIG += CONSOLE + + INCLUDEPATH += $$BASEDIR/lib/sdl/msvc/include \ + $$BASEDIR/lib/opal/include \ + $$BASEDIR/lib/msinttypes + #"C:\Program Files\Microsoft SDKs\Windows\v7.0\Include" + + LIBS += -L$$BASEDIR/lib/sdl/msvc/lib \ + -lSDLmain -lSDL + +exists($$BASEDIR/lib/osg123) { +message("Building support for OSG") +DEPENDENCIES_PRESENT += osg + +# Include OpenSceneGraph and osgEarth libraries +INCLUDEPATH += $$BASEDIR/lib/osgEarth/win32/include \ + $$BASEDIR/lib/osgEarth_3rdparty/win32/OpenSceneGraph-2.8.2/include +LIBS += -L$$BASEDIR/lib/osgEarth_3rdparty/win32/OpenSceneGraph-2.8.2/lib \ + -losg \ + -losgViewer \ + -losgGA \ + -losgDB \ + -losgText \ + -lOpenThreads +DEFINES += QGC_OSG_ENABLED +exists($$BASEDIR/lib/osgEarth123) { + DEPENDENCIES_PRESENT += osgearth + message("Building support for osgEarth") + DEFINES += QGC_OSGEARTH_ENABLED + LIBS += -L$$BASEDIR/lib/osgEarth/win32/lib \ + -losgEarth \ + -losgEarthUtil +} +} + + RC_FILE = $$BASEDIR/qgroundcontrol.rc + + # Copy dependencies + BASEDIR_WIN = $$replace(BASEDIR,"/","\\") + TARGETDIR_WIN = $$replace(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\" + } + + 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\" + } + +} + +# Windows (32bit) +win32-g++ { + + message(Building for Windows Platform (32bit)) + + # Special settings for debug + #CONFIG += CONSOLE + + INCLUDEPATH += $$BASEDIR/lib/sdl/include \ + $$BASEDIR/lib/opal/include #\ #\ + #"C:\Program Files\Microsoft SDKs\Windows\v7.0\Include" + + LIBS += -L$$BASEDIR/lib/sdl/win32 \ + -lmingw32 -lSDLmain -lSDL -mwindows + + + + debug { + #DESTDIR = $$BUILDDIR/debug + } + + release { + #DESTDIR = $$BUILDDIR/release + } + + RC_FILE = $$BASEDIR/qgroundcontrol.rc + + # Copy dependencies + + 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 + QMAKE_POST_LINK += && cp -r $$BASEDIR/models $$TARGETDIR/debug/models + } + + release { + QMAKE_POST_LINK += && cp $$BASEDIR/lib/sdl/win32/SDL.dll $$TARGETDIR/release/SDL.dll + QMAKE_POST_LINK += && cp -r $$BASEDIR/audio $$TARGETDIR/release/audio + QMAKE_POST_LINK += && cp -r $$BASEDIR/models $$TARGETDIR/release/models + } + + # 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 f7cbd8f43d98af22e8a403dbac93becf2c066678..731315a90bc0390a31368b497e8064c017fc51f6 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -249,7 +249,9 @@ HEADERS += src/MG.h \ src/ui/SlugsHilSim.h \ src/ui/SlugsPIDControl.h \ src/ui/SlugsVideoCamControl.h \ - src/ui/SlugsPadCameraControl.h + src/ui/SlugsPadCameraControl.h \ + src/ui/QGCMainWindowAPConfigurator.h \ + src/comm/MAVLinkSwarmSimulationLink.h contains(DEPENDENCIES_PRESENT, osg) { message("Including headers for OpenSceneGraph") @@ -359,7 +361,9 @@ SOURCES += src/main.cc \ src/ui/SlugsHilSim.cc \ src/ui/SlugsPIDControl.cpp \ src/ui/SlugsVideoCamControl.cpp \ - src/ui/SlugsPadCameraControl.cpp + src/ui/SlugsPadCameraControl.cpp \ + src/ui/QGCMainWindowAPConfigurator.cc \ + src/comm/MAVLinkSwarmSimulationLink.cc contains(DEPENDENCIES_PRESENT, osg) { message("Including sources for OpenSceneGraph") diff --git a/src/Core.cc b/src/Core.cc index d63bd0d919bda5645fbcd3f87a9e27aac3a393a5..e49a06c53cd57ec73c143533a3e849f4acff607d 100644 --- a/src/Core.cc +++ b/src/Core.cc @@ -157,6 +157,9 @@ Core::Core(int &argc, char* argv[]) : QApplication(argc, argv) **/ Core::~Core() { + //mainWindow->storeSettings(); + mainWindow->hide(); + mainWindow->deleteLater(); // Delete singletons delete LinkManager::instance(); delete UASManager::instance(); diff --git a/src/QGC.h b/src/QGC.h index 1ea849a230599933f07ae20dbcd2c26714b81d24..4751cdda47404bea2c0d454f84ad560f51984ea7 100644 --- a/src/QGC.h +++ b/src/QGC.h @@ -14,6 +14,9 @@ namespace QGC /** @brief Get the current ground time in microseconds */ quint64 groundTimeUsecs(); + + const QString APPNAME = "QGROUNDCONTROL"; + const QString COMPANYNAME = "OPENMAV"; } #endif // QGC_H diff --git a/src/comm/LinkManager.cc b/src/comm/LinkManager.cc index 76c4fde4c142de7e24b6655c863b4fd70b5106f2..802cf1741f7642efe92cfea223405ce301f7f88d 100644 --- a/src/comm/LinkManager.cc +++ b/src/comm/LinkManager.cc @@ -32,6 +32,7 @@ This file is part of the QGROUNDCONTROL project #include #include #include "LinkManager.h" +#include #include @@ -65,6 +66,7 @@ LinkManager::~LinkManager() void LinkManager::add(LinkInterface* link) { + if(!link) return; links.append(link); emit newLink(link); } @@ -73,6 +75,7 @@ void LinkManager::addProtocol(LinkInterface* link, ProtocolInterface* protocol) { // Connect link to protocol // the protocol will receive new bytes from the link + if(!link || !protocol) return; connect(link, SIGNAL(bytesReceived(LinkInterface*, QByteArray)), protocol, SLOT(receiveBytes(LinkInterface*, QByteArray))); // Store the connection information in the protocol links map protocolLinks.insertMulti(protocol, link); @@ -91,7 +94,8 @@ bool LinkManager::connectAll() foreach (LinkInterface* link, links) { - if(! link->connect()) allConnected = false; + if(!link) {} + else if(!link->connect()) allConnected = false; } return allConnected; @@ -103,7 +107,9 @@ bool LinkManager::disconnectAll() foreach (LinkInterface* link, links) { - if(! link->disconnect()) allDisconnected = false; + //static int i=0; + if(!link){} + else if(!link->disconnect()) allDisconnected = false; } return allDisconnected; @@ -111,14 +117,32 @@ bool LinkManager::disconnectAll() bool LinkManager::connectLink(LinkInterface* link) { + if(!link) return false; return link->connect(); } bool LinkManager::disconnectLink(LinkInterface* link) { + if(!link) return false; return link->disconnect(); } +bool LinkManager::removeLink(LinkInterface* link) +{ + if(link) + { + for (int i=0; i < QList(links).size(); i++) + { + if(link==links.at(i)) + { + links.removeAt(i); //remove from link list + } + } + return true; + } + return false; +} + /** * The access time is linear in the number of links. * diff --git a/src/comm/LinkManager.h b/src/comm/LinkManager.h index b1768758b2a356362c37d93ed85ca5c22c1ca3e9..8d283c3de0d3f1273bf41afdabdccd8724812d7a 100644 --- a/src/comm/LinkManager.h +++ b/src/comm/LinkManager.h @@ -67,6 +67,8 @@ public slots: void add(LinkInterface* link); void addProtocol(LinkInterface* link, ProtocolInterface* protocol); + bool removeLink(LinkInterface* link); + bool connectAll(); bool connectLink(LinkInterface* link); diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index 835064194b6da2915a199ef8680681721de5e06e..b992633a428f3982fe0eda1b1b92dfff2a25a4c3 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -159,7 +159,9 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) switch (heartbeat.autopilot) { case MAV_AUTOPILOT_GENERIC: + uas = new UAS(this, message.sysid); + // Connect this robot to the UAS object connect(this, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), uas, SLOT(receiveMessage(LinkInterface*, mavlink_message_t))); break; @@ -202,11 +204,15 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) break; } + // Set the autopilot type + uas->setAutopilotType((int)heartbeat.autopilot); // Make UAS aware that this link can be used to communicate with the actual robot uas->addLink(link); + // Now add UAS to "official" list, which makes the whole application aware of it UASManager::instance()->addUAS(uas); + } // Only count message if UAS exists for this message diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc index 6d6ecf93579c3a6044ddc80ea6b8787850df0033..f35a129467e2f05efbd0efbb0080ff234f6c49aa 100644 --- a/src/comm/MAVLinkSimulationLink.cc +++ b/src/comm/MAVLinkSimulationLink.cc @@ -559,16 +559,17 @@ void MAVLinkSimulationLink::mainloop() // Send controller states - uint8_t attControl = 1; - uint8_t posXYControl = 1; - uint8_t posZControl = 0; - uint8_t posYawControl = 1; - uint8_t gpsLock = 2; - uint8_t visLock = 3; - uint8_t posLock = qMax(gpsLock, visLock); #ifdef MAVLINK_ENABLED_PIXHAWK + uint8_t attControl = 1; + uint8_t posXYControl = 1; + uint8_t posZControl = 0; + uint8_t posYawControl = 1; + + uint8_t gpsLock = 2; + uint8_t visLock = 3; + uint8_t posLock = qMax(gpsLock, visLock); messageSize = mavlink_msg_control_status_pack(systemId, componentId, &msg, posLock, visLock, gpsLock, attControl, posXYControl, posZControl, posYawControl); #endif @@ -659,7 +660,7 @@ qint64 MAVLinkSimulationLink::bytesAvailable() void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size) { - qDebug() << "Simulation received " << size << " bytes from groundstation: "; + //qDebug() << "Simulation received " << size << " bytes from groundstation: "; // Increase write counter //bitsSentTotal += size * 8; @@ -837,9 +838,9 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size) } unsigned char v=data[i]; - fprintf(stderr,"%02x ", v); + //fprintf(stderr,"%02x ", v); } - fprintf(stderr,"\n"); + //fprintf(stderr,"\n"); readyBufferMutex.lock(); for (int i = 0; i < streampointer; i++) diff --git a/src/comm/MAVLinkSimulationLink.h b/src/comm/MAVLinkSimulationLink.h index 49cc4d5c028bd36b3a9432a86fc43cb3b719c96c..fe92e5fa87058b328db57c32c7cdf367db700774 100644 --- a/src/comm/MAVLinkSimulationLink.h +++ b/src/comm/MAVLinkSimulationLink.h @@ -85,7 +85,7 @@ public: public slots: void writeBytes(const char* data, qint64 size); void readBytes(); - void mainloop(); + virtual void mainloop(); bool connectLink(bool connect); 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 d4b48cdc1143beda514fabbeb9dcd5e77eb693de..404a8dbfc8049ee7c56afa7191c207c40e58a731 100644 --- a/src/comm/SerialLink.cc +++ b/src/comm/SerialLink.cc @@ -30,10 +30,13 @@ This file is part of the QGROUNDCONTROL project #include #include +#include #include #include "SerialLink.h" #include "LinkManager.h" +#include "QGC.h" #include +#include #ifdef _WIN32 #include "windows.h" #endif @@ -54,12 +57,41 @@ SerialLink::SerialLink(QString portname, BaudRateType baudrate, FlowType flow, P #endif // Set unique ID and add link to the list of links this->id = getNextLinkId(); - this->baudrate = baudrate; - this->flow = flow; - this->parity = parity; - this->dataBits = dataBits; - this->stopBits = stopBits; - this->timeout = 1; ///< The timeout controls how long the program flow should wait for new serial bytes. As we're polling, we don't want to wait at all. + + // Load defaults from settings + QSettings settings(QGC::COMPANYNAME, QGC::APPNAME); + settings.sync(); + if (settings.contains("SERIALLINK_COMM_PORT")) + { + this->porthandle = settings.value("SERIALLINK_COMM_PORT").toString(); + } + + // *nix (Linux, MacOS tested) serial port support + port = new QextSerialPort(porthandle, QextSerialPort::Polling); + //port = new QextSerialPort(porthandle, QextSerialPort::EventDriven); + + if (settings.contains("SERIALLINK_COMM_PORT")) + { + setBaudRate(settings.value("SERIALLINK_COMM_BAUD").toInt()); + setParityType(settings.value("SERIALLINK_COMM_PARITY").toInt()); + setStopBitsType(settings.value("SERIALLINK_COMM_STOPBITS").toInt()); + setDataBitsType(settings.value("SERIALLINK_COMM_DATABITS").toInt()); + } + else + { + this->baudrate = baudrate; + this->flow = flow; + this->parity = parity; + this->dataBits = dataBits; + this->stopBits = stopBits; + this->timeout = 1; ///< The timeout controls how long the program flow should wait for new serial bytes. As we're polling, we don't want to wait at all. + } + port->setTimeout(timeout); // Timeout of 0 ms, we don't want to wait for data, we just poll again next time + port->setBaudRate(baudrate); + port->setFlowControl(flow); + port->setParity(parity); + port->setDataBits(dataBits); + port->setStopBits(stopBits); // Set the port name if (porthandle == "") @@ -88,15 +120,7 @@ SerialLink::SerialLink(QString portname, BaudRateType baudrate, FlowType flow, P //some other error occurred. Inform user. } #else - // *nix (Linux, MacOS tested) serial port support - port = new QextSerialPort(porthandle, QextSerialPort::Polling); - //port = new QextSerialPort(porthandle, QextSerialPort::EventDriven); - port->setTimeout(timeout); // Timeout of 0 ms, we don't want to wait for data, we just poll again next time - port->setBaudRate(baudrate); - port->setFlowControl(flow); - port->setParity(parity); - port->setDataBits(dataBits); - port->setStopBits(stopBits); + #endif // Link is setup, register it with link manager @@ -106,7 +130,7 @@ SerialLink::SerialLink(QString portname, BaudRateType baudrate, FlowType flow, P SerialLink::~SerialLink() { disconnect(); - delete port; + if(port) delete port; port = NULL; } @@ -170,7 +194,7 @@ void SerialLink::writeBytes(const char* data, qint64 size) { unsigned char v=data[i]; - fprintf(stderr,"%02x ", v); + //fprintf(stderr,"%02x ", v); } } } @@ -239,6 +263,8 @@ bool SerialLink::disconnect() port->close(); dataMutex.unlock(); + if(this->isRunning()) this->terminate(); //stop running the thread, restart it upon connect + bool closed = true; //port->isOpen(); @@ -298,6 +324,15 @@ bool SerialLink::hardwareConnect() if(connectionUp) { emit connected(); emit connected(true); + + // Store settings + QSettings settings(QGC::COMPANYNAME, QGC::APPNAME); + settings.setValue("SERIALLINK_COMM_PORT", this->porthandle); + settings.setValue("SERIALLINK_COMM_BAUD", getBaudRate()); + settings.setValue("SERIALLINK_COMM_PARITY", getParityType()); + settings.setValue("SERIALLINK_COMM_STOPBITS", getStopBitsType()); + settings.setValue("SERIALLINK_COMM_DATABITS", getDataBitsType()); + settings.sync(); } return connectionUp; @@ -311,7 +346,14 @@ bool SerialLink::hardwareConnect() **/ bool SerialLink::isConnected() { - return port->isOpen(); + if (port) + { + return port->isOpen(); + } + else + { + return false; + } } int SerialLink::getId() @@ -516,7 +558,7 @@ bool SerialLink::setPortName(QString portName) this->porthandle = "\\\\.\\" + this->porthandle; } #endif - delete port; + if(port) delete port; port = new QextSerialPort(porthandle, QextSerialPort::Polling); port->setBaudRate(baudrate); @@ -707,9 +749,16 @@ bool SerialLink::setBaudRate(int rate) break; } - port->setBaudRate(this->baudrate); - if(reconnect) connect(); - return accepted; + if (port) + { + port->setBaudRate(this->baudrate); + if(reconnect) connect(); + return accepted; + } + else + { + return false; + } } bool SerialLink::setFlowType(int flow) diff --git a/src/input/Freenect.cc b/src/input/Freenect.cc index 01f751bfe5cbb520325ef4d24d85b9aedd31a4a0..1ac1bba5483e4f9bc8577685364246a9bcf54c6a 100644 --- a/src/input/Freenect.cc +++ b/src/input/Freenect.cc @@ -208,8 +208,9 @@ Freenect::get3DPointCloudData(void) { if (data[i] > 0 && data[i] <= 2048) { - // see www.ros.org/wiki/kinect_node for details - double range = 1.0f / (-0.00307f * static_cast(data[i]) + 3.33f); + double range = baseline * depthCameraParameters.fx + / (1.0 / 8.0 * (disparityOffset + - static_cast(data[i]))); if (range > 0.0f) { @@ -340,7 +341,10 @@ Freenect::readConfigFile(void) settings.value("transform/R33").toDouble(), settings.value("transform/Tz").toDouble(), 0.0, 0.0, 0.0, 1.0); - transformMatrix = transformMatrix.transposed(); + transformMatrix = transformMatrix.inverted(); + + baseline = settings.value("transform/baseline").toDouble(); + disparityOffset = settings.value("transform/disparity_offset").toDouble(); } void diff --git a/src/input/Freenect.h b/src/input/Freenect.h index 0995715e31a7d34e80fe47f2f00d81faffd3f701..8fd316ebc296ba089c3cd3c0013e96e416b1f259 100644 --- a/src/input/Freenect.h +++ b/src/input/Freenect.h @@ -119,6 +119,8 @@ private: IntrinsicCameraParameters depthCameraParameters; QMatrix4x4 transformMatrix; + double baseline; + double disparityOffset; // tilt angle of Kinect camera int tiltAngle; diff --git a/src/uas/PxQuadMAV.cc b/src/uas/PxQuadMAV.cc index 7f9a9125e07cf1aa4f5969057e7984e152d97655..2b9739ee4d9ed5108f0d1029410abc537b3855da 100644 --- a/src/uas/PxQuadMAV.cc +++ b/src/uas/PxQuadMAV.cc @@ -41,12 +41,12 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) { // Let UAS handle the default message set UAS::receiveMessage(link, message); - mavlink_message_t* msg = &message; //qDebug() << "PX RECEIVED" << msg->sysid << msg->compid << msg->msgid; // Only compile this portion if matching MAVLink packets have been compiled #ifdef MAVLINK_ENABLED_PIXHAWK + mavlink_message_t* msg = &message; if (message.sysid == uasId) { @@ -168,5 +168,9 @@ void PxQuadMAV::sendProcessCommand(int watchdogId, int processId, unsigned int c mavlink_message_t msg; mavlink_msg_watchdog_command_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &payload); sendMessage(msg); +#else + Q_UNUSED(watchdogId); + Q_UNUSED(processId); + Q_UNUSED(command); #endif } diff --git a/src/uas/SlugsMAV.cc b/src/uas/SlugsMAV.cc index 025087bf9ceee405e9cc6baecf1cb792d80f38e8..edc2a4d3bc6b7c7b2d22927a6d0b10f0648ba054 100644 --- a/src/uas/SlugsMAV.cc +++ b/src/uas/SlugsMAV.cc @@ -169,10 +169,16 @@ void SlugsMAV::emitSignals (void){ case 2: emit slugsAirData(uasId, mlAirData); emit slugsDiagnostic(uasId,mlDiagnosticData); + break; case 3: - emit slugsPilotConsolePWM(uasId,mlPilotConsoleData); + 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(0,(mlPilotConsoleData.dt- 1000.0f)/1000.0f); + emit slugsPWM(uasId, mlPwmCommands); break; @@ -184,11 +190,13 @@ void SlugsMAV::emitSignals (void){ case 5: emit slugsFilteredData(uasId,mlFilteredData); emit slugsGPSDateTime(uasId, mlGpsDateTime); + break; case 6: emit slugsActionAck(uasId,mlActionAck); emit emitGpsSignals(); + break; } @@ -212,20 +220,30 @@ void SlugsMAV::emitSignals (void){ #ifdef MAVLINK_ENABLED_SLUGS void SlugsMAV::emitGpsSignals (void){ - if (mlGpsData.fix_type > 0){ + qDebug()<<"After Emit GPS Signal"< 0){ emit globalPositionChanged(this, mlGpsData.lon, mlGpsData.lat, mlGpsData.alt, 0.0); - // Smaller than threshold and not NaN - if (mlGpsData.v < 1000000 && mlGpsData.v == mlGpsData.v){ - emit speedChanged(this, (double)mlGpsData.v, 0.0, 0.0, 0.0); - } else { - emit textMessageReceived(uasId, uasId, 255, QString("GCS ERROR: RECEIVED INVALID SPEED OF %1 m/s").arg(mlGpsData.v)); - } - } + emit slugsGPSCogSog(uasId,mlGpsData.hdg, mlGpsData.v); + +// // Smaller than threshold and not NaN +// if (mlGpsData.v < 1000000 && mlGpsData.v == mlGpsData.v){ +// // emit speedChanged(this, (double)mlGpsData.v, 0.0, 0.0, 0.0); + +// } +// else { +// emit textMessageReceived(uasId, uasId, 255, QString("GCS ERROR: RECEIVED INVALID SPEED OF %1 m/s").arg(mlGpsData.v)); +// } + //} + } void SlugsMAV::emitPidSignal() diff --git a/src/uas/SlugsMAV.h b/src/uas/SlugsMAV.h index 295c8206530267bda6c4574215251824b628bf10..c30996d32369dbb66290b71af97408fac7cd748b 100644 --- a/src/uas/SlugsMAV.h +++ b/src/uas/SlugsMAV.h @@ -47,6 +47,7 @@ public slots: signals: void slugsRawImu(int uasId, const mavlink_raw_imu_t& rawData); + void slugsGPSCogSog(int uasId, double cog, double sog); #ifdef MAVLINK_ENABLED_SLUGS @@ -67,6 +68,8 @@ signals: void slugsBootMsg(int uasId, mavlink_boot_t& boot); void slugsAttitude(int uasId, mavlink_attitude_t& attitude); + + #endif protected: diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 60f0e043f9c7259b5a7e0182e54cfec2029384ab..f503a062e4a5ab8c82e949adfb615f8b37da97d7 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -43,12 +43,16 @@ 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(), uasId(id), startTime(MG::TIME::getGroundTimeNow()), commStatus(COMM_DISCONNECTED), name(""), + autopilot(-1), links(new QList()), unknownPackets(), mavlink(protocol), @@ -93,6 +97,7 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(), UAS::~UAS() { delete links; + links=NULL; } int UAS::getUASID() const @@ -124,6 +129,7 @@ void UAS::setSelected() void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) { + if (!link) return; if (!links->contains(link)) { addLink(link); @@ -149,8 +155,10 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) if (this->type != mavlink_msg_heartbeat_get_type(&message)) { this->type = mavlink_msg_heartbeat_get_type(&message); + this->autopilot = mavlink_msg_heartbeat_get_autopilot(&message); emit systemTypeSet(this, type); } + break; case MAVLINK_MSG_ID_BOOT: getStatusForCode((int)MAV_STATE_BOOT, uasState, stateDescription); @@ -328,6 +336,10 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) emit valueChanged(uasId, "Vz", pos.vz, time); emit localPositionChanged(this, pos.x, pos.y, pos.z, time); emit speedChanged(this, pos.vx, pos.vy, pos.vz, time); + +// qDebug()<<"Local Position = "<notifyPositive(); } positionLock = true; - - // Send to patch antenna - mavlink_message_t msg; - mavlink_msg_global_position_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, pos.usec, pos.lat, pos.lon, pos.alt, pos.vx, pos.vy, pos.vz); - sendMessage(msg); + //TODO fix this hack for forwarding of global position for patch antenna tracking + forwardMessage(message); } break; case MAVLINK_MSG_ID_GPS_RAW: @@ -695,7 +704,12 @@ void UAS::setLocalPositionSetpoint(float x, float y, float z, float yaw) mavlink_message_t msg; mavlink_msg_position_control_setpoint_set_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, 0, x, y, z, yaw); sendMessage(msg); -#endif + #else + Q_UNUSED(x); + Q_UNUSED(y); + Q_UNUSED(z); + Q_UNUSED(yaw); + #endif } void UAS::setLocalPositionOffset(float x, float y, float z, float yaw) @@ -704,6 +718,11 @@ void UAS::setLocalPositionOffset(float x, float y, float z, float yaw) mavlink_message_t msg; mavlink_msg_position_control_offset_set_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, x, y, z, yaw); sendMessage(msg); +#else +Q_UNUSED(x); +Q_UNUSED(y); +Q_UNUSED(z); +Q_UNUSED(yaw); #endif } @@ -819,8 +838,31 @@ 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 241e3db6f16281814c2176b5ca192adf7897d2d0..22c655f19b462f852ec956ee2728a832fe2691cb 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -79,25 +79,26 @@ public: /** @brief Get the links associated with this robot */ QList* getLinks(); - double getLocalX() const { return localX; }; - double getLocalY() const { return localY; }; - double getLocalZ() const { return localZ; }; - double getLatitude() const { return latitude; }; - double getLongitude() const { return longitude; }; - double getAltitude() const { return altitude; }; + double getLocalX() const { return localX; } + double getLocalY() const { return localY; } + double getLocalZ() const { return localZ; } + double getLatitude() const { return latitude; } + double getLongitude() const { return longitude; } + double getAltitude() const { return altitude; } - double getRoll() const { return roll; }; - double getPitch() const { return pitch; }; - double getYaw() const { return yaw; }; + double getRoll() const { return roll; } + double getPitch() const { return pitch; } + double getYaw() const { return yaw; } friend class UASWaypointManager; protected: int uasId; ///< Unique system ID - int type; ///< UAS type (from type enum) + unsigned char type; ///< UAS type (from type enum) quint64 startTime; ///< The time the UAS was switched on CommStatus commStatus; ///< Communication status QString name; ///< Human-friendly name of the vehicle, e.g. bravo + int autopilot; ///< Type of the Autopilot: -1: None, 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM QList* links; ///< List of links this UAS can be reached by QList unknownPackets; ///< Packet IDs which are unknown and have been received MAVLinkProtocol* mavlink; ///< Reference to the MAVLink instance @@ -164,6 +165,8 @@ protected: public: UASWaypointManager &getWaypointManager(void) { return waypointManager; } int getSystemType(); + int getAutopilotType() {return autopilot;} + void setAutopilotType(int apType) { autopilot = apType;} public slots: /** @brief Launches the system **/ @@ -215,6 +218,9 @@ public slots: /** @brief Send a message over all links this UAS can be reached with (!= all links) */ void sendMessage(mavlink_message_t message); + /** @brief Temporary Hack for sending packets to patch Antenna. Send a message over all serial links except for this UAS's */ + void forwardMessage(mavlink_message_t message); + /** @brief Set this UAS as the system currently in focus, e.g. in the main display widgets */ void setSelected(); diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index 1aa7c88126d962cd5e976ee81b78f8f5f0af2c17..332f90e44c5a441037ed6bed836212889fd17516 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -157,6 +157,9 @@ public: return color; } + virtual int getAutopilotType() = 0; + virtual void setAutopilotType(int apType)= 0; + public slots: /** @brief Launches the system/Liftof **/ @@ -241,6 +244,8 @@ public slots: virtual void startGyroscopeCalibration() = 0; virtual void startPressureCalibration() = 0; + + protected: QColor color; diff --git a/src/uas/UASManager.cc b/src/uas/UASManager.cc index bba66dda702204e6c166734d75a54b22bd9cc618..fabfec6ba1072d55fbf5101a2a710114d2631710 100644 --- a/src/uas/UASManager.cc +++ b/src/uas/UASManager.cc @@ -85,6 +85,11 @@ void UASManager::addUAS(UASInterface* uas) } } +QList UASManager::getUASList() +{ + return systems.values(); +} + UASInterface* UASManager::getActiveUAS() { if(!activeUAS) @@ -96,6 +101,11 @@ UASInterface* UASManager::getActiveUAS() return activeUAS; ///< Return zero pointer if no UAS has been loaded } +UASInterface* UASManager::silentGetActiveUAS() +{ + return activeUAS; ///< Return zero pointer if no UAS has been loaded +} + bool UASManager::launchActiveUAS() { // If the active UAS is set, execute command diff --git a/src/uas/UASManager.h b/src/uas/UASManager.h index 7b8dabfb501bdc190b78e186440b094857a218e8..b4d97789df578036d4b7a56d914a673ced3a159a 100644 --- a/src/uas/UASManager.h +++ b/src/uas/UASManager.h @@ -57,6 +57,7 @@ public: * @return NULL pointer if no UAS exists, active UAS else **/ UASInterface* getActiveUAS(); + UASInterface* silentGetActiveUAS(); /** * @brief Get the UAS with this id * @@ -68,6 +69,8 @@ public: **/ UASInterface* getUASForId(int id); + QList getUASList(); + public slots: diff --git a/src/ui/CommConfigurationWindow.cc b/src/ui/CommConfigurationWindow.cc index 0708aeef9f3b0e32e42db0b216e323a058cac989..33e57ba2514c5bc6138ac22f3b9a228362158e88 100644 --- a/src/ui/CommConfigurationWindow.cc +++ b/src/ui/CommConfigurationWindow.cc @@ -47,6 +47,7 @@ This file is part of the QGROUNDCONTROL project #endif #include "MAVLinkProtocol.h" #include "MAVLinkSettingsWidget.h" +#include "LinkManager.h" CommConfigurationWindow::CommConfigurationWindow(LinkInterface* link, ProtocolInterface* protocol, QWidget *parent, Qt::WindowFlags flags) : QWidget(parent, flags) { @@ -58,6 +59,10 @@ CommConfigurationWindow::CommConfigurationWindow(LinkInterface* link, ProtocolIn // add link types ui.linkType->addItem("Serial",QGC_LINK_SERIAL); ui.linkType->addItem("UDP",QGC_LINK_UDP); + ui.linkType->addItem("Simulation",QGC_LINK_SIMULATION); + ui.linkType->addItem("Serial Forwarding",QGC_LINK_FORWARDING); + + ui.connectionType->addItem("MAVLink", QGC_PROTOCOL_MAVLINK); // Create action to open this menu // Create configuration action for this link @@ -195,11 +200,20 @@ void CommConfigurationWindow::setLinkName(QString name) void CommConfigurationWindow::remove() { - link->disconnect(); - //delete link; - //delete action; + if(action) delete action; //delete action first since it has a pointer to link + action=NULL; + + if(link) + { + LinkManager::instance()->removeLink(link); //remove link from LinkManager list + link->disconnect(); //disconnect port, and also calls terminate() to stop the thread + if (link->isRunning()) link->terminate(); // terminate() the serial thread just in case it is still running + link->wait(); // wait() until thread is stoped before deleting + delete link; + } + link=NULL; + this->window()->close(); - qDebug() << "TODO: Link cannot be deleted: CommConfigurationWindow::remove() NOT IMPLEMENTED!"; } void CommConfigurationWindow::connectionState(bool connect) diff --git a/src/ui/CommConfigurationWindow.h b/src/ui/CommConfigurationWindow.h index fce37dbbe380be280ff7f347bfb78d4b2b0a99c7..93f1566aeceb26a15d5f080291402eaa40f470bc 100644 --- a/src/ui/CommConfigurationWindow.h +++ b/src/ui/CommConfigurationWindow.h @@ -42,7 +42,14 @@ This file is part of the QGROUNDCONTROL project enum qgc_link_t { QGC_LINK_SERIAL, - QGC_LINK_UDP + QGC_LINK_UDP, + QGC_LINK_SIMULATION, + QGC_LINK_FORWARDING +}; + +enum qgc_protocol_t +{ + QGC_PROTOCOL_MAVLINK }; #ifdef OPAL_RT diff --git a/src/ui/CommSettings.ui b/src/ui/CommSettings.ui index 8aec750ea97e720f99ae659f9a5c723cae78563f..71a4c30cd881d5eda30099d868921d2cd96e4d38 100644 --- a/src/ui/CommSettings.ui +++ b/src/ui/CommSettings.ui @@ -43,23 +43,7 @@ - - - - Serial Link - - - - - UDP - - - - - Simulation - - - + @@ -71,13 +55,8 @@ - 0 + -1 - - - MAVLink - - diff --git a/src/ui/HUD.cc b/src/ui/HUD.cc index 872f2b9659566cffd36883cd97fe28a144768bda..c95107ea10545701aa35b8e4ef550167f9809258 100644 --- a/src/ui/HUD.cc +++ b/src/ui/HUD.cc @@ -165,6 +165,19 @@ HUD::~HUD() } +void HUD::showEvent(QShowEvent* event) +{ + Q_UNUSED(event); + if (isVisible()) + { + refreshTimer->start(); + } + else + { + refreshTimer->stop(); + } +} + void HUD::start() { refreshTimer->start(); diff --git a/src/ui/HUD.h b/src/ui/HUD.h index 9255ac788e720d6e99e7dc92306f6267cd43ce03..658054de3ce8a67bb096da831e8db85925c5565a 100644 --- a/src/ui/HUD.h +++ b/src/ui/HUD.h @@ -123,6 +123,8 @@ 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); QImage* image; ///< Double buffer image QImage glImage; ///< The background / camera image diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index 9d149644ece5971a715abe4d5db231bca9f91bf8..b19f097ce382d47d4d4833134ca9b8372b2a355e 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -16,6 +16,7 @@ #include #include "MG.h" +#include "QGC.h" #include "MAVLinkSimulationLink.h" #include "SerialLink.h" #include "UDPLink.h" @@ -44,8 +45,10 @@ * * @see QMainWindow::show() **/ -MainWindow::MainWindow(QWidget *parent) : +MainWindow::MainWindow(QWidget *parent): QMainWindow(parent), + toolsMenuActions(), + currentView(VIEW_MAVLINK), settings() { this->hide(); @@ -54,11 +57,11 @@ MainWindow::MainWindow(QWidget *parent) : // Setup user interface ui.setupUi(this); - buildWidgets(); + buildCommonWidgets(); - connectWidgets(); + connectCommonWidgets(); - arrangeCenterStack(); + arrangeCommonCenterStack(); configureWindowName(); @@ -72,15 +75,41 @@ MainWindow::MainWindow(QWidget *parent) : // Set style sheet as last step reloadStylesheet(); - // Create actions - connectActions(); + connectCommonActions(); - // Load widgets and show application windowa - loadWidgets(); + // Load mavlink view as default widget set + loadMAVLinkView(); // 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()) + { + 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()); + } + } + } + settings.endGroup(); + } + + + + this->show(); } MainWindow::~MainWindow() @@ -89,8 +118,148 @@ MainWindow::~MainWindow() statusBar = NULL; } +void MainWindow::buildCommonWidgets() +{ + //TODO: move protocol outside UI + mavlink = new MAVLinkProtocol(); + + // Dock widgets + controlDockWidget = new QDockWidget(tr("Control"), this); + controlDockWidget->setWidget( new UASControlWidget(this) ); + addToToolsMenu (controlDockWidget, tr("UAS Control"), SLOT(showToolWidget()), MENU_UAS_CONTROL, Qt::LeftDockWidgetArea); + + listDockWidget = new QDockWidget(tr("Unmanned Systems"), this); + listDockWidget->setWidget( new UASListWidget(this) ); + addToToolsMenu (listDockWidget, tr("UAS List"), SLOT(showToolWidget()), MENU_UAS_LIST, Qt::RightDockWidgetArea); + + waypointsDockWidget = new QDockWidget(tr("Waypoint List"), this); + waypointsDockWidget->setWidget( new WaypointList(this, NULL) ); + addToToolsMenu (waypointsDockWidget, tr("Waypoints List"), SLOT(showToolWidget()), MENU_WAYPOINTS, Qt::BottomDockWidgetArea); + + infoDockWidget = new QDockWidget(tr("Status Details"), this); + infoDockWidget->setWidget( new UASInfoWidget(this) ); + addToToolsMenu (infoDockWidget, tr("Status Details"), SLOT(showToolWidget()), MENU_STATUS, Qt::RightDockWidgetArea); + + + debugConsoleDockWidget = new QDockWidget(tr("Communication Console"), this); + debugConsoleDockWidget->setWidget( new DebugConsole(this) ); + addToToolsMenu (debugConsoleDockWidget, tr("Communication Console"), SLOT(showToolWidget()), MENU_DEBUG_CONSOLE, Qt::BottomDockWidgetArea); + + // Center widgets + mapWidget = new MapWidget(this); + addToCentralWidgetsMenu (mapWidget, "Maps", SLOT(showCentralWidget()),CENTRAL_MAP); + + protocolWidget = new XMLCommProtocolWidget(this); + addToCentralWidgetsMenu (protocolWidget, "Mavlink Generator", SLOT(showCentralWidget()),CENTRAL_PROTOCOL); + + +} + +//======= +//void MainWindow::storeSettings() +//{ +// QSettings settings(QGC::COMPANYNAME, QGC::APPNAME); -void MainWindow::buildWidgets() +// 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 QStringList* acceptList = new QStringList(); @@ -106,121 +275,465 @@ void MainWindow::buildWidgets() acceptList2->append("Battery"); acceptList2->append("Pressure"); - //TODO: move protocol outside UI - mavlink = new MAVLinkProtocol(); - // Center widgets linechartWidget = new Linecharts(this); + addToCentralWidgetsMenu(linechartWidget, "Line Plots", SLOT(showCentralWidget()), CENTRAL_LINECHART); + + hudWidget = new HUD(320, 240, this); - mapWidget = new MapWidget(this); - protocolWidget = new XMLCommProtocolWidget(this); + addToCentralWidgetsMenu(hudWidget, "HUD", SLOT(showCentralWidget()), CENTRAL_HUD); + dataplotWidget = new QGCDataPlot2D(this); + addToCentralWidgetsMenu(dataplotWidget, "Data Plots", SLOT(showCentralWidget()), CENTRAL_DATA_PLOT); + #ifdef QGC_OSG_ENABLED _3DWidget = Q3DWidgetFactory::get("PIXHAWK"); + addToCentralWidgetsMenu(_3DWidget, "Local 3D", SLOT(showCentralWidget()), CENTRAL_3D_LOCAL); + #endif #ifdef QGC_OSGEARTH_ENABLED _3DMapWidget = Q3DWidgetFactory::get("MAP3D"); + addToCentralWidgetsMenu(_3DMapWidget, "OSG Earth 3D", SLOT(showCentralWidget()), CENTRAL_OSGEARTH); + #endif #if (defined Q_OS_WIN) | (defined Q_OS_MAC) gEarthWidget = new QGCGoogleEarthView(this); + addToCentralWidgetsMenu(gEarthWidget, "Google Earth", SLOT(showCentralWidget()), CENTRAL_GOOGLE_EARTH); + #endif // Dock widgets - controlDockWidget = new QDockWidget(tr("Control"), this); - controlDockWidget->setWidget( new UASControlWidget(this) ); - - listDockWidget = new QDockWidget(tr("Unmanned Systems"), this); - listDockWidget->setWidget( new UASListWidget(this) ); - - waypointsDockWidget = new QDockWidget(tr("Waypoint List"), this); - waypointsDockWidget->setWidget( new WaypointList(this, NULL) ); - - infoDockWidget = new QDockWidget(tr("Status Details"), this); - infoDockWidget->setWidget( new UASInfoWidget(this) ); detectionDockWidget = new QDockWidget(tr("Object Recognition"), this); detectionDockWidget->setWidget( new ObjectDetectionView("images/patterns", this) ); + addToToolsMenu (detectionDockWidget, tr("Object Recognition"), SLOT(showToolWidget()), MENU_DETECTION, Qt::RightDockWidgetArea); - debugConsoleDockWidget = new QDockWidget(tr("Communication Console"), this); - debugConsoleDockWidget->setWidget( new DebugConsole(this) ); parametersDockWidget = new QDockWidget(tr("Onboard Parameters"), this); parametersDockWidget->setWidget( new ParameterInterface(this) ); + addToToolsMenu (parametersDockWidget, tr("Onboard Parameters"), SLOT(showToolWidget()), MENU_PARAMETERS, Qt::RightDockWidgetArea); watchdogControlDockWidget = new QDockWidget(tr("Process Control"), this); watchdogControlDockWidget->setWidget( new WatchdogControl(this) ); + addToToolsMenu (watchdogControlDockWidget, tr("Process Control"), SLOT(showToolWidget()), MENU_WATCHDOG, Qt::BottomDockWidgetArea); + hsiDockWidget = new QDockWidget(tr("Horizontal Situation Indicator"), this); hsiDockWidget->setWidget( new HSIDisplay(this) ); - - headDown1DockWidget = new QDockWidget(tr("Primary Flight Display"), 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 + + 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); + headDown2DockWidget = new QDockWidget(tr("Payload Status"), this); headDown2DockWidget->setWidget( new HDDisplay(acceptList2, this) ); + addToToolsMenu (headDown2DockWidget, tr("Payload Status"), SLOT(showToolWidget()), MENU_HDD_2, Qt::RightDockWidgetArea); rcViewDockWidget = new QDockWidget(tr("Radio Control"), this); rcViewDockWidget->setWidget( new QGCRemoteControlView(this) ); + addToToolsMenu (rcViewDockWidget, tr("Radio Control"), SLOT(showToolWidget()), MENU_RC_VIEW, Qt::BottomDockWidgetArea); +//======= +// addDockWidget(Qt::RightDockWidgetArea, headDown1DockWidget); - headUpDockWidget = new QDockWidget(tr("Control Indicator"), this); +// 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 + + 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 joystick = new JoystickInput(); +} + +void MainWindow::buildSlugsWidgets() +{ + // Center widgets + linechartWidget = new Linecharts(this); + + // Dock widgets + headUpDockWidget = new QDockWidget(tr("Control Indicator"), this); + headUpDockWidget->setWidget( new HUD(320, 240, this)); + addToToolsMenu (headUpDockWidget, tr("HUD"), SLOT(showToolWidget()), MENU_HUD, Qt::LeftDockWidgetArea); + + + rcViewDockWidget = new QDockWidget(tr("Radio Control"), this); + rcViewDockWidget->setWidget( new QGCRemoteControlView(this) ); + addToToolsMenu (rcViewDockWidget, tr("Radio Control"), SLOT(showToolWidget()), MENU_RC_VIEW, Qt::BottomDockWidgetArea); + + + // Dialog widgets slugsDataWidget = new QDockWidget(tr("Slugs Data"), this); slugsDataWidget->setWidget( new SlugsDataSensorView(this)); + addToToolsMenu (slugsDataWidget, tr("Telemetry Data"), SLOT(showToolWidget()), MENU_SLUGS_DATA, Qt::RightDockWidgetArea); - slugsPIDControlWidget = new QDockWidget(tr("PID Control"), this); +//======= +// this->addDockWidget(Qt::LeftDockWidgetArea, headUpDockWidget); + +// // SLUGS +// slugsDataWidget = new QDockWidget(tr("Slugs Data"), this); +// slugsDataWidget->setWidget( new SlugsDataSensorView(this)); +// addDockWidget(Qt::LeftDockWidgetArea, slugsDataWidget); +// slugsDataWidget->hide(); +//>>>>>>> master + + 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); slugsHilSimWidget = new QDockWidget(tr("Slugs Hil Sim"), this); slugsHilSimWidget->setWidget( new SlugsHilSim(this)); - - slugsCamControlWidget = new QDockWidget(tr("Video Camera Control"), this); + addToToolsMenu (slugsHilSimWidget, tr("HIL Sim Configuration"), SLOT(showToolWidget()), MENU_SLUGS_HIL, Qt::LeftDockWidgetArea); +//======= +// addDockWidget(Qt::BottomDockWidgetArea, slugsPIDControlWidget); +// slugsPIDControlWidget->hide(); + +// slugsHilSimWidget = new QDockWidget(tr("Slugs Hil Sim"), this); +// slugsHilSimWidget->setWidget( new SlugsHilSim(this)); +// addDockWidget(Qt::BottomDockWidgetArea, slugsHilSimWidget); +// slugsHilSimWidget->hide(); +//>>>>>>> master + + 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){ + QAction* tempAction; + + + // Add the separator that will separate tools from central Widgets + if (!toolsMenuActions[CENTRAL_SEPARATOR]){ + tempAction = ui.menuTools->addSeparator(); + toolsMenuActions[CENTRAL_SEPARATOR] = tempAction; + tempAction->setData(CENTRAL_SEPARATOR); + } + + tempAction = ui.menuTools->addAction(title); + + tempAction->setCheckable(true); + tempAction->setData(centralWidget); + + // populate the Hashes + toolsMenuActions[centralWidget] = tempAction; + dockWidgets[centralWidget] = widget; + + QString chKey = buildMenuKey(SUB_SECTION_CHECKED, centralWidget, currentView); + + if (!settings.contains(chKey)){ + settings.setValue(chKey,false); + tempAction->setChecked(false); + } +// else { +// tempAction->setChecked(settings.value(chKey).toBool()); +// } + + // connect the action + connect(tempAction,SIGNAL(triggered()),this, slotName); + +} + + +void MainWindow::showCentralWidget(){ + QAction* senderAction = qobject_cast(sender()); + int tool = senderAction->data().toInt(); + QString chKey; + + // check the current action + + if (senderAction && dockWidgets[tool]){ + + // uncheck all central widget actions + QHashIterator i(toolsMenuActions); + while (i.hasNext()) { + i.next(); + qDebug() << "shCW" << i.key() << "read"; + if (i.value() && i.value()->data().toInt() > 255){ + i.value()->setChecked(false); + // update the settings + chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(i.value()->data().toInt()), currentView); + settings.setValue(chKey,false); + } + } + + // check the current action + qDebug() << senderAction->text(); + senderAction->setChecked(true); + + // update the central widget + centerStack->setCurrentWidget(dockWidgets[tool]); + + // store the selected central widget + chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(tool), currentView); + settings.setValue(chKey,true); + + presentView(); + } +} + +void MainWindow::addToToolsMenu ( QWidget* widget, + const QString title, + const char * slotName, + TOOLS_WIDGET_NAMES tool, + Qt::DockWidgetArea location){ + QAction* tempAction; + QString posKey, chKey; + + + if (toolsMenuActions[CENTRAL_SEPARATOR]){ + tempAction = new QAction(title, this); + ui.menuTools->insertAction(toolsMenuActions[CENTRAL_SEPARATOR], + tempAction); + } else { + tempAction = ui.menuTools->addAction(title); + } + + tempAction->setCheckable(true); + tempAction->setData(tool); + + // populate the Hashes + toolsMenuActions[tool] = tempAction; + dockWidgets[tool] = widget; + qDebug() << widget; + + posKey = buildMenuKey (SUB_SECTION_LOCATION,tool, currentView); + + if (!settings.contains(posKey)){ + settings.setValue(posKey,location); + dockWidgetLocations[tool] = location; + } else { + dockWidgetLocations[tool] = static_cast (settings.value(posKey).toInt()); + } + + chKey = buildMenuKey(SUB_SECTION_CHECKED,tool, currentView); + + if (!settings.contains(chKey)){ + settings.setValue(chKey,false); + tempAction->setChecked(false); + } else { + tempAction->setChecked(settings.value(chKey).toBool()); + } + + // connect the action + connect(tempAction,SIGNAL(triggered()),this, slotName); + + connect(qobject_cast (dockWidgets[tool]), + SIGNAL(visibilityChanged(bool)), this, SLOT(updateVisibilitySettings(bool))); + + connect(qobject_cast (dockWidgets[tool]), + SIGNAL(dockLocationChanged(Qt::DockWidgetArea)), this, SLOT(updateLocationSettings(Qt::DockWidgetArea))); + +} + +void MainWindow::showToolWidget(){ + QAction* temp = qobject_cast(sender()); + int tool = temp->data().toInt(); + + + if (temp && dockWidgets[tool]){ + if (temp->isChecked()){ + addDockWidget(dockWidgetLocations[tool], qobject_cast (dockWidgets[tool])); + qobject_cast(dockWidgets[tool])->show(); + } else { + removeDockWidget(qobject_cast(dockWidgets[tool])); + } + QString chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(tool), currentView); + settings.setValue(chKey,temp->isChecked()); + } +} + + +void MainWindow::showTheWidget (TOOLS_WIDGET_NAMES widget, VIEW_SECTIONS view){ + bool tempVisible; + Qt::DockWidgetArea tempLocation; + QDockWidget* tempWidget = static_cast (dockWidgets[widget]); + + tempVisible = settings.value(buildMenuKey (SUB_SECTION_CHECKED,widget,view)).toBool(); + + if (tempWidget){ + toolsMenuActions[widget]->setChecked(tempVisible); + } + + + //qDebug() << buildMenuKey (SUB_SECTION_CHECKED,widget,view) << tempVisible; + + tempLocation = static_cast (settings.value(buildMenuKey (SUB_SECTION_LOCATION,widget, view)).toInt()); + + if (tempWidget && tempVisible){ + addDockWidget(tempLocation, tempWidget); + tempWidget->show(); + } + +} + +QString MainWindow::buildMenuKey(SETTINGS_SECTIONS section, TOOLS_WIDGET_NAMES tool, VIEW_SECTIONS view){ + // Key is built as follows: autopilot_type/section_menu/view/tool/section + int apType; + + apType = (UASManager::instance() && UASManager::instance()->silentGetActiveUAS())? + UASManager::instance()->getActiveUAS()->getAutopilotType(): + -1; + + return (QString::number(apType) + "/" + + QString::number(SECTION_MENU) + "/" + + QString::number(view) + "/" + + QString::number(tool) + "/" + + QString::number(section) + "/" ); +} + +void MainWindow::updateVisibilitySettings (bool vis){ + Q_UNUSED(vis); + + // This is commented since when the application closes + // sets the visibility to false. + + // TODO: A workaround is needed. The QApplication::aboutToQuit + // did not work + + /* + QDockWidget* temp = qobject_cast(sender()); + + QHashIterator i(dockWidgets); + while (i.hasNext()) { + i.next(); + if ((static_cast (dockWidgets[i.key()])) == temp){ + QString chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(i.key())); + qDebug() << "Key in visibility changed" << chKey; + settings.setValue(chKey,vis); + toolsMenuActions[i.key()]->setChecked(vis); + break; + } + } +*/ +} + +void MainWindow::updateLocationSettings (Qt::DockWidgetArea location){ + QDockWidget* temp = qobject_cast(sender()); + + QHashIterator i(dockWidgets); + while (i.hasNext()) { + i.next(); + if ((static_cast (dockWidgets[i.key()])) == temp){ + QString posKey = buildMenuKey (SUB_SECTION_LOCATION,static_cast(i.key()), currentView); + settings.setValue(posKey,location); + break; + } + } +//======= +// addDockWidget(Qt::BottomDockWidgetArea, slugsCamControlWidget); +// slugsCamControlWidget->hide(); + +// //FIXME: free memory in destructor +// joystick = new JoystickInput(); +//>>>>>>> master } /** - * Connect all signals and slots of the main window widgets + * Connect the signals and slots of the common window widgets */ -void MainWindow::connectWidgets() +void MainWindow::connectCommonWidgets() { - if (linechartWidget) - { - connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)), - linechartWidget, SLOT(addSystem(UASInterface*))); - connect(UASManager::instance(), SIGNAL(activeUASSet(int)), - linechartWidget, SLOT(selectSystem(int))); - connect(linechartWidget, SIGNAL(logfileWritten(QString)), - this, SLOT(loadDataView(QString))); - } if (infoDockWidget && infoDockWidget->widget()) { connect(mavlink, SIGNAL(receiveLossChanged(int, float)), infoDockWidget->widget(), SLOT(updateSendLoss(int, float))); } + if (mapWidget && waypointsDockWidget->widget()) { // clear path create on the map 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))); +} + +void MainWindow::connectPxWidgets() +{ + if (linechartWidget) + { + connect(linechartWidget, SIGNAL(logfileWritten(QString)), + this, SLOT(loadDataView(QString))); + } + +} + +void MainWindow::connectSlugsWidgets() +{ + if (linechartWidget) + { + connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)), + linechartWidget, SLOT(addSystem(UASInterface*))); + connect(UASManager::instance(), SIGNAL(activeUASSet(int)), + linechartWidget, SLOT(selectSystem(int))); + connect(linechartWidget, SIGNAL(logfileWritten(QString)), + this, SLOT(loadDataView(QString))); } if (slugsHilSimWidget && slugsHilSimWidget->widget()){ @@ -236,14 +749,28 @@ void MainWindow::connectWidgets() } -void MainWindow::arrangeCenterStack() +void MainWindow::arrangeCommonCenterStack() { + centerStack = new QStackedWidget(this); - QStackedWidget *centerStack = new QStackedWidget(this); if (!centerStack) return; - if (linechartWidget) centerStack->addWidget(linechartWidget); - if (protocolWidget) centerStack->addWidget(protocolWidget); + if (mapWidget) centerStack->addWidget(mapWidget); + if (protocolWidget) centerStack->addWidget(protocolWidget); + + setCentralWidget(centerStack); +} + +void MainWindow::arrangePxCenterStack() +{ + + if (!centerStack) { + qDebug() << "Center Stack not Created!"; + return; + } + + if (linechartWidget) centerStack->addWidget(linechartWidget); + #ifdef QGC_OSG_ENABLED if (_3DWidget) centerStack->addWidget(_3DWidget); #endif @@ -256,7 +783,21 @@ void MainWindow::arrangeCenterStack() if (hudWidget) centerStack->addWidget(hudWidget); if (dataplotWidget) centerStack->addWidget(dataplotWidget); - setCentralWidget(centerStack); +} + +void MainWindow::arrangeSlugsCenterStack() +{ + + if (!centerStack) { + qDebug() << "Center Stack not Created!"; + return; + } + + if (linechartWidget) centerStack->addWidget(linechartWidget); + + + if (hudWidget) centerStack->addWidget(hudWidget); + } void MainWindow::configureWindowName() @@ -360,11 +901,14 @@ void MainWindow::reloadStylesheet() void MainWindow::showStatusMessage(const QString& status, int timeout) { + Q_UNUSED(status); + Q_UNUSED(timeout); //statusBar->showMessage(status, timeout); } void MainWindow::showStatusMessage(const QString& status) { + Q_UNUSED(status); //statusBar->showMessage(status, 5); } @@ -372,15 +916,16 @@ void MainWindow::showStatusMessage(const QString& status) * @brief Create all actions associated to the main window * **/ -void MainWindow::connectActions() +void MainWindow::connectCommonActions() { + // Connect actions from ui connect(ui.actionAdd_Link, SIGNAL(triggered()), this, SLOT(addLink())); // Connect internal actions connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)), this, SLOT(UASCreated(UASInterface*))); - // Connect user interface controls + // Unmanned System controls connect(ui.actionLiftoff, SIGNAL(triggered()), UASManager::instance(), SLOT(launchActiveUAS())); connect(ui.actionLand, SIGNAL(triggered()), UASManager::instance(), SLOT(returnActiveUAS())); connect(ui.actionEmergency_Land, SIGNAL(triggered()), UASManager::instance(), SLOT(stopActiveUAS())); @@ -388,43 +933,33 @@ void MainWindow::connectActions() connect(ui.actionConfiguration, SIGNAL(triggered()), UASManager::instance(), SLOT(configureActiveUAS())); - // User interface actions - connect(ui.actionPilotView, SIGNAL(triggered()), this, SLOT(loadPilotView())); - connect(ui.actionEngineerView, SIGNAL(triggered()), this, SLOT(loadEngineerView())); - connect(ui.actionOperatorView, SIGNAL(triggered()), this, SLOT(loadOperatorView())); -#ifdef QGC_OSG_ENABLED - connect(ui.action3DView, SIGNAL(triggered()), this, SLOT(load3DView())); -#else - ui.menuWindow->removeAction(ui.action3DView); -#endif + // Views actions + connect(ui.actionPilotsView, SIGNAL(triggered()), this, SLOT(loadPilotView())); + connect(ui.actionEngineersView, SIGNAL(triggered()), this, SLOT(loadEngineerView())); + connect(ui.actionOperatorsView, SIGNAL(triggered()), this, SLOT(loadOperatorView())); -#ifdef QGC_OSGEARTH_ENABLED - connect(ui.action3DMapView, SIGNAL(triggered()), this, SLOT(load3DMapView())); -#else - ui.menuWindow->removeAction(ui.action3DMapView); -#endif - connect(ui.actionShow_full_view, SIGNAL(triggered()), this, SLOT(loadAllView())); - connect(ui.actionShow_MAVLink_view, SIGNAL(triggered()), this, SLOT(loadMAVLinkView())); - connect(ui.actionShow_data_analysis_view, SIGNAL(triggered()), this, SLOT(loadDataView())); - connect(ui.actionStyleConfig, SIGNAL(triggered()), this, SLOT(reloadStylesheet())); - connect(ui.actionGlobalOperatorView, SIGNAL(triggered()), this, SLOT(loadGlobalOperatorView())); + connect(ui.actionMavlinkView, SIGNAL(triggered()), this, SLOT(loadMAVLinkView())); + connect(ui.actionReloadStyle, SIGNAL(triggered()), this, SLOT(reloadStylesheet())); + + // Help Actions connect(ui.actionOnline_documentation, SIGNAL(triggered()), this, SLOT(showHelp())); - connect(ui.actionCredits_Developers, SIGNAL(triggered()), this, SLOT(showCredits())); + connect(ui.actionDeveloper_Credits, 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())); +void MainWindow::connectPxActions() +{ - //GlobalOperatorView - // connect(ui.actionGlobalOperatorView,SIGNAL(triggered()),waypointsDockWidget->widget(),SLOT()) + ui.actionJoystickSettings->setVisible(true); + + // Joystick configuration + connect(ui.actionJoystickSettings, SIGNAL(triggered()), this, SLOT(configure())); + +} + +void MainWindow::connectSlugsActions() +{ } @@ -579,21 +1114,60 @@ void MainWindow::UASCreated(UASInterface* uas) // TODO Stylesheet reloading should in theory not be necessary reloadStylesheet(); - // Check which type this UAS is of - PxQuadMAV* mav = dynamic_cast(uas); - if (mav) loadPixhawkView(); - - if (slugsDataWidget) { - SlugsDataSensorView* dataWidget = dynamic_cast(slugsDataWidget->widget()); - if (dataWidget) { - SlugsMAV* mav2 = dynamic_cast(uas); - if (mav2) { - dataWidget->addUAS(uas); - //loadSlugsView(); - loadGlobalOperatorView(); - } - } + switch (uas->getAutopilotType()){ + case (MAV_AUTOPILOT_GENERIC): + case (MAV_AUTOPILOT_ARDUPILOTMEGA): + case (MAV_AUTOPILOT_PIXHAWK): + { + // Build Pixhawk Widgets + buildPxWidgets(); + + // Connect Pixhawk Widgets + connectPxWidgets(); + + // Arrange Pixhawk Centerstack + arrangePxCenterStack(); + + // Connect Pixhawk Actions + connectPxActions(); + + // FIXME: This type checking might be redundant + // Check which type this UAS is of +// PxQuadMAV* mav = dynamic_cast(uas); +// if (mav) loadPixhawkEngineerView(); + } break; + + case (MAV_AUTOPILOT_SLUGS): + { + // Build Slugs Widgets + buildSlugsWidgets(); + + // Connect Slugs Widgets + connectSlugsWidgets(); + + // Arrange Slugs Centerstack + arrangeSlugsCenterStack(); + + // Connect Slugs Actions + connectSlugsActions(); + + // FIXME: This type checking might be redundant +// if (slugsDataWidget) { +// SlugsDataSensorView* dataWidget = dynamic_cast(slugsDataWidget->widget()); +// if (dataWidget) { +// SlugsMAV* mav2 = dynamic_cast(uas); +// if (mav2) { + (dynamic_cast(slugsDataWidget->widget()))->addUAS(uas); +// //loadSlugsView(); +// loadGlobalOperatorView(); +// } +// } +// } + } break; + + loadEngineerView(); } + } } @@ -602,21 +1176,25 @@ void MainWindow::UASCreated(UASInterface* uas) */ void MainWindow::clearView() { - // Halt HUD + // Halt HUD central widget if (hudWidget) hudWidget->stop(); + // Disable linechart if (linechartWidget) linechartWidget->setActive(false); + // Halt HDDs if (headDown1DockWidget) { HDDisplay* hddWidget = dynamic_cast( headDown1DockWidget->widget() ); if (hddWidget) hddWidget->stop(); } + if (headDown2DockWidget) { HDDisplay* hddWidget = dynamic_cast( headDown2DockWidget->widget() ); if (hddWidget) hddWidget->stop(); } + // Halt HSI if (hsiDockWidget) { @@ -624,6 +1202,13 @@ void MainWindow::clearView() if (hsi) hsi->stop(); } + // Halt HUD if in docked widget mode + if (headUpDockWidget) + { + HUD* hud = dynamic_cast( headUpDockWidget->widget() ); + if (hud) hud->stop(); + } + // Remove all dock widgets from main window QObjectList childList( this->children() ); @@ -635,7 +1220,8 @@ void MainWindow::clearView() if (dockWidget) { // Remove dock widget from main window - this->removeDockWidget(dockWidget); + //this->removeDockWidget(dockWidget); + dockWidget->setVisible(false); // Deletion of dockWidget would also delete all child // widgets of dockWidget // Is there a way to unset a widget from QDockWidget? @@ -643,194 +1229,266 @@ void MainWindow::clearView() } } -void MainWindow::loadSlugsView() +void MainWindow::loadEngineerView() { - clearView(); - // Engineer view, used in EMAV2009 - - // LINE CHART - if (linechartWidget) - { - QStackedWidget *centerStack = dynamic_cast(centralWidget()); - if (centerStack) - { - linechartWidget->setActive(true); - centerStack->setCurrentWidget(linechartWidget); - } - } + clearView(); - // UAS CONTROL - if (controlDockWidget) - { - addDockWidget(Qt::LeftDockWidgetArea, controlDockWidget); - controlDockWidget->show(); - } + currentView = VIEW_ENGINEER; - // UAS LIST - if (listDockWidget) - { - addDockWidget(Qt::BottomDockWidgetArea, listDockWidget); - listDockWidget->show(); - } + presentView(); +} - // UAS STATUS - if (infoDockWidget) - { - addDockWidget(Qt::LeftDockWidgetArea, infoDockWidget); - infoDockWidget->show(); - } +void MainWindow::loadOperatorView() +{ + clearView(); + currentView = VIEW_OPERATOR; - // WAYPOINT LIST - if (waypointsDockWidget) - { - addDockWidget(Qt::BottomDockWidgetArea, waypointsDockWidget); - waypointsDockWidget->show(); - } + presentView(); +} - // DEBUG CONSOLE - if (debugConsoleDockWidget) - { - addDockWidget(Qt::BottomDockWidgetArea, debugConsoleDockWidget); - debugConsoleDockWidget->show(); - } +void MainWindow::loadPilotView() +{ + clearView(); - // Slugs Data View - if (slugsDataWidget) - { - addDockWidget(Qt::RightDockWidgetArea, slugsDataWidget); - slugsDataWidget->show(); - } + currentView = VIEW_PILOT; - // Slugs Data View - if (slugsHilSimWidget) - { - addDockWidget(Qt::LeftDockWidgetArea, slugsHilSimWidget); - slugsHilSimWidget->show(); - } - this->show(); + presentView(); } -void MainWindow::loadPixhawkView() +void MainWindow::loadMAVLinkView() { clearView(); - // Engineer view, used in EMAV2009 -#ifdef QGC_OSG_ENABLED - // 3D map - if (_3DWidget) - { - QStackedWidget *centerStack = dynamic_cast(centralWidget()); - if (centerStack) - { - //map3DWidget->setActive(true); - centerStack->setCurrentWidget(_3DWidget); - } - } -#endif + currentView = VIEW_MAVLINK; - // UAS CONTROL - if (controlDockWidget) - { - addDockWidget(Qt::LeftDockWidgetArea, controlDockWidget); - controlDockWidget->show(); - } + presentView(); +//======= +// // Slugs Data View +// if (slugsHilSimWidget) +// { +// addDockWidget(Qt::LeftDockWidgetArea, slugsHilSimWidget); +// slugsHilSimWidget->show(); +// } +//>>>>>>> master +} - // HORIZONTAL SITUATION INDICATOR - if (hsiDockWidget) - { - HSIDisplay* hsi = dynamic_cast( hsiDockWidget->widget() ); - if (hsi) - { - hsi->start(); - addDockWidget(Qt::LeftDockWidgetArea, hsiDockWidget); - hsiDockWidget->show(); - } - } +void MainWindow::presentView() { - // UAS LIST - if (listDockWidget) - { - addDockWidget(Qt::BottomDockWidgetArea, listDockWidget); - listDockWidget->show(); - } - // UAS STATUS - if (infoDockWidget) - { - addDockWidget(Qt::LeftDockWidgetArea, infoDockWidget); - infoDockWidget->show(); - } +#ifdef QGC_OSG_ENABLED + // 3D map + if (_3DWidget) + { + if (centerStack) + { + //map3DWidget->setActive(true); + centerStack->setCurrentWidget(_3DWidget); + } + } +#endif - // WAYPOINT LIST - if (waypointsDockWidget) - { - addDockWidget(Qt::BottomDockWidgetArea, waypointsDockWidget); - waypointsDockWidget->show(); - } + 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"; + showTheCentralWidget(CENTRAL_MAP, currentView); + + // PROTOCOL + qDebug() << "CP"; + showTheCentralWidget(CENTRAL_PROTOCOL, currentView); + + // 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(); + } + } + + // Show docked widgets based on current view and autopilot type + + // UAS CONTROL + showTheWidget(MENU_UAS_CONTROL, currentView); + + // UAS LIST + showTheWidget(MENU_UAS_LIST, currentView); + + // WAYPOINT LIST + showTheWidget(MENU_WAYPOINTS, currentView); + + // UAS STATUS + showTheWidget(MENU_STATUS, currentView); + + // DETECTION + showTheWidget(MENU_DETECTION, currentView); + + // DEBUG CONSOLE + showTheWidget(MENU_DEBUG_CONSOLE, currentView); + + // ONBOARD PARAMETERS + showTheWidget(MENU_PARAMETERS, currentView); + + // WATCHDOG + showTheWidget(MENU_WATCHDOG, currentView); + + // HUD + showTheWidget(MENU_HUD, currentView); + if (headUpDockWidget) + { + HUD* tmpHud = dynamic_cast( headUpDockWidget->widget() ); + if (tmpHud){ + if (settings.value(buildMenuKey (SUB_SECTION_CHECKED,MENU_HUD,currentView)).toBool()){ + tmpHud->start(); + addDockWidget(static_cast (settings.value(buildMenuKey (SUB_SECTION_LOCATION,MENU_HUD, currentView)).toInt()), + hsiDockWidget); + headUpDockWidget->show(); + } else { + tmpHud->stop(); + headUpDockWidget->hide(); + } + } + } + + + // RC View + showTheWidget(MENU_RC_VIEW, currentView); + + // SLUGS DATA + showTheWidget(MENU_SLUGS_DATA, currentView); + + // SLUGS PID + showTheWidget(MENU_SLUGS_PID, currentView); + + // SLUGS HIL + showTheWidget(MENU_SLUGS_HIL, currentView); + + // SLUGS CAMERA + showTheWidget(MENU_SLUGS_CAMERA, currentView); + + // HORIZONTAL SITUATION INDICATOR + 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(); + } + } + } + + // 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(); + } + } + } + + // 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(); + } + } + } - // DEBUG CONSOLE - if (debugConsoleDockWidget) - { - addDockWidget(Qt::BottomDockWidgetArea, debugConsoleDockWidget); - debugConsoleDockWidget->show(); - } + this->show(); - // ONBOARD PARAMETERS - if (parametersDockWidget) - { - addDockWidget(Qt::RightDockWidgetArea, parametersDockWidget); - parametersDockWidget->show(); - } +} - this->show(); +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]){ + toolsMenuActions[centralWidget]->setChecked(tempVisible); + } + + if (centerStack && tempWidget && tempVisible){ + centerStack->setCurrentWidget(tempWidget); + } } -void MainWindow::loadDataView() -{ - clearView(); - // DATAPLOT - if (dataplotWidget) - { - QStackedWidget *centerStack = dynamic_cast(centralWidget()); - if (centerStack) - centerStack->setCurrentWidget(dataplotWidget); - } -} -void MainWindow::loadDataView(QString fileName) +/* +========================================================== + Potentially Deprecated +========================================================== +*/ + +void MainWindow::loadPixhawkEngineerView() { - clearView(); - // DATAPLOT - if (dataplotWidget) - { - QStackedWidget *centerStack = dynamic_cast(centralWidget()); - if (centerStack) - { - centerStack->setCurrentWidget(dataplotWidget); - dataplotWidget->loadFile(fileName); - } - } } -void MainWindow::loadPilotView() + +void MainWindow::loadAllView() { clearView(); - // HEAD UP DISPLAY - if (hudWidget) - { - QStackedWidget *centerStack = dynamic_cast(centralWidget()); - if (centerStack) - { - centerStack->setCurrentWidget(hudWidget); - hudWidget->start(); - } - } - - //connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), pfd, SLOT(setActiveUAS(UASInterface*))); if (headDown1DockWidget) { HDDisplay *hdd = dynamic_cast(headDown1DockWidget->widget()); @@ -840,7 +1498,7 @@ void MainWindow::loadPilotView() headDown1DockWidget->show(); hdd->start(); } - + } if (headDown2DockWidget) { @@ -852,23 +1510,24 @@ void MainWindow::loadPilotView() hdd->start(); } } +//<<<<<<< HEAD +//======= +//} - this->show(); -} - -void MainWindow::loadOperatorView() -{ - clearView(); +//void MainWindow::loadOperatorView() +//{ +// clearView(); - // MAP - if (mapWidget) - { - QStackedWidget *centerStack = dynamic_cast(centralWidget()); - if (centerStack) - { - centerStack->setCurrentWidget(mapWidget); - } - } +// // MAP +// if (mapWidget) +// { +// QStackedWidget *centerStack = dynamic_cast(centralWidget()); +// if (centerStack) +// { +// centerStack->setCurrentWidget(mapWidget); +// } +// } +//>>>>>>> master // UAS CONTROL if (controlDockWidget) @@ -898,16 +1557,11 @@ void MainWindow::loadOperatorView() waypointsDockWidget->show(); } - // HORIZONTAL SITUATION INDICATOR - if (hsiDockWidget) + // DEBUG CONSOLE + if (debugConsoleDockWidget) { - HSIDisplay* hsi = dynamic_cast( hsiDockWidget->widget() ); - if (hsi) - { - addDockWidget(Qt::BottomDockWidgetArea, hsiDockWidget); - hsiDockWidget->show(); - hsi->start(); - } + addDockWidget(Qt::BottomDockWidgetArea, debugConsoleDockWidget); + debugConsoleDockWidget->show(); } // OBJECT DETECTION @@ -917,116 +1571,60 @@ void MainWindow::loadOperatorView() detectionDockWidget->show(); } - // PROCESS CONTROL - if (watchdogControlDockWidget) + // LINE CHART + if (linechartWidget) + { + QStackedWidget *centerStack = dynamic_cast(centralWidget()); + if (centerStack) + { + linechartWidget->setActive(true); + centerStack->setCurrentWidget(linechartWidget); + } + } + + // ONBOARD PARAMETERS + if (parametersDockWidget) { - addDockWidget(Qt::RightDockWidgetArea, watchdogControlDockWidget); - watchdogControlDockWidget->show(); + addDockWidget(Qt::RightDockWidgetArea, parametersDockWidget); + parametersDockWidget->show(); } +} - this->show(); +void MainWindow::loadWidgets() +{ + //loadOperatorView(); + loadMAVLinkView(); + //loadPilotView(); } -void MainWindow::loadGlobalOperatorView() +void MainWindow::loadDataView() { clearView(); - // MAP - if (mapWidget) + // DATAPLOT + if (dataplotWidget) { QStackedWidget *centerStack = dynamic_cast(centralWidget()); if (centerStack) - { - centerStack->setCurrentWidget(mapWidget); - } - } - - // WAYPOINT LIST - if (waypointsDockWidget) - { - addDockWidget(Qt::BottomDockWidgetArea, waypointsDockWidget); - waypointsDockWidget->show(); - + centerStack->setCurrentWidget(dataplotWidget); } +} - // Slugs Data View - if (slugsDataWidget) - { - addDockWidget(Qt::RightDockWidgetArea, slugsDataWidget); - slugsDataWidget->show(); - } +void MainWindow::loadDataView(QString fileName) +{ + clearView(); - // Slugs Data View - if (slugsPIDControlWidget) + // DATAPLOT + if (dataplotWidget) { - addDockWidget(Qt::LeftDockWidgetArea, slugsPIDControlWidget); - slugsPIDControlWidget->show(); + QStackedWidget *centerStack = dynamic_cast(centralWidget()); + if (centerStack) + { + centerStack->setCurrentWidget(dataplotWidget); + dataplotWidget->loadFile(fileName); + } } - - if (slugsCamControlWidget) - { - addDockWidget(Qt::BottomDockWidgetArea, slugsCamControlWidget); - slugsCamControlWidget->show(); - } - - - -// // UAS CONTROL -// if (controlDockWidget) -// { -// addDockWidget(Qt::LeftDockWidgetArea, controlDockWidget); -// controlDockWidget->show(); -// } - -// // UAS LIST -// if (listDockWidget) -// { -// addDockWidget(Qt::BottomDockWidgetArea, listDockWidget); -// listDockWidget->show(); -// } - -// // UAS STATUS -// if (infoDockWidget) -// { -// addDockWidget(Qt::LeftDockWidgetArea, infoDockWidget); -// infoDockWidget->show(); -// } - - -// // HORIZONTAL SITUATION INDICATOR -// if (hsiDockWidget) -// { -// HSIDisplay* hsi = dynamic_cast( hsiDockWidget->widget() ); -// if (hsi) -// { -// addDockWidget(Qt::BottomDockWidgetArea, hsiDockWidget); -// hsiDockWidget->show(); -// hsi->start(); -// } -// } - - // PROCESS CONTROL -// if (watchdogControlDockWidget) -// { -// addDockWidget(Qt::RightDockWidgetArea, watchdogControlDockWidget); -// watchdogControlDockWidget->show(); -// } - - // HEAD UP DISPLAY -// if (headUpDockWidget) -// { -// addDockWidget(Qt::RightDockWidgetArea, headUpDockWidget); -// // FIXME Replace with default ->show() call -// HUD* hud = dynamic_cast(headUpDockWidget->widget()); - -// if (hud) -// { -// headUpDockWidget->show(); -// hud->start(); -// } -// } - -} +} void MainWindow::load3DMapView() { @@ -1077,7 +1675,6 @@ void MainWindow::load3DMapView() } } #endif - this->show(); } void MainWindow::loadGoogleEarthView() @@ -1127,12 +1724,10 @@ void MainWindow::loadGoogleEarthView() hsiDockWidget->show(); } } - this->show(); #endif } - void MainWindow::load3DView() { #ifdef QGC_OSG_ENABLED @@ -1182,82 +1777,117 @@ void MainWindow::load3DView() } } #endif - this->show(); - } -void MainWindow::loadEngineerView() +/* + ================================== + ========== ATTIC ================= + ================================== + +void MainWindow::buildCommonWidgets() { - clearView(); - // Engineer view, used in EMAV2009 + //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"); - // LINE CHART - if (linechartWidget) - { - QStackedWidget *centerStack = dynamic_cast(centralWidget()); - if (centerStack) - { - linechartWidget->setActive(true); - centerStack->setCurrentWidget(linechartWidget); - } - } + //FIXME: memory of acceptList2 will never be freed again + QStringList* acceptList2 = new QStringList(); + acceptList2->append("Battery"); + acceptList2->append("Pressure"); - // UAS CONTROL - if (controlDockWidget) - { - addDockWidget(Qt::LeftDockWidgetArea, controlDockWidget); - controlDockWidget->show(); - } + //TODO: move protocol outside UI + mavlink = new MAVLinkProtocol(); - // UAS LIST - if (listDockWidget) - { - addDockWidget(Qt::BottomDockWidgetArea, listDockWidget); - listDockWidget->show(); - } + // 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 - // UAS STATUS - if (infoDockWidget) - { - addDockWidget(Qt::LeftDockWidgetArea, infoDockWidget); - infoDockWidget->show(); - } +#ifdef QGC_OSGEARTH_ENABLED + _3DMapWidget = Q3DWidgetFactory::get("MAP3D"); +#endif +#if (defined Q_OS_WIN) | (defined Q_OS_MAC) + gEarthWidget = new QGCGoogleEarthView(this); +#endif - // WAYPOINT LIST - if (waypointsDockWidget) - { - addDockWidget(Qt::BottomDockWidgetArea, waypointsDockWidget); - waypointsDockWidget->show(); - } + // Dock widgets + controlDockWidget = new QDockWidget(tr("Control"), this); + controlDockWidget->setWidget( new UASControlWidget(this) ); - // DEBUG CONSOLE - if (debugConsoleDockWidget) - { - addDockWidget(Qt::BottomDockWidgetArea, debugConsoleDockWidget); - debugConsoleDockWidget->show(); - } + listDockWidget = new QDockWidget(tr("Unmanned Systems"), this); + listDockWidget->setWidget( new UASListWidget(this) ); - // ONBOARD PARAMETERS - if (parametersDockWidget) - { - addDockWidget(Qt::RightDockWidgetArea, parametersDockWidget); - parametersDockWidget->show(); - } +<<<<<<< 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(); } - - this->show(); } +>>>>>>> master -void MainWindow::loadMAVLinkView() -{ - clearView(); + 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()); @@ -1266,102 +1896,148 @@ void MainWindow::loadMAVLinkView() centerStack->setCurrentWidget(protocolWidget); } } - - this->show(); +>>>>>>> master } -void MainWindow::loadAllView() +void MainWindow::connectWidgets() { - clearView(); - - if (headDown1DockWidget) + if (linechartWidget) { - HDDisplay *hdd = dynamic_cast(headDown1DockWidget->widget()); - if (hdd) - { - addDockWidget(Qt::RightDockWidgetArea, headDown1DockWidget); - headDown1DockWidget->show(); - hdd->start(); - } - + 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 (headDown2DockWidget) + if (infoDockWidget && infoDockWidget->widget()) { - HDDisplay *hdd = dynamic_cast(headDown2DockWidget->widget()); - if (hdd) - { - addDockWidget(Qt::RightDockWidgetArea, headDown2DockWidget); - headDown2DockWidget->show(); - hdd->start(); - } + connect(mavlink, SIGNAL(receiveLossChanged(int, float)), + infoDockWidget->widget(), SLOT(updateSendLoss(int, float))); } - - // UAS CONTROL - if (controlDockWidget) + if (mapWidget && waypointsDockWidget->widget()) { - addDockWidget(Qt::LeftDockWidgetArea, controlDockWidget); - controlDockWidget->show(); - } + // 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)) ); - // UAS LIST - if (listDockWidget) - { - addDockWidget(Qt::BottomDockWidgetArea, listDockWidget); - listDockWidget->show(); - } + // 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))); - // UAS STATUS - if (infoDockWidget) - { - addDockWidget(Qt::LeftDockWidgetArea, infoDockWidget); - infoDockWidget->show(); + connect(slugsCamControlWidget->widget(),SIGNAL(viewCamBorderAtMap(bool)),mapWidget,SLOT(drawBorderCamAtMap(bool))); + connect(slugsCamControlWidget->widget(),SIGNAL(changeCamPosition(double,double,QString)),mapWidget,SLOT(updateCameraPosition(double,double, QString))); } - // WAYPOINT LIST - if (waypointsDockWidget) - { - addDockWidget(Qt::BottomDockWidgetArea, waypointsDockWidget); - waypointsDockWidget->show(); + if (slugsHilSimWidget && slugsHilSimWidget->widget()){ + connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), + slugsHilSimWidget->widget(), SLOT(activeUasSet(UASInterface*))); } - // DEBUG CONSOLE - if (debugConsoleDockWidget) - { - addDockWidget(Qt::BottomDockWidgetArea, debugConsoleDockWidget); - debugConsoleDockWidget->show(); + if (slugsDataWidget && slugsDataWidget->widget()){ + connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), + slugsDataWidget->widget(), SLOT(setActiveUAS(UASInterface*))); } - // 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); - } - } +} +<<<<<<< HEAD +void MainWindow::arrangeCommonCenterStack() +{ + + QStackedWidget *centerStack = new QStackedWidget(this); + if (!centerStack) return; + if (linechartWidget) centerStack->addWidget(linechartWidget); + if (protocolWidget) centerStack->addWidget(protocolWidget); + if (mapWidget) centerStack->addWidget(mapWidget); +#ifdef QGC_OSG_ENABLED + if (_3DWidget) centerStack->addWidget(_3DWidget); +#endif +#ifdef QGC_OSGEARTH_ENABLED + if (_3DMapWidget) centerStack->addWidget(_3DMapWidget); +#endif +#if (defined Q_OS_WIN) | (defined Q_OS_MAC) + if (gEarthWidget) centerStack->addWidget(gEarthWidget); +#endif + if (hudWidget) centerStack->addWidget(hudWidget); + if (dataplotWidget) centerStack->addWidget(dataplotWidget); + + setCentralWidget(centerStack); +======= // ONBOARD PARAMETERS if (parametersDockWidget) { addDockWidget(Qt::RightDockWidgetArea, parametersDockWidget); parametersDockWidget->show(); } - - this->show(); +>>>>>>> master } -void MainWindow::loadWidgets() +void MainWindow::connectActions() { - //loadOperatorView(); - loadEngineerView(); - //loadPilotView(); +<<<<<<< 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 f423c60ed8dc87585bb832d4fabd6b2de844b1e3..be8b3184f1fdfa90a9ad7bc1458d150cee57c5fc 100644 --- a/src/ui/MainWindow.h +++ b/src/ui/MainWindow.h @@ -70,7 +70,7 @@ This file is part of the QGROUNDCONTROL project #include "SlugsPIDControl.h" -#include "slugshilsim.h" +#include "SlugsHilSim.h" #include "SlugsVideoCamControl.h" @@ -87,6 +87,9 @@ public: ~MainWindow(); public slots: +// /** @brief Store the mainwindow settings */ +// void storeSettings(); + /** * @brief Shows a status message on the bottom status bar * @@ -119,22 +122,8 @@ public slots: void loadEngineerView(); /** @brief Load view for operator */ void loadOperatorView(); - /** @brief Load 3D view */ - void load3DView(); - /** @brief Load 3D Google Earth view */ - void loadGoogleEarthView(); - /** @brief Load 3D map view */ - void load3DMapView(); - /** @brief Load view with all widgets */ - void loadAllView(); /** @brief Load MAVLink XML generator view */ void loadMAVLinkView(); - /** @brief Load data view, allowing to plot flight data */ - void loadDataView(); - /** @brief Load data view, allowing to plot flight data */ - void loadDataView(QString fileName); - /** @brief Load view for global operator, allowing to flight on earth */ - void loadGlobalOperatorView(); /** @brief Show the online help for users */ void showHelp(); @@ -143,21 +132,199 @@ public slots: /** @brief Show the project roadmap */ void showRoadMap(); - // Fixme find a nicer solution that scales to more AP types - void loadSlugsView(); - void loadPixhawkView(); + /** @brief Shows the widgets based on configuration and current view and autopilot */ + void presentView(); /** @brief Reload the CSS style sheet */ void reloadStylesheet(); + + /* + ========================================================== + Potentially Deprecated + ========================================================== + */ + + void loadPixhawkEngineerView(); + + /** @brief Load view with all widgets */ + void loadAllView(); + + void loadWidgets(); + + /** @brief Load data view, allowing to plot flight data */ + void loadDataView(); + /** @brief Load data view, allowing to plot flight data */ + void loadDataView(QString fileName); + + /** @brief Load 3D map view */ + void load3DMapView(); + + /** @brief Load 3D Google Earth view */ + void loadGoogleEarthView(); + + /** @brief Load 3D view */ + void load3DView(); + + /** + * @brief Shows a Docked Widget based on the action sender + * + * This slot is written to be used in conjunction with the addToToolsMenu function + * It shows the QDockedWidget based on the action sender + * + */ + void showToolWidget(); + + /** + * @brief Shows a Widget from the center stack based on the action sender + * + * This slot is written to be used in conjunction with the addToCentralWidgetsMenu function + * It shows the Widget based on the action sender + * + */ + void showCentralWidget(); + + /** @brief Updates a QDockWidget's checked status based on its visibility */ + void updateVisibilitySettings (bool vis); + + /** @brief Updates a QDockWidget's location */ + void updateLocationSettings (Qt::DockWidgetArea location); + protected: + + // These defines are used to save the settings when selecting with + // which widgets populate the views + // FIXME: DO NOT PUT CUSTOM VALUES IN THIS ENUM since it is iterated over + // this will be fixed in a future release. + typedef enum _TOOLS_WIDGET_NAMES { + MENU_UAS_CONTROL, + MENU_UAS_INFO, + MENU_CAMERA, + MENU_UAS_LIST, + MENU_WAYPOINTS, + MENU_STATUS, + MENU_DETECTION, + MENU_DEBUG_CONSOLE, + MENU_PARAMETERS, + MENU_HDD_1, + MENU_HDD_2, + MENU_WATCHDOG, + MENU_HUD, + MENU_HSI, + MENU_RC_VIEW, + MENU_SLUGS_DATA, + MENU_SLUGS_PID, + MENU_SLUGS_HIL, + MENU_SLUGS_CAMERA, + CENTRAL_SEPARATOR= 255, // do not change + CENTRAL_LINECHART, + CENTRAL_PROTOCOL, + CENTRAL_MAP, + CENTRAL_3D_LOCAL, + CENTRAL_3D_MAP, + CENTRAL_OSGEARTH, + CENTRAL_GOOGLE_EARTH, + CENTRAL_HUD, + CENTRAL_DATA_PLOT, + + }TOOLS_WIDGET_NAMES; + + typedef enum _SETTINGS_SECTIONS { + SECTION_MENU, + SUB_SECTION_CHECKED, + SUB_SECTION_LOCATION, + } SETTINGS_SECTIONS; + + typedef enum _VIEW_SECTIONS { + VIEW_ENGINEER, + VIEW_OPERATOR, + VIEW_PILOT, + VIEW_MAVLINK, + } VIEW_SECTIONS; + + + QHash toolsMenuActions; // Holds ptr to the Menu Actions + QHash dockWidgets; // Holds ptr to the Actual Dock widget + QHash dockWidgetLocations; // Holds the location + + /** + * @brief Adds an already instantiated QDockedWidget to the Tools Menu + * + * This function does all the hosekeeping to have a QDockedWidget added to the + * tools menu and connects the QMenuAction to a slot that shows the widget and + * checks/unchecks the tools menu item + * + * @param widget The QDockedWidget being added + * @param title The entry that will appear in the Menu and in the QDockedWidget title bar + * @param slotName The slot to which the triggered() signal of the menu action will be connected. + * @param tool The ENUM defined in MainWindow.h that is associated to the widget + * @param location The default location for the QDockedWidget in case there is no previous key in the settings + */ + void addToToolsMenu (QWidget* widget, const QString title, const char * slotName, TOOLS_WIDGET_NAMES tool, Qt::DockWidgetArea location); + + /** + * @brief Determines if a QDockWidget needs to be show and if so, shows it + * + * Based on the the autopilot and the current view it queries the settings and shows the + * widget if necessary + * + * @param widget The QDockWidget requested to be shown + * @param view The view for which the QDockWidget is requested + */ + void showTheWidget (TOOLS_WIDGET_NAMES widget, VIEW_SECTIONS view = VIEW_MAVLINK); + + /** + * @brief Adds an already instantiated QWidget to the center stack + * + * This function does all the hosekeeping to have a QWidget added to the tools menu + * tools menu and connects the QMenuAction to a slot that shows the widget and + * checks/unchecks the tools menu item. This is used for all the central widgets (those in + * the center stack. + * + * @param widget The QWidget being added + * @param title The entry that will appear in the Menu + * @param slotName The slot to which the triggered() signal of the menu action will be connected. + * @param centralWidget The ENUM defined in MainWindow.h that is associated to the widget + */ + void addToCentralWidgetsMenu ( QWidget* widget, const QString title,const char * slotName, TOOLS_WIDGET_NAMES centralWidget); + + /** + * @brief Determines if a QWidget needs to be show and if so, shows it + * + * Based on the the autopilot and the current view it queries the settings and shows the + * widget if necessary + * + * @param centralWidget The QWidget requested to be shown + * @param view The view for which the QWidget is requested + */ + void showTheCentralWidget (TOOLS_WIDGET_NAMES centralWidget, VIEW_SECTIONS view); + + + /** @brief Keeps track of the current view */ + VIEW_SECTIONS currentView; + QStatusBar* statusBar; QStatusBar* createStatusBar(); - void loadWidgets(); - void connectActions(); + + void clearView(); - void buildWidgets(); - void connectWidgets(); - void arrangeCenterStack(); + + void buildCommonWidgets(); + void buildPxWidgets(); + void buildSlugsWidgets(); + + void connectCommonWidgets(); + void connectPxWidgets(); + void connectSlugsWidgets(); + + void arrangeCommonCenterStack(); + void arrangePxCenterStack(); + void arrangeSlugsCenterStack(); + + void connectCommonActions(); + void connectPxActions(); + void connectSlugsActions(); + + void configureWindowName(); // TODO Should be moved elsewhere, as the protocol does not belong to the UI @@ -168,9 +335,13 @@ protected: LinkInterface* udpLink; QSettings settings; + QStackedWidget *centerStack; + // Center widgets QPointer linechartWidget; + QPointer hudWidget; + QPointer mapWidget; QPointer protocolWidget; QPointer dataplotWidget; @@ -193,9 +364,12 @@ protected: QPointer headDown1DockWidget; QPointer headDown2DockWidget; QPointer watchdogControlDockWidget; + QPointer headUpDockWidget; + QPointer hsiDockWidget; QPointer rcViewDockWidget; + QPointer hudDockWidget; QPointer slugsDataWidget; QPointer slugsPIDControlWidget; QPointer slugsHilSimWidget; @@ -216,6 +390,7 @@ protected: QAction* killUASAct; QAction* simulateUASAct; + LogCompressor* comp; QString screenFileName; QTimer* videoTimer; @@ -223,6 +398,8 @@ protected: private: Ui::MainWindow ui; + QString buildMenuKey (SETTINGS_SECTIONS section , TOOLS_WIDGET_NAMES tool, VIEW_SECTIONS view); + }; #endif /* _MAINWINDOW_H_ */ diff --git a/src/ui/MainWindow.ui b/src/ui/MainWindow.ui index 323c51a5c2d34cf4a521cb06fc97b66470f1d7dc..de35a73d547a73cb8896053fd51a32b07dcd0439 100644 --- a/src/ui/MainWindow.ui +++ b/src/ui/MainWindow.ui @@ -45,15 +45,30 @@ File - + + + + Network + + + + + + + Select System + + Unmanned System + + false + @@ -61,59 +76,39 @@ - - - Network - - - - - + - Window + Tools - - - - - - - - - - - - - Help - - - + + + - + - Select System + Perspectives + + + + + + - + + - - - TopToolBarArea - - - false - - + @@ -218,6 +213,9 @@ Joystick settings + + true + @@ -348,6 +346,90 @@ Show Slugs View + + + + :/images/devices/input-gaming.svg:/images/devices/input-gaming.svg + + + Joystick Settings + + + false + + + + + + :/images/apps/utilities-system-monitor.svg:/images/apps/utilities-system-monitor.svg + + + Online Documentation + + + + + + :/images/status/software-update-available.svg:/images/status/software-update-available.svg + + + Project Roadmap + + + + + + :/images/categories/preferences-system.svg:/images/categories/preferences-system.svg + + + Developer Credits + + + + + + :/images/status/weather-overcast.svg:/images/status/weather-overcast.svg + + + Operator + + + + + + :/images/apps/utilities-system-monitor.svg:/images/apps/utilities-system-monitor.svg + + + Engineer + + + + + + :/images/devices/network-wired.svg:/images/devices/network-wired.svg + + + Mavlink + + + + + + :/images/categories/applications-internet.svg:/images/categories/applications-internet.svg + + + Reload Style + + + + + + :/images/status/network-wireless-encrypted.svg:/images/status/network-wireless-encrypted.svg + + + Pilot + + @@ -372,3 +454,5 @@ + + diff --git a/src/ui/MapWidget.cc b/src/ui/MapWidget.cc index 1f244dd5b732697c5431fdfc71d35fa78c541e3f..b95e7547086c6b559e69d3a1b6e09550063ed407 100644 --- a/src/ui/MapWidget.cc +++ b/src/ui/MapWidget.cc @@ -562,6 +562,7 @@ void MapWidget::updateGlobalPosition(UASInterface* uas, double lat, double lon, // Icon QPen* pointpen = new QPen(uasColor); + qDebug() << uas->getUASName(); MAV2DIcon* p = new MAV2DIcon(lat, lon, 20, uas->getUASName(), qmapcontrol::Point::Middle, pointpen); uasIcons.insert(uas->getUASID(), p); geomLayer->addGeometry(p); @@ -592,21 +593,12 @@ void MapWidget::updateGlobalPosition(UASInterface* uas, double lat, double lon, uasTrails.value(uas->getUASID())->addPoint(new qmapcontrol::Point(lat, lon, QString("lat: %1 lon: %2").arg(lat, lon))); } - // points.append(new CirclePoint(8.275145, 50.016992, 15, "Wiesbaden-Mainz-Kastel, Johannes-Goßner-Straße", Point::Middle, pointpen)); - // points.append(new CirclePoint(8.270476, 50.021426, 15, "Wiesbaden-Mainz-Kastel, Ruthof", Point::Middle, pointpen)); - // // "Blind" Points - // points.append(new Point(8.266445, 50.025913, "Wiesbaden-Mainz-Kastel, Mudra Kaserne")); - // points.append(new Point(8.260378, 50.030345, "Wiesbaden-Mainz-Amoneburg, Dyckerhoffstraße")); // Connect click events of the layer to this object // connect(osmLayer, SIGNAL(geometryClicked(Geometry*, QPoint)), // this, SLOT(geometryClicked(Geometry*, QPoint))); // Sets the view to the interesting area - //QList view; - //view.append(QPointF(8.24764, 50.0319)); - //view.append(QPointF(8.28412, 49.9998)); - // mc->setView(view); updatePosition(0, lat, lon); } } diff --git a/src/ui/ObjectDetectionView.cc b/src/ui/ObjectDetectionView.cc index 03d8c007e8a2f6db0c168721ffb3f2d9a0fccd3b..922a1c61562365f7dec4c6acd273f214d6207546 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(), @@ -117,7 +115,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/ParameterInterface.cc b/src/ui/ParameterInterface.cc index f44e08389301787445e07e46c16ab4da9a2b8d34..d6a3a59aae206baa6f968cf0ec0641b2e79462f3 100644 --- a/src/ui/ParameterInterface.cc +++ b/src/ui/ParameterInterface.cc @@ -51,6 +51,15 @@ ParameterInterface::ParameterInterface(QWidget *parent) : // Setup UI connections connect(m_ui->vehicleComboBox, SIGNAL(activated(int)), this, SLOT(selectUAS(int))); + // Get current MAV list + QList systems = UASManager::instance()->getUASList(); + + // Add each of them + foreach (UASInterface* sys, systems) + { + addUAS(sys); + } + // Setup MAV connections connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)), this, SLOT(addUAS(UASInterface*))); this->setVisible(false); diff --git a/src/ui/QGCMainWindowAPConfigurator.cc b/src/ui/QGCMainWindowAPConfigurator.cc new file mode 100644 index 0000000000000000000000000000000000000000..69da8c599ed6460879cd74c5d5f53df28802a93b --- /dev/null +++ b/src/ui/QGCMainWindowAPConfigurator.cc @@ -0,0 +1,6 @@ +#include "QGCMainWindowAPConfigurator.h" + +QGCMainWindowAPConfigurator::QGCMainWindowAPConfigurator(QObject *parent) : + QObject(parent) +{ +} diff --git a/src/ui/QGCMainWindowAPConfigurator.h b/src/ui/QGCMainWindowAPConfigurator.h new file mode 100644 index 0000000000000000000000000000000000000000..f4806f8202c7dee906868232fd013f3cf5980d78 --- /dev/null +++ b/src/ui/QGCMainWindowAPConfigurator.h @@ -0,0 +1,18 @@ +#ifndef QGCMAINWINDOWAPCONFIGURATOR_H +#define QGCMAINWINDOWAPCONFIGURATOR_H + +#include + +class QGCMainWindowAPConfigurator : public QObject +{ + Q_OBJECT +public: + explicit QGCMainWindowAPConfigurator(QObject *parent = 0); + +signals: + +public slots: + +}; + +#endif // QGCMAINWINDOWAPCONFIGURATOR_H diff --git a/src/ui/QGCRemoteControlView.cc b/src/ui/QGCRemoteControlView.cc index 77aa64948e388f95fcb1cc5f39afdc90197d68d2..106992fae340b5cffc88be2a5e0e5e75830b90d1 100644 --- a/src/ui/QGCRemoteControlView.cc +++ b/src/ui/QGCRemoteControlView.cc @@ -115,10 +115,10 @@ void QGCRemoteControlView::setUASId(int id) connect(newUAS, SIGNAL(remoteControlRSSIChanged(float)), this, SLOT(setRemoteRSSI(float))); connect(newUAS, SIGNAL(remoteControlChannelRawChanged(int,float)), this, SLOT(setChannelRaw(int,float))); - connect(newUAS, SIGNAL(remoteControlChannelRawChanged(int,float)), calibrationWindow, SLOT(setChannelRaw(int,float))); +// connect(newUAS, SIGNAL(remoteControlChannelRawChanged(int,float)), calibrationWindow, SLOT(setChannelRaw(int,float))); connect(newUAS, SIGNAL(remoteControlChannelScaledChanged(int,float)), this, SLOT(setChannelScaled(int,float))); - connect(newUAS, SIGNAL(remoteControlChannelScaledChanged(int,float)), calibrationWindow, SLOT(setChannelScaled(int,float))); +// connect(newUAS, SIGNAL(remoteControlChannelScaledChanged(int,float)), calibrationWindow, SLOT(setChannelScaled(int,float))); } } 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.cc b/src/ui/SerialConfigurationWindow.cc index 5542bc93bd382ec0971c464a00c644c7e3303c9b..d85802706f1dc578cfeaf33546a37098a55d7490 100644 --- a/src/ui/SerialConfigurationWindow.cc +++ b/src/ui/SerialConfigurationWindow.cc @@ -33,6 +33,7 @@ This file is part of the QGROUNDCONTROL project #include #include #include +#include #include #ifdef _WIN32 #include @@ -509,12 +510,3 @@ void SerialConfigurationWindow::setLinkName(QString name) setWindowTitle(tr("Configuration of ") + link->getName()); } -void SerialConfigurationWindow::remove() -{ - link->disconnect(); - //delete link; - //delete action; - this->window()->close(); - qDebug() << "TODO: Link cannot be deleted: SerialConfigurationWindow::remove() NOT IMPLEMENTED!"; -} - diff --git a/src/ui/SerialConfigurationWindow.h b/src/ui/SerialConfigurationWindow.h index d71fb95e2aa5f415f1033c93cab511fa66e07805..abfa5c138c0ea3ba8ffb5e7c118ff26ab2ce86c5 100644 --- a/src/ui/SerialConfigurationWindow.h +++ b/src/ui/SerialConfigurationWindow.h @@ -60,12 +60,6 @@ public slots: void setParityEven(); void setPortName(QString port); void setLinkName(QString name); - /** - * @brief Remove this link - * - * Disconnects the associated link, removes it from all menus and closes the window. - */ - void remove(); void setupPortList(); protected slots: diff --git a/src/ui/SlugsDataSensorView.cc b/src/ui/SlugsDataSensorView.cc index 749f2bd19506eb1e35ea26e0c2523616ffe51b0c..c8a12cd56a61860670c546e09003cc47e434be98 100644 --- a/src/ui/SlugsDataSensorView.cc +++ b/src/ui/SlugsDataSensorView.cc @@ -41,10 +41,12 @@ void SlugsDataSensorView::addUAS(UASInterface* uas) connect(slugsMav, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(slugSpeedLocalPositionChanged(UASInterface*,double,double,double,quint64))); connect(slugsMav, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(slugAttitudeChanged(UASInterface*,double,double,double,quint64))); connect(slugsMav, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(slugsGlobalPositionChanged(UASInterface*,double,double,double,quint64))); + connect(slugsMav,SIGNAL(slugsGPSCogSog(int,double,double)),this,SLOT(slugsGPSCogSog(int,double,double))); - //connect slugs especial messages + + //connect slugs especial messages connect(slugsMav, SIGNAL(slugsSensorBias(int,const mavlink_sensor_bias_t&)), this, SLOT(slugsSensorBiasChanged(int,const mavlink_sensor_bias_t&))); connect(slugsMav, SIGNAL(slugsDiagnostic(int,const mavlink_diagnostic_t&)), this, SLOT(slugsDiagnosticMessageChanged(int,const mavlink_diagnostic_t&))); connect(slugsMav, SIGNAL(slugsCPULoad(int,const mavlink_cpu_load_t&)), this, SLOT(slugsCpuLoadChanged(int,const mavlink_cpu_load_t&))); @@ -53,6 +55,8 @@ void SlugsDataSensorView::addUAS(UASInterface* uas) connect(slugsMav, SIGNAL(slugsPWM(int,const mavlink_pwm_commands_t&)),this,SLOT(slugsPWMChanged(int,const mavlink_pwm_commands_t&))); connect(slugsMav, SIGNAL(slugsFilteredData(int,const mavlink_filtered_data_t&)),this,SLOT(slugsFilteredDataChanged(int,const mavlink_filtered_data_t&))); connect(slugsMav, SIGNAL(slugsGPSDateTime(int,const mavlink_gps_date_time_t&)),this,SLOT(slugsGPSDateTimeChanged(int,const mavlink_gps_date_time_t&))); + connect(slugsMav,SIGNAL(slugsAirData(int, const mavlink_air_data_t&)),this,SLOT(slugsAirDataChanged(int, const mavlink_air_data_t&))); + #endif // MAVLINK_ENABLED_SLUGS // Set this UAS as active if it is the first one @@ -69,6 +73,15 @@ void SlugsDataSensorView::slugRawDataChanged(int uasId, const mavlink_raw_imu_t ui->m_Axr->setText(QString::number(rawData.xacc)); ui->m_Ayr->setText(QString::number(rawData.yacc)); ui->m_Azr->setText(QString::number(rawData.zacc)); + + ui->m_Mxr->setText(QString::number(rawData.xmag)); + ui->m_Myr->setText(QString::number(rawData.ymag)); + ui->m_Mzr->setText(QString::number(rawData.zmag)); + + ui->m_Gxr->setText(QString::number(rawData.xgyro)); + ui->m_Gyr->setText(QString::number(rawData.ygyro)); + ui->m_Gzr->setText(QString::number(rawData.zgyro)); + } void SlugsDataSensorView::setActiveUAS(UASInterface* uas){ @@ -85,9 +98,13 @@ void SlugsDataSensorView::slugsGlobalPositionChanged(UASInterface *uas, Q_UNUSED(uas); Q_UNUSED(time); + + ui->m_GpsLatitude->setText(QString::number(lat)); ui->m_GpsLongitude->setText(QString::number(lon)); ui->m_GpsHeight->setText(QString::number(alt)); + + qDebug()<<"GPS Position = "<ed_y->setPlainText(QString::number(y)); ui->ed_z->setPlainText(QString::number(z)); + //qDebug()<<"Local Position = "<ed_vy->setPlainText(QString::number(vy)); ui->ed_vz->setPlainText(QString::number(vz)); + //qDebug()<<"Speed Local Position = "<m_Pitch->setPlainText(QString::number(slugpitch)); ui->m_Yaw->setPlainText(QString::number(slugyaw)); + qDebug()<<"Attitude change = "<m_GpsDate->setText(QString::number(gpsDateTime.month) + "/" + - QString::number(gpsDateTime.day) + "/" + + + QString month, day; + + month = QString::number(gpsDateTime.month); + day = QString::number(gpsDateTime.day); + + if(gpsDateTime.month < 10) month = "0" + QString::number(gpsDateTime.month); + if(gpsDateTime.day < 10) day = "0" + QString::number(gpsDateTime.day); + + + ui->m_GpsDate->setText(day + "/" + + month + "/" + QString::number(gpsDateTime.year)); - ui->m_GpsTime->setText(QString::number(gpsDateTime.hour) + ":" + - QString::number(gpsDateTime.min) + ":" + - QString::number(gpsDateTime.sec)); + QString hour, min, sec; + + hour = QString::number(gpsDateTime.hour); + min = QString::number(gpsDateTime.min); + sec = QString::number(gpsDateTime.sec); + + if(gpsDateTime.hour < 10) hour = "0" + QString::number(gpsDateTime.hour); + if(gpsDateTime.min < 10) min = "0" + QString::number(gpsDateTime.min); + if(gpsDateTime.sec < 10) sec = "0" + QString::number(gpsDateTime.sec); + + ui->m_GpsTime->setText(hour + ":" + + min + ":" + + sec); ui->m_GpsSat->setText(QString::number(gpsDateTime.visSat)); + + +} + +/** + * @brief Updates the air data widget - 171 +*/ +void SlugsDataSensorView::slugsAirDataChanged(int systemId, const mavlink_air_data_t &airData) +{ + Q_UNUSED(systemId); + ui->ed_dynamic->setText(QString::number(airData.dynamicPressure)); + ui->ed_static->setText(QString::number(airData.staticPressure)); + ui->ed_temp->setText(QString::number(airData.temperature)); + +// qDebug()<<"Air Data = "<m_GpsCog->setText(QString::number(cog)); + ui->m_GpsSog->setText(QString::number(sog)); + +} + + + + #endif // MAVLINK_ENABLED_SLUGS diff --git a/src/ui/SlugsDataSensorView.h b/src/ui/SlugsDataSensorView.h index ed0b9f07c7dd2195ddca02c739a7b1e795635fc7..b1adcca6a18b4dd6519858f9e82d0343185946a5 100644 --- a/src/ui/SlugsDataSensorView.h +++ b/src/ui/SlugsDataSensorView.h @@ -70,6 +70,9 @@ public slots: void setActiveUAS(UASInterface* uas); + /** + * @brief Updates the Raw Data widget + */ void slugRawDataChanged (int uasId, const mavlink_raw_imu_t& rawData); #ifdef MAVLINK_ENABLED_SLUGS @@ -114,58 +117,79 @@ public slots: double lon, double alt, quint64 time); + /** - * @brief Updates the sensor bias widget + * @brief set COG and SOG values + * + * COG and SOG GPS display on the Widgets + */ + void slugsGPSCogSog(int systemId, + double cog, + double sog); + + + + /** + * @brief Updates the CPU load widget - 170 + */ + void slugsCpuLoadChanged(int systemId, + const mavlink_cpu_load_t& cpuLoad); + + /** + * @brief Updates the air data widget - 171 + */ + void slugsAirDataChanged(int systemId, + const mavlink_air_data_t& airData); + + /** + * @brief Updates the sensor bias widget - 172 */ void slugsSensorBiasChanged(int systemId, const mavlink_sensor_bias_t& sensorBias); /** - * @brief Updates the diagnostic widget + * @brief Updates the diagnostic widget - 173 */ void slugsDiagnosticMessageChanged(int systemId, const mavlink_diagnostic_t& diagnostic); - /** - * @brief Updates the CPU load widget - */ - void slugsCpuLoadChanged(int systemId, - const mavlink_cpu_load_t& cpuLoad); - /** - * @brief Updates the Navigation widget + * @brief Updates the Navigation widget - 176 */ void slugsNavegationChanged(int systemId, const mavlink_slugs_navigation_t& slugsNavigation); /** - * @brief Updates the Data Log widget + * @brief Updates the Data Log widget - 177 */ void slugsDataLogChanged(int systemId, const mavlink_data_log_t& dataLog); /** - * @brief Updates the PWM Commands widget + * @brief Updates the PWM Commands widget - 175 */ void slugsPWMChanged(int systemId, const mavlink_pwm_commands_t& pwmCommands); /** - * @brief Updates the filtered sensor measurements widget + * @brief Updates the filtered sensor measurements widget - 178 */ void slugsFilteredDataChanged(int systemId, const mavlink_filtered_data_t& filteredData); /** - * @brief Updates the gps Date Time widget + * @brief Updates the gps Date Time widget - 179 */ void slugsGPSDateTimeChanged(int systemId, const mavlink_gps_date_time_t& gpsDateTime); + + + #endif // MAVLINK_ENABLED_SLUGS protected: diff --git a/src/ui/SlugsDataSensorView.ui b/src/ui/SlugsDataSensorView.ui index 4a6e2de6bc0a1e9004a25ffdd440a16eb4c1a06a..5474afb0abff4307da40cddbe7905e408ca65eb2 100644 --- a/src/ui/SlugsDataSensorView.ui +++ b/src/ui/SlugsDataSensorView.ui @@ -7,7 +7,7 @@ 0 0 399 - 667 + 604 @@ -24,18 +24,18 @@ - 16777215 - 16777215 + 399 + 604 Form - - + + - 0 + 1 @@ -989,7 +989,7 @@ 20 - 13 + 5 @@ -1791,8 +1791,8 @@ Tab 2 - - + + @@ -4483,11 +4483,6 @@ - - - Tab 4 - - diff --git a/src/ui/slugshilsim.cc b/src/ui/SlugsHilSim.cc similarity index 99% rename from src/ui/slugshilsim.cc rename to src/ui/SlugsHilSim.cc index 03911c370910175bb0a8fb0467a2b5dc0bd4e22f..09baad795a2cbb81493cc202d2c2d8118fe6498e 100644 --- a/src/ui/slugshilsim.cc +++ b/src/ui/SlugsHilSim.cc @@ -28,8 +28,8 @@ This file is part of the QGROUNDCONTROL project */ -#include "slugshilsim.h" -#include "ui_slugshilsim.h" +#include "SlugsHilSim.h" +#include "ui_SlugsHilSim.h" #include "LinkManager.h" SlugsHilSim::SlugsHilSim(QWidget *parent) : diff --git a/src/ui/slugshilsim.h b/src/ui/SlugsHilSim.h similarity index 100% rename from src/ui/slugshilsim.h rename to src/ui/SlugsHilSim.h diff --git a/src/ui/slugshilsim.ui b/src/ui/SlugsHilSim.ui similarity index 100% rename from src/ui/slugshilsim.ui rename to src/ui/SlugsHilSim.ui diff --git a/src/ui/SlugsPIDControl.cpp b/src/ui/SlugsPIDControl.cpp index eec5dd58ffb82a1c1b15eec11bdeb89531df6bb2..214c55477706499aa056a063f7d7863504ec0419 100644 --- a/src/ui/SlugsPIDControl.cpp +++ b/src/ui/SlugsPIDControl.cpp @@ -55,6 +55,7 @@ void SlugsPIDControl::activeUasSet(UASInterface* uas) connect(slugsMav,SIGNAL(slugsPidValues(int,mavlink_pid_t)),this, SLOT(receivePidValues(int,mavlink_pid_t)) ); connect(ui->setGeneral_pushButton,SIGNAL(clicked()),this,SLOT(slugsTimerStartSet())); + connect(ui->getGeneral_pushButton,SIGNAL(clicked()),this,SLOT(slugsTimerStartGet())); } #endif // MAVLINK_ENABLED_SLUG @@ -473,9 +474,9 @@ void SlugsPIDControl::changeColor_GREEN_Pitch2dT_groupBox() //create the packet pidMessage.target = activeUAS->getUASID(); pidMessage.idx = 8; - pidMessage.pVal = ui->dR_P_set->text().toFloat(); - pidMessage.iVal = ui->dR_I_set->text().toFloat(); - pidMessage.dVal = ui->dR_D_set->text().toFloat(); + pidMessage.pVal = ui->P2dT_FF_set->text().toFloat(); + pidMessage.iVal = 0;//ui->dR_I_set->text().toFloat(); + pidMessage.dVal = 0;//ui->dR_D_set->text().toFloat(); mavlink_message_t msg; @@ -543,6 +544,7 @@ void SlugsPIDControl::receivePidValues(int systemId, const mavlink_pid_t &pidVal break; case 8: ui->P2dT_FF_get->setText(QString::number(pidValues.pVal)); + break; default: diff --git a/src/ui/SlugsPIDControl.h b/src/ui/SlugsPIDControl.h index 051dc3f22278cd97a27363e268dfd29eaec70412..d820cee362831457c9ee21baca5d6372f3fcdd48 100644 --- a/src/ui/SlugsPIDControl.h +++ b/src/ui/SlugsPIDControl.h @@ -283,10 +283,10 @@ private: #endif QTimer* refreshTimerSet; ///< The main timer, controls the update view - QTimer* refreshTimerGet; ///< The main timer, controls the update view + QTimer* refreshTimerGet; ///< The main timer, controls the update view int counterRefreshSet; int counterRefreshGet; - QMutex valuesMutex; + QMutex valuesMutex; }; #endif // SLUGSPIDCONTROL_H diff --git a/src/ui/SlugsPIDControl.ui b/src/ui/SlugsPIDControl.ui index 042f3d92c3fe31622011b0295bb50fa622ff0a7b..6de57a0178d35a4d94f14b72a70e166ae82714cf 100644 --- a/src/ui/SlugsPIDControl.ui +++ b/src/ui/SlugsPIDControl.ui @@ -121,13 +121,6 @@ - - - - Recepcion - - - @@ -956,6 +949,13 @@ + + + + Recepcion + + + diff --git a/src/ui/SlugsPadCameraControl.cpp b/src/ui/SlugsPadCameraControl.cpp index 40b40c072ec83feb6c385387f04c696cccac969e..3e198c68be7473290b6bb517e3f65250acbdaf51 100644 --- a/src/ui/SlugsPadCameraControl.cpp +++ b/src/ui/SlugsPadCameraControl.cpp @@ -13,6 +13,9 @@ SlugsPadCameraControl::SlugsPadCameraControl(QWidget *parent) : ui->setupUi(this); x1= 0; y1 = 0; + bearingPad = 0; + distancePad = 0; + directionPad = "no"; } @@ -23,13 +26,25 @@ SlugsPadCameraControl::~SlugsPadCameraControl() void SlugsPadCameraControl::mouseMoveEvent(QMouseEvent *event) { - emit mouseMoveCoord(event->x(),event->y()); + //emit mouseMoveCoord(event->x(),event->y()); + if(dragging) + { + if(abs(x1-event->x())>20 || abs(y1-event->y())>20) + { + + getDeltaPositionPad(event->x(), event->y()); + x1 = event->x(); + y1 = event->y(); + } + } + } void SlugsPadCameraControl::mousePressEvent(QMouseEvent *event) { - emit mousePressCoord(event->x(),event->y()); + //emit mousePressCoord(event->x(),event->y()); + dragging = true; x1 = event->x(); y1 = event->y(); @@ -37,8 +52,13 @@ void SlugsPadCameraControl::mousePressEvent(QMouseEvent *event) void SlugsPadCameraControl::mouseReleaseEvent(QMouseEvent *event) { - emit mouseReleaseCoord(event->x(),event->y()); - getDeltaPositionPad(event->x(), event->y()); + dragging = false; + //emit mouseReleaseCoord(event->x(),event->y()); + //getDeltaPositionPad(event->x(), event->y()); + + xFin = event->x(); + yFin = event->y(); + } @@ -59,6 +79,13 @@ void SlugsPadCameraControl::paintEvent(QPaintEvent *pe) painter.drawLine(QPoint(ui->frame->geometry().topLeft().x(),ui->frame->height()/2), QPoint(ui->frame->geometry().bottomRight().x(),ui->frame->height()/2)); + painter.setPen(Qt::white); + + //QPointF coordTemp = getPointBy_BearingDistance(ui->frame->width()/2,ui->frame->height()/2,bearingPad,distancePad); + + painter.drawLine(QPoint(ui->frame->width()/2,ui->frame->height()/2), + QPoint(xFin,yFin)); + // painter.drawLine(QPoint()); //painter.drawLines(padLines); @@ -92,7 +119,7 @@ void SlugsPadCameraControl::getDeltaPositionPad(int x2, int y2) { emit dirCursorText("right up"); //bearing = 315; - dir = "riht up"; + dir = "right up"; } else { @@ -100,7 +127,7 @@ void SlugsPadCameraControl::getDeltaPositionPad(int x2, int y2) { emit dirCursorText("right"); //bearing = 315; - dir = "riht"; + dir = "right"; } else { @@ -108,7 +135,7 @@ void SlugsPadCameraControl::getDeltaPositionPad(int x2, int y2) { emit dirCursorText("right down"); //bearing = 315; - dir = "riht down"; + dir = "right down"; } else { @@ -158,8 +185,15 @@ void SlugsPadCameraControl::getDeltaPositionPad(int x2, int y2) } + bearingPad = bearing; + distancePad = dist; + directionPad = dir; emit changeCursorPosition(bearing, dist, dir); + update(); + + + } double SlugsPadCameraControl::getDistPixel(int x1, int y1, int x2, int y2) @@ -229,3 +263,19 @@ QPointF SlugsPadCameraControl::ObtenerMarcacionDistanciaPixel(double lon1, doubl return QPointF(marcacion,distancia); } + + + +QPointF SlugsPadCameraControl::getPointBy_BearingDistance(double lat1, double lon1, double rumbo, double distancia) +{ + double lon2 = 0; + double lat2 = 0; + double rad= M_PI/180; + + rumbo = rumbo*rad; + lon2=(lon1 + ((distancia/60) * (sin(rumbo)))); + lat2=(lat1 + ((distancia/60) * (cos(rumbo)))); + + return QPointF(lon2,lat2); +} + diff --git a/src/ui/SlugsPadCameraControl.h b/src/ui/SlugsPadCameraControl.h index 06a923328c5c12042a81d197072254fec56df9c4..bb35976cf2952d27c775f7ba1e6b7174e1f83495 100644 --- a/src/ui/SlugsPadCameraControl.h +++ b/src/ui/SlugsPadCameraControl.h @@ -21,6 +21,9 @@ public slots: void getDeltaPositionPad(int x, int y); double getDistPixel(int x1, int y1, int x2, int y2); QPointF ObtenerMarcacionDistanciaPixel(double lon1, double lat1, double lon2, double lat2); + QPointF getPointBy_BearingDistance(double lat1, double lon1, double rumbo, double distancia); + + signals: void mouseMoveCoord(int x, int y); @@ -36,11 +39,17 @@ protected: void mouseMoveEvent(QMouseEvent* event); void paintEvent(QPaintEvent *pe); + private: Ui::SlugsPadCameraControl *ui; bool dragging; int x1; int y1; + int xFin; + int yFin; + double bearingPad; + double distancePad; + QString directionPad; }; diff --git a/src/ui/SlugsVideoCamControl.cpp b/src/ui/SlugsVideoCamControl.cpp index 1eb0b5264e29a990e0e11e6a56933242f4ae0f46..a7c9c67624e450342302e0b047f1a69aa5ebb210 100644 --- a/src/ui/SlugsVideoCamControl.cpp +++ b/src/ui/SlugsVideoCamControl.cpp @@ -20,29 +20,12 @@ SlugsVideoCamControl::SlugsVideoCamControl(QWidget *parent) : QWidget(parent), ui(new Ui::SlugsVideoCamControl) - //dragging(0) - { ui->setupUi(this); // x1= 0; // y1 = 0; - connect(ui->viewCamBordeatMap_checkBox,SIGNAL(clicked(bool)),this,SLOT(changeViewCamBorderAtMapStatus(bool))); -// tL = ui->padCamContro_frame->frameGeometry().topLeft(); -// bR = ui->padCamContro_frame->frameGeometry().bottomRight(); - //ui->padCamContro_frame->setVisible(true); - -// // create a layout for camera pad -// QGridLayout* padCameraLayout = new QGridLayout(this); -// padCameraLayout->setSpacing(2); -// padCameraLayout->setMargin(0); -// padCameraLayout->setAlignment(Qt::AlignTop); - - //ui->padCamContro_frame->setLayout(padCameraLayout); - // create a camera pad widget - //test = new QPushButton(QIcon(":/images/actions/list-add.svg"), "", this); - //test->setMaximumWidth(50); - //ui->gridLayout->addWidget(test, 0,0); + connect(ui->viewCamBordeatMap_checkBox,SIGNAL(clicked(bool)),this,SLOT(changeViewCamBorderAtMapStatus(bool))); padCamera = new SlugsPadCameraControl(this); ui->gridLayout->addWidget(padCamera); @@ -53,28 +36,6 @@ SlugsVideoCamControl::SlugsVideoCamControl(QWidget *parent) : connect(padCamera,SIGNAL(changeCursorPosition(double,double,QString)),this,SLOT(getDeltaPositionPad(double,double,QString))); - - //padCamera->setVisible(true); - - - - // padCameraLayout->addWidget(padCamera); - - - -// QGraphicsScene *scene = new QGraphicsScene(ui->CamControlPanel_graphicsView); -// scene->setItemIndexMethod(QGraphicsScene::NoIndex); -// scene->setSceneRect(-200, -200, 400, 400); -// setScene(scene); -// setCacheMode(CacheBackground); -// setViewportUpdateMode(BoundingRectViewportUpdate); -// setRenderHint(QPainter::Antialiasing); -// setTransformationAnchor(AnchorUnderMouse); -// setResizeAnchor(AnchorViewCenter); - -// ui->CamControlPanel_graphicsView->installEventFilter(this); -// ui->label_x->installEventFilter(this); - } SlugsVideoCamControl::~SlugsVideoCamControl() @@ -82,41 +43,41 @@ SlugsVideoCamControl::~SlugsVideoCamControl() delete ui; } -void SlugsVideoCamControl::mouseMoveEvent(QMouseEvent *event) -{ - Q_UNUSED(event); +//void SlugsVideoCamControl::mouseMoveEvent(QMouseEvent *event) +//{ +// Q_UNUSED(event); -} +//} -void SlugsVideoCamControl::mousePressEvent(QMouseEvent *evnt) -{ - Q_UNUSED(evnt); +//void SlugsVideoCamControl::mousePressEvent(QMouseEvent *evnt) +//{ +// Q_UNUSED(evnt); -} +//} -void SlugsVideoCamControl::mouseReleaseEvent(QMouseEvent *evnt) -{ - Q_UNUSED(evnt); +//void SlugsVideoCamControl::mouseReleaseEvent(QMouseEvent *evnt) +//{ +// Q_UNUSED(evnt); -} +//} -void SlugsVideoCamControl::mousePadMoveEvent(int x, int y) -{ +//void SlugsVideoCamControl::mousePadMoveEvent(int x, int y) +//{ -} +//} -void SlugsVideoCamControl::mousePadPressEvent(int x, int y) -{ +//void SlugsVideoCamControl::mousePadPressEvent(int x, int y) +//{ -} +//} -void SlugsVideoCamControl::mousePadReleaseEvent(int x, int y) -{ +//void SlugsVideoCamControl::mousePadReleaseEvent(int x, int y) +//{ -} +//} void SlugsVideoCamControl::changeViewCamBorderAtMapStatus(bool status) { diff --git a/src/ui/SlugsVideoCamControl.h b/src/ui/SlugsVideoCamControl.h index 2580033a5e1dee3883ea60008707112b6f11f674..0180fd52a5c8737d73a1104113548c62715e30ea 100644 --- a/src/ui/SlugsVideoCamControl.h +++ b/src/ui/SlugsVideoCamControl.h @@ -27,22 +27,43 @@ public: ~SlugsVideoCamControl(); public slots: + /** + * @brief status = true: emit signal to draw a border cam over the map + */ void changeViewCamBorderAtMapStatus(bool status); - void getDeltaPositionPad(double dir, double dist, QString dirText); - - - void mousePadPressEvent(int x, int y); - void mousePadReleaseEvent(int x, int y); - void mousePadMoveEvent(int x, int y); + /** + * @brief show the values of mousepad on ui (labels) and emit a changeCamPosition(signal) + * with values: + * bearing and distance from mouse over the pad + * dirText: direction of mouse movement in text format (up, right,right up,right down, + * left, left up, left down, down) + */ + void getDeltaPositionPad(double bearing, double distance, QString dirText); + +// /** +// * @brief +// */ +// void mousePadPressEvent(int x, int y); +// void mousePadReleaseEvent(int x, int y); +// void mousePadMoveEvent(int x, int y); signals: - void changeCamPosition(double dist, double dir, QString textDir); + /** + * @brief emit values from mousepad: + * bearing and distance from mouse over the pad + * dirText: direction of mouse movement in text format (up, right,right up,right down, + * left, left up, left down, down) + */ + void changeCamPosition(double distance, double bearing, QString textDir); + /** + * @brief emit signal to draw a border cam over the map if status is true + */ void viewCamBorderAtMap(bool status); protected: - void mousePressEvent(QMouseEvent* event); - void mouseReleaseEvent(QMouseEvent* event); - void mouseMoveEvent(QMouseEvent* event); +// void mousePressEvent(QMouseEvent* event); +// void mouseReleaseEvent(QMouseEvent* event); +// void mouseMoveEvent(QMouseEvent* event); diff --git a/src/ui/WaypointList.cc b/src/ui/WaypointList.cc index 3b5b96786435d4abe87c9bad83df39e91ed820b0..d2d4aaeb2a46bb40c915483b42317da08d0f4f8c 100644 --- a/src/ui/WaypointList.cc +++ b/src/ui/WaypointList.cc @@ -153,7 +153,7 @@ void WaypointList::loadWaypoints() QString fileName = QFileDialog::getOpenFileName(this, tr("Load File"), ".", tr("Waypoint File (*.txt)")); uas->getWaypointManager().loadWaypoints(fileName); - } + } } void WaypointList::transmit() @@ -474,14 +474,14 @@ void WaypointList::on_clearWPListButton_clicked() if (uas) { - emit clearPathclicked(); + emit clearPathclicked(); const QVector &waypoints = uas->getWaypointManager().getWaypointList(); while(!waypoints.isEmpty())//for(int i = 0; i <= waypoints.size(); i++) { WaypointView* widget = wpViews.find(waypoints[0]).value(); widget->remove(); - } } + } else { // if(isGlobalWP) 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/XMLCommProtocolWidget.cc b/src/ui/XMLCommProtocolWidget.cc index 582c74ac639480592dfbd36dca607fb5db4c7b7c..c2ea91ea47901f644dedbb4b0cad98b76160ff8f 100644 --- a/src/ui/XMLCommProtocolWidget.cc +++ b/src/ui/XMLCommProtocolWidget.cc @@ -7,6 +7,7 @@ #include "ui_XMLCommProtocolWidget.h" #include "MAVLinkXMLParser.h" #include "MAVLinkSyntaxHighlighter.h" +#include "QGC.h" #include #include @@ -31,7 +32,7 @@ XMLCommProtocolWidget::XMLCommProtocolWidget(QWidget *parent) : void XMLCommProtocolWidget::selectXMLFile() { //QString fileName = QFileDialog::getOpenFileName(this, tr("Load Protocol Definition File"), ".", "*.xml"); - QSettings settings; + QSettings settings(QGC::COMPANYNAME, QGC::APPNAME); const QString mavlinkXML = "MAVLINK_XML_FILE"; QString dirPath = settings.value(mavlinkXML, QCoreApplication::applicationDirPath() + "../").toString(); QFileDialog dialog; @@ -56,6 +57,7 @@ void XMLCommProtocolWidget::selectXMLFile() setXML(instanceText); // Store filename for next time settings.setValue(mavlinkXML, QFileInfo(file).absoluteFilePath()); + settings.sync(); } else { @@ -92,7 +94,7 @@ void XMLCommProtocolWidget::setXML(const QString& xml) void XMLCommProtocolWidget::selectOutputDirectory() { - QSettings settings; + QSettings settings(QGC::COMPANYNAME, QGC::APPNAME); const QString mavlinkOutputDir = "MAVLINK_OUTPUT_DIR"; QString dirPath = settings.value(mavlinkOutputDir, QCoreApplication::applicationDirPath() + "../").toString(); QFileDialog dialog; @@ -110,7 +112,8 @@ void XMLCommProtocolWidget::selectOutputDirectory() { m_ui->outputDirNameLabel->setText(fileNames.first()); // Store directory for next time - settings.setValue(mavlinkOutputDir, fileNames.first()); + settings.setValue(mavlinkOutputDir, QFileInfo(fileNames.first()).absoluteFilePath()); + settings.sync(); //QFile file(fileName); } } diff --git a/src/ui/linechart/LinechartPlot.cc b/src/ui/linechart/LinechartPlot.cc index 7d1613845034a908f3978e9db2db6af1a78cd213..3a7cb0183279d73dc87ff51d98e2f9bea52e1f3c 100644 --- a/src/ui/linechart/LinechartPlot.cc +++ b/src/ui/linechart/LinechartPlot.cc @@ -56,7 +56,7 @@ maxInterval(MAX_STORAGE_INTERVAL), timeScaleStep(DEFAULT_SCALE_INTERVAL), // 10 seconds automaticScrollActive(false), m_active(true), -m_groundTime(false), +m_groundTime(true), d_data(NULL), d_curve(NULL) { diff --git a/src/ui/linechart/LinechartWidget.cc b/src/ui/linechart/LinechartWidget.cc index 4b5cbd1edc91c530fe25f3898eeb1869e836a597..03153e9acb6537b96bf75b740521664f92022058 100644 --- a/src/ui/linechart/LinechartWidget.cc +++ b/src/ui/linechart/LinechartWidget.cc @@ -159,7 +159,9 @@ void LinechartWidget::createLayout() QToolButton* timeButton = new QToolButton(this); timeButton->setText(tr("Ground Time")); timeButton->setCheckable(true); - timeButton->setChecked(false); + bool gTimeDefault = true; + if (activePlot) activePlot->enforceGroundTime(gTimeDefault); + timeButton->setChecked(gTimeDefault); layout->addWidget(timeButton, 1, 4); layout->setColumnStretch(4, 0); connect(timeButton, SIGNAL(clicked(bool)), activePlot, SLOT(enforceGroundTime(bool))); @@ -352,8 +354,6 @@ QWidget* LinechartWidget::createCurveItem(QString curve) QLabel* label; QLabel* value; QLabel* mean; - QLabel* median; - form->setAutoFillBackground(false); horizontalLayout = new QHBoxLayout(form); horizontalLayout->setSpacing(5); @@ -444,6 +444,12 @@ void LinechartWidget::removeCurve(QString curve) // Remove name } +void LinechartWidget::showEvent(QShowEvent* event) +{ + Q_UNUSED(event); + setActive(isVisible()); +} + void LinechartWidget::setActive(bool active) { if (activePlot) diff --git a/src/ui/linechart/LinechartWidget.h b/src/ui/linechart/LinechartWidget.h index 26644f755d2ae76e3e15f10e7cf6dd939ca9cb4a..850b0aaa9f64caedc05d234b8799b110000b74a6 100644 --- a/src/ui/linechart/LinechartWidget.h +++ b/src/ui/linechart/LinechartWidget.h @@ -77,6 +77,8 @@ public slots: void setPlotWindowPosition(int scrollBarValue); void setPlotWindowPosition(quint64 position); void setPlotInterval(quint64 interval); + /** @brief Override base class show */ + virtual void showEvent(QShowEvent* event); void setActive(bool active); /** @brief Set the number of values to average over */ void setAverageWindow(int windowSize); diff --git a/src/ui/linechart/Linecharts.cc b/src/ui/linechart/Linecharts.cc index 2cfaf83ad5fd35daa9bba71d17a5000e86ee8675..b47ea71a7f5a32b0d2281b57f2a0a166d8378ee5 100644 --- a/src/ui/linechart/Linecharts.cc +++ b/src/ui/linechart/Linecharts.cc @@ -1,4 +1,5 @@ #include "Linecharts.h" +#include "UASManager.h" Linecharts::Linecharts(QWidget *parent) : QStackedWidget(parent), @@ -6,6 +7,18 @@ Linecharts::Linecharts(QWidget *parent) : active(true) { this->setVisible(false); + // Get current MAV list + QList systems = UASManager::instance()->getUASList(); + + // Add each of them + foreach (UASInterface* sys, systems) + { + addSystem(sys); + } + connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)), + this, SLOT(addSystem(UASInterface*))); + connect(UASManager::instance(), SIGNAL(activeUASSet(int)), + this, SLOT(selectSystem(int))); } diff --git a/src/ui/map/MAV2DIcon.cc b/src/ui/map/MAV2DIcon.cc index 573149a853e851695af11d316bc059bd36f49ab8..a87962c052174e0c91b751a8b5f48ddd53a37b43 100644 --- a/src/ui/map/MAV2DIcon.cc +++ b/src/ui/map/MAV2DIcon.cc @@ -14,7 +14,10 @@ MAV2DIcon::MAV2DIcon(qreal x, qreal y, QString name, Alignment alignment, QPen* { int radius = 10; size = QSize(radius, radius); - drawIcon(pen); + if (pen) + { + drawIcon(pen); + } } MAV2DIcon::~MAV2DIcon() @@ -24,8 +27,11 @@ MAV2DIcon::~MAV2DIcon() void MAV2DIcon::setPen(QPen* pen) { - mypen = pen; - drawIcon(pen); + if (pen) + { + mypen = pen; + drawIcon(pen); + } } /** @@ -38,46 +44,49 @@ void MAV2DIcon::setYaw(float yaw) void MAV2DIcon::drawIcon(QPen* pen) { - mypixmap = new QPixmap(radius+1, radius+1); - mypixmap->fill(Qt::transparent); - QPainter painter(mypixmap); + if (pen) + { + mypixmap = new QPixmap(radius+1, radius+1); + mypixmap->fill(Qt::transparent); + QPainter painter(mypixmap); - // DRAW MICRO AIR VEHICLE - QPointF p(radius/2, radius/2); + // DRAW MICRO AIR VEHICLE + QPointF p(radius/2, radius/2); - float waypointSize = radius; - QPolygonF poly(3); - // Top point - poly.replace(0, QPointF(p.x(), p.y()+waypointSize/2.0f)); - // Right point - poly.replace(1, QPointF(p.x()-waypointSize/2.0f, p.y()-waypointSize/2.0f)); - // Left point - poly.replace(2, QPointF(p.x()+waypointSize/2.0f, p.y() + waypointSize/2.0f)); + float waypointSize = radius; + QPolygonF poly(3); + // Top point + poly.replace(0, QPointF(p.x(), p.y()+waypointSize/2.0f)); + // Right point + poly.replace(1, QPointF(p.x()-waypointSize/2.0f, p.y()-waypointSize/2.0f)); + // Left point + poly.replace(2, QPointF(p.x()+waypointSize/2.0f, p.y() + waypointSize/2.0f)); -// // Select color based on if this is the current waypoint -// if (list.at(i)->getCurrent()) -// { -// color = QGC::colorCyan;//uas->getColor(); -// pen.setWidthF(refLineWidthToPen(0.8f)); -// } -// else -// { -// color = uas->getColor(); -// pen.setWidthF(refLineWidthToPen(0.4f)); -// } + // // Select color based on if this is the current waypoint + // if (list.at(i)->getCurrent()) + // { + // color = QGC::colorCyan;//uas->getColor(); + // pen.setWidthF(refLineWidthToPen(0.8f)); + // } + // else + // { + // color = uas->getColor(); + // pen.setWidthF(refLineWidthToPen(0.4f)); + // } - //pen.setColor(color); - if (pen) - { - pen->setWidthF(2); - painter.setPen(*pen); - } - else - { - QPen pen2(Qt::yellow); - pen2.setWidth(2); - painter.setPen(pen2); + //pen.setColor(color); + if (pen) + { + pen->setWidthF(2); + painter.setPen(*pen); + } + else + { + QPen pen2(Qt::yellow); + pen2.setWidth(2); + painter.setPen(pen2); + } + painter.setBrush(Qt::NoBrush); + painter.drawPolygon(poly); } - painter.setBrush(Qt::NoBrush); - painter.drawPolygon(poly); } diff --git a/src/ui/map3D/GCManipulator.cc b/src/ui/map3D/GCManipulator.cc index 6f2fb0e7f80bf0496ccf72707de2c132f7793bf4..06a7527b4233a16cf945d7c95c173bf607adfd1b 100644 --- a/src/ui/map3D/GCManipulator.cc +++ b/src/ui/map3D/GCManipulator.cc @@ -30,6 +30,7 @@ This file is part of the QGROUNDCONTROL project */ #include "GCManipulator.h" +#include GCManipulator::GCManipulator() { @@ -254,7 +255,7 @@ GCManipulator::calcMovement(void) if (buttonMask == GUIEventAdapter::LEFT_MOUSE_BUTTON) { // rotate camera -#ifdef __APPLE__ +#if ((OPENSCENEGRAPH_MAJOR_VERSION == 2) & (OPENSCENEGRAPH_MINOR_VERSION > 8)) | (OPENSCENEGRAPH_MAJOR_VERSION > 2) osg::Vec3d axis; #else osg::Vec3 axis; 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/QGCGoogleEarthView.cc b/src/ui/map3D/QGCGoogleEarthView.cc index 2754dc6aadddd4023679a052c65add3e907e3b14..5762082ea545f1722c2ed6a7cbaf9534fdadd7a9 100644 --- a/src/ui/map3D/QGCGoogleEarthView.cc +++ b/src/ui/map3D/QGCGoogleEarthView.cc @@ -16,9 +16,11 @@ QGCGoogleEarthView::QGCGoogleEarthView(QWidget *parent) : QWidget(parent), updateTimer(new QTimer(this)), + refreshRateMs(200), mav(NULL), followCamera(true), trailEnabled(true), + webViewInitialized(false), #if (defined Q_OS_MAC) webViewMac(new QWebView(this)), #endif @@ -34,22 +36,14 @@ QGCGoogleEarthView::QGCGoogleEarthView(QWidget *parent) : #endif ui->setupUi(this); - #if (defined Q_OS_MAC) ui->webViewLayout->addWidget(webViewMac); - 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")); #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(200); + updateTimer->start(refreshRateMs); #endif // Follow checkbox @@ -94,53 +88,82 @@ void QGCGoogleEarthView::follow(bool follow) followCamera = follow; } -void QGCGoogleEarthView::updateState() +void QGCGoogleEarthView::hide() { -#ifdef Q_OS_MAC - if (webViewMac->page()->currentFrame()->evaluateJavaScript("isInitialized();").toBool()) + updateTimer->stop(); + QWidget::hide(); +} + +void QGCGoogleEarthView::show() +{ + if (!webViewInitialized) { - 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; +#if (defined Q_OS_MAC) + webViewMac->setPage(new QGCWebPage(webViewMac)); + webViewMac->settings()->setAttribute(QWebSettings::PluginsEnabled, true); + webViewMac->load(QUrl("earth.html")); +#endif - float roll = 0.0f; - float pitch = 0.0f; - float yaw = 0.0f; +#if (defined Q_OS_WIN) & !(defined __MINGW32__) + webViewWin->load(QUrl("earth.html")); +#endif + webViewInitialized = true; + } + updateTimer->start(); + QWidget::show(); +} - if (mav) - { - uasId = mav->getUASID(); - lat = mav->getLatitude(); - lon = mav->getLongitude(); - alt = mav->getAltitude(); - roll = mav->getRoll(); - pitch = mav->getPitch(); - yaw = mav->getYaw(); - } - webViewMac->page()->currentFrame()->evaluateJavaScript(QString("setAircraftPositionAttitude(%1, %2, %3, %4, %6, %7, %8);") - .arg(uasId) - .arg(lat) - .arg(lon) - .arg(alt+500) - .arg(roll) - .arg(pitch) - .arg(yaw)); - - if (followCamera) +void QGCGoogleEarthView::updateState() +{ +#ifdef Q_OS_MAC + if (isVisible()) + { + if (webViewMac->page()->currentFrame()->evaluateJavaScript("isInitialized();").toBool()) { - webViewMac->page()->currentFrame()->evaluateJavaScript(QString("updateFollowAircraft()")); + 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; + + float roll = 0.0f; + float pitch = 0.0f; + float yaw = 0.0f; + + if (mav) + { + uasId = mav->getUASID(); + lat = mav->getLatitude(); + lon = mav->getLongitude(); + alt = mav->getAltitude(); + roll = mav->getRoll(); + pitch = mav->getPitch(); + yaw = mav->getYaw(); + } + webViewMac->page()->currentFrame()->evaluateJavaScript(QString("setAircraftPositionAttitude(%1, %2, %3, %4, %6, %7, %8);") + .arg(uasId) + .arg(lat) + .arg(lon) + .arg(alt+500) + .arg(roll) + .arg(pitch) + .arg(yaw)); + + if (followCamera) + { + webViewMac->page()->currentFrame()->evaluateJavaScript(QString("updateFollowAircraft()")); + } } } #endif } + void QGCGoogleEarthView::changeEvent(QEvent *e) { QWidget::changeEvent(e); diff --git a/src/ui/map3D/QGCGoogleEarthView.h b/src/ui/map3D/QGCGoogleEarthView.h index 060b71895c44ebbd90cc07ef4a2876ed9f12636f..63a3879fb8d20a77785df3c88f46168360d9beeb 100644 --- a/src/ui/map3D/QGCGoogleEarthView.h +++ b/src/ui/map3D/QGCGoogleEarthView.h @@ -62,14 +62,21 @@ public slots: 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(); protected: void changeEvent(QEvent *e); QTimer* updateTimer; + int refreshRateMs; UASInterface* mav; bool followCamera; bool trailEnabled; + bool webViewInitialized; #if (defined Q_OS_WIN) & (defined _MSVC_VER) +#if (defined Q_OS_WIN) && !(defined __MINGW32__) WebAxWidget* webViewWin; #endif #if (defined Q_OS_MAC) @@ -85,4 +92,4 @@ private: #endif }; -#endif // QGCGOOGLEEARTHVIEW_H +#endif // QGCGOOGLEEARTHVIEW_H \ No newline at end of file diff --git a/src/ui/map3D/QMap3D.cc b/src/ui/map3D/QMap3D.cc index ab4c98b915d22351867f28a6a43e06f1c5ea8b0d..f5d7ffb8e5932903602f610677d6b066adca6a4f 100644 --- a/src/ui/map3D/QMap3D.cc +++ b/src/ui/map3D/QMap3D.cc @@ -38,8 +38,13 @@ This file is part of the QGROUNDCONTROL project QMap3D::QMap3D(QWidget * parent, const char * name, WindowFlags f) : QWidget(parent,f) { + Q_UNUSED(name); setupUi(this); +#if ((OPENSCENEGRAPH_MAJOR_VERSION == 2) & (OPENSCENEGRAPH_MINOR_VERSION > 8)) | (OPENSCENEGRAPH_MAJOR_VERSION > 2) + graphicsView->setCameraManipulator(new osgEarth::Util::EarthManipulator); +#else graphicsView->setCameraManipulator(new osgEarthUtil::EarthManipulator); +#endif graphicsView->setSceneData(new osg::Group); graphicsView->updateCamera(); show(); diff --git a/src/ui/map3D/QOSGWidget.h b/src/ui/map3D/QOSGWidget.h index ebc2dee7f84537bcff4c5f9fc3e5df464d93012d..3e788e03da5d53cee3776e23bf6765cff695187f 100644 --- a/src/ui/map3D/QOSGWidget.h +++ b/src/ui/map3D/QOSGWidget.h @@ -123,7 +123,7 @@ public: getCamera()->setGraphicsContext(getGraphicsWindow()); } - virtual void paintEvent( QPaintEvent * event ) { frame(); } + virtual void paintEvent( QPaintEvent * event ) { Q_UNUSED(event); frame(); } protected: @@ -158,7 +158,7 @@ public: _timer.start(1000.0f/fps); } - virtual void paintEvent( QPaintEvent * event ) { frame(); } + virtual void paintEvent( QPaintEvent * event ) { Q_UNUSED(event); frame(); } void keyPressEvent( QKeyEvent* event ) { diff --git a/src/ui/uas/UASControlWidget.cc b/src/ui/uas/UASControlWidget.cc index d638e1cc3c73171c62f9d176db75c3beeb46adff..a007e7c96c0ee3bd9df2969ec93d210723cc23c1 100644 --- a/src/ui/uas/UASControlWidget.cc +++ b/src/ui/uas/UASControlWidget.cc @@ -50,7 +50,7 @@ This file is part of the PIXHAWK project #define CONTROL_MODE_TEST2 "MODE TEST2" #define CONTROL_MODE_TEST3 "MODE TEST3" #define CONTROL_MODE_READY "MODE TEST3" -#define CONTROL_MODE_RC_TRAINING "MODE RC TRAINING" +#define CONTROL_MODE_RC_TRAINING "RC SIMULATION" #define CONTROL_MODE_LOCKED_INDEX 1 #define CONTROL_MODE_MANUAL_INDEX 2 diff --git a/user_config.pri.dist b/user_config.pri.dist index fa149a1d25c1d79760c9501fa6eae47896b75ca2..d42218893571c601190d9fdb9008bcdf99a00c6a 100644 --- a/user_config.pri.dist +++ b/user_config.pri.dist @@ -26,7 +26,8 @@ # Add or remove custom message specs here. The matching mavlink headers are # included in the main qgroundcontrol.pro file. -MAVLINK_CONF += pixhawk \ - slugs \ - ualberta +MAVLINK_CONF += pixhawk + +# slugs \ +# ualberta