diff --git a/QGCSetup.pri b/QGCSetup.pri index 05bd3b844f..c8173a4e8c 100644 --- a/QGCSetup.pri +++ b/QGCSetup.pri @@ -28,24 +28,27 @@ COPY_RESOURCE_LIST = \ $$BASEDIR/qml \ $$BASEDIR/data \ $$BASEDIR/sik_uploader - + WindowsBuild { - DESTDIR_COPY_RESOURCE_LIST = $$replace(DESTDIR,"/","\\") + DESTDIR_COPY_RESOURCE_LIST = $$replace(DESTDIR,"/","\\") COPY_RESOURCE_LIST = $$replace(COPY_RESOURCE_LIST, "/","\\") CONCATCMD = $$escape_expand(\\n) + for(COPY_DIR, COPY_RESOURCE_LIST):QMAKE_POST_LINK += $$CONCATCMD $$QMAKE_COPY_DIR $${COPY_DIR} $$DESTDIR_COPY_RESOURCE_LIST"\\"$$basename(COPY_DIR) } LinuxBuild { DESTDIR_COPY_RESOURCE_LIST = $$DESTDIR CONCATCMD = && + for(COPY_DIR, COPY_RESOURCE_LIST):QMAKE_POST_LINK += $$CONCATCMD $$QMAKE_COPY_DIR $${COPY_DIR} $$DESTDIR_COPY_RESOURCE_LIST + } MacBuild { DESTDIR_COPY_RESOURCE_LIST = $$DESTDIR/$${TARGET}.app/Contents/MacOS CONCATCMD = && + for(COPY_DIR, COPY_RESOURCE_LIST):QMAKE_POST_LINK += $$CONCATCMD $$QMAKE_COPY_DIR $${COPY_DIR} $$DESTDIR_COPY_RESOURCE_LIST + } - -for(COPY_DIR, COPY_RESOURCE_LIST):QMAKE_POST_LINK += $$CONCATCMD $$QMAKE_COPY_DIR $${COPY_DIR} $$DESTDIR_COPY_RESOURCE_LIST # # Perform platform specific setup @@ -71,16 +74,16 @@ WindowsBuild { DebugBuild: DLL_QT_DEBUGCHAR = "d" ReleaseBuild: DLL_QT_DEBUGCHAR = "" COPY_FILE_LIST = \ - $$BASEDIR_WIN\\libs\\lib\\sdl\\win32\\SDL.dll \ + $$BASEDIR_WIN\\libs\\lib\\sdl\\win32\\SDL2.dll \ $$BASEDIR_WIN\\libs\\thirdParty\\libxbee\\lib\\libxbee.dll \ $$(QTDIR)\\bin\\Qt5WebKitWidgets$${DLL_QT_DEBUGCHAR}.dll \ $$(QTDIR)\\bin\\Qt5MultimediaWidgets$${DLL_QT_DEBUGCHAR}.dll \ $$(QTDIR)\\bin\\Qt5Multimedia$${DLL_QT_DEBUGCHAR}.dll \ $$(QTDIR)\\bin\\Qt5Gui$${DLL_QT_DEBUGCHAR}.dll \ $$(QTDIR)\\bin\\Qt5Core$${DLL_QT_DEBUGCHAR}.dll \ - $$(QTDIR)\\bin\\icuin51.dll \ - $$(QTDIR)\\bin\\icuuc51.dll \ - $$(QTDIR)\\bin\\icudt51.dll \ + $$(QTDIR)\\bin\\icuin53.dll \ + $$(QTDIR)\\bin\\icuuc53.dll \ + $$(QTDIR)\\bin\\icudt53.dll \ $$(QTDIR)\\bin\\Qt5Network$${DLL_QT_DEBUGCHAR}.dll \ $$(QTDIR)\\bin\\Qt5Widgets$${DLL_QT_DEBUGCHAR}.dll \ $$(QTDIR)\\bin\\Qt5OpenGL$${DLL_QT_DEBUGCHAR}.dll \ @@ -97,6 +100,7 @@ WindowsBuild { $$(QTDIR)\\bin\\Qt5Script$${DLL_QT_DEBUGCHAR}.dll \ $$(QTDIR)\\bin\\Qt5Svg$${DLL_QT_DEBUGCHAR}.dll \ $$(QTDIR)\\bin\\Qt5Test$${DLL_QT_DEBUGCHAR}.dll \ + $$(QTDIR)\\bin\\Qt5WebChannel$${DLL_QT_DEBUGCHAR}.dll \ $$(QTDIR)\\bin\\Qt5SerialPort$${DLL_QT_DEBUGCHAR}.dll for(COPY_FILE, COPY_FILE_LIST) { @@ -104,15 +108,15 @@ WindowsBuild { } ReleaseBuild { - QMAKE_POST_LINK += $$escape_expand(\\n) $$quote(del /F "$$DESTDIR_WIN\\$${TARGET}.exp") - # Copy Visual Studio DLLs - # Note that this is only done for release because the debugging versions of these DLLs cannot be redistributed. - # I'm not certain of the path for VS2008, so this only works for VS2010. win32-msvc2010 { QMAKE_POST_LINK += $$escape_expand(\\n) $$quote(xcopy /D /Y "\"C:\\Program Files \(x86\)\\Microsoft Visual Studio 10.0\\VC\\redist\\x86\\Microsoft.VC100.CRT\\*.dll\"" "$$DESTDIR_WIN\\") - } - } + QMAKE_POST_LINK += $$escape_expand(\\n) $$quote(del /F "$$DESTDIR_WIN\\$${TARGET}.exp") + } + win32-msvc2013 { + QMAKE_POST_LINK += $$escape_expand(\\n) $$quote(xcopy /D /Y "\"C:\\Program Files \(x86\)\\Microsoft Visual Studio 12.0\\VC\\redist\\x86\\Microsoft.VC120.CRT\\*.dll\"" "$$DESTDIR_WIN\\") + } + } } LinuxBuild { diff --git a/README.md b/README.md index 99bf9ab285..c6bb9ee853 100644 --- a/README.md +++ b/README.md @@ -48,6 +48,8 @@ Linux Building on Linux (tested against Ubuntu 14.04 LTS): +(**NOTE:** There is an error in 14.04LTS with libxcb crashing sometimes, try this [fix](https://gist.github.com/slimsag/a26d838ccc4480ce21bc)) + 1) Install the required packages: Be sure to run apt-get update first @@ -57,7 +59,8 @@ sudo apt-get update sudo apt-get install git qt5-qmake qt5-default \ qtscript5-dev libqt5webkit5-dev libqt5serialport5-dev \ libqt5svg5-dev libsdl1.2-dev libsndfile-dev \ - flite1-dev libssl-dev libudev-dev libsdl2-dev + flite1-dev libssl-dev libudev-dev libsdl2-dev \ + qtdeclarative5-qtquick2-plugin ``` 2) Clone the repository in your workspace: diff --git a/apm_planner_version.json b/apm_planner_version.json index 04458fe074..d8887da00d 100644 --- a/apm_planner_version.json +++ b/apm_planner_version.json @@ -5,110 +5,74 @@ { "platform": "osx", "type": "stable", - "url": "http://firmware.diydrones.com/Tools/APMPlanner/apm_planner_2.0.15_osx.dmg", - "name" : "apm_planner_2.0.15_osx.dmg", - "version" : "2.0.15", + "url": "http://firmware.diydrones.com/Tools/APMPlanner/apm_planner_2.0.17_osx.dmg", + "name" : "apm_planner_2.0.17_osx.dmg", + "version" : "2.0.17", "sha" : "", - "date" : "2014-11-18" + "date" : "2014-04-05" }, { "platform": "win", "type": "stable", - "url": "http://firmware.diydrones.com/Tools/APMPlanner/apm_planner_2.0.15_win.exe", - "name" : "apm_planner_2.0.15_win.exe", - "version": "2.0.15", + "url": "http://firmware.diydrones.com/Tools/APMPlanner/apm_planner_2.0.17_win.exe", + "name" : "apm_planner_2.0.17_win.exe", + "version": "2.0.17", "sha" : "", - "date" : "2014-11-18" - }, - { - "platform": "debian", - "type": "stable", - "url": "http://firmware.diydrones.com/Tools/APMPlanner/apm_planner_2.0.14_debian.deb", - "name" : "apm_planner_2.0.14_debian.deb", - "version": "2.0.14", - "sha" : "", - "date" : "2014-08-01" - }, - { - "platform": "debian64", - "type": "stable", - "url": "http://firmware.diydrones.com/Tools/APMPlanner/apm_planner_2.0.14_debian64.deb", - "name" : "apm_planner_2.0.14_debian64.deb", - "version": "2.0.14", - "sha" : "", - "date" : "2014-08-01" - }, - { - "platform": "ubuntu", - "type": "stable", - "url": "http://firmware.diydrones.com/Tools/APMPlanner/apm_planner_2.0.14_ubuntu.deb", - "name" : "apm_planner_2.0.14_ubuntu.deb", - "version": "2.0.14", - "sha" : "", - "date" : "2014-08-01" - }, - { - "platform": "ubuntu64", - "type": "stable", - "url": "http://firmware.diydrones.com/Tools/APMPlanner/apm_planner_2.0.14_ubuntu64.deb", - "name" : "apm_planner_2.0.14_ubuntu64.deb", - "version": "2.0.14", - "sha" : "", - "date" : "2014-08-01" + "date" : "2014-04-05" }, { "platform": "ubuntu_trusty", "type": "stable", - "url": "http://firmware.diydrones.com/Tools/APMPlanner/apm_planner_2.0.15_ubuntu_trusty.deb", - "name" : "apm_planner_2.0.15_ubuntu_trusty.deb", - "version": "2.0.15", + "url": "http://firmware.diydrones.com/Tools/APMPlanner/apm_planner_2.0.17_ubuntu_trusty.deb", + "name" : "apm_planner_2.0.17_ubuntu_trusty.deb", + "version": "2.0.17", "sha" : "", - "date" : "2014-11-18" + "date" : "2014-04-05" }, { "platform": "ubuntu_trusty64", "type": "stable", - "url": "http://firmware.diydrones.com/Tools/APMPlanner/apm_planner_2.0.15_ubuntu_trusty64.deb", - "name" : "apm_planner_2.0.15_ubuntu_trusty64.deb", - "version": "2.0.15", + "url": "http://firmware.diydrones.com/Tools/APMPlanner/apm_planner_2.0.17_ubuntu_trusty64.deb", + "name" : "apm_planner_2.0.17_ubuntu_trusty64.deb", + "version": "2.0.17", "sha" : "", - "date" : "2014-11-18" + "date" : "2014-04-05" }, { "platform": "osx", "type": "beta", - "url": "http://firmware.diydrones.com/Tools/APMPlanner/beta/apm_planner_2.0.16-rc3_osx.dmg", - "name" : "apm_planner_2.0.16-rc3_osx.dmg", - "version" : "2.0.16-rc3", + "url": "http://firmware.diydrones.com/Tools/APMPlanner/beta/apm_planner_2.0.18-rc2_osx.dmg", + "name" : "apm_planner_2.0.18-rc2_osx.dmg", + "version" : "2.0.18-rc2", "sha" : "", - "date" : "2015-02-06" + "date" : "2015-06-17" }, { "platform": "win", "type": "beta", - "url": "http://firmware.diydrones.com/Tools/APMPlanner/beta/apm_planner_2.0.16-rc3_win.exe", - "name" : "apm_planner_2.0.16-rc3_win.exe", - "version": "2.0.16-rc3", + "url": "http://firmware.diydrones.com/Tools/APMPlanner/beta/apm_planner_2.0.18-rc2_win.exe", + "name" : "apm_planner_2.0.18-rc2_win.exe", + "version": "2.0.18-rc2", "sha" : "", - "date" : "2015-02-06" + "date" : "2015-06-17" }, { "platform": "ubuntu_trusty", "type": "beta", - "url": "http://firmware.diydrones.com/Tools/APMPlanner/beta/apm_planner_2.0.16-rc3_ubuntu_trusty.deb", - "name" : "apm_planner_2.0.16-rc3_ubuntu_trusty.deb", - "version": "2.0.16-rc3", + "url": "http://firmware.diydrones.com/Tools/APMPlanner/beta/apm_planner_2.0.18-rc2_ubuntu_trusty.deb", + "name" : "apm_planner_2.0.18-rc2_ubuntu_trusty.deb", + "version": "2.0.18-rc2", "sha" : "", - "date" : "2015-02-06" + "date" : "2015-06-17" }, { "platform": "ubuntu_trusty64", "type": "beta", - "url": "http://firmware.diydrones.com/Tools/APMPlanner/beta/apm_planner_2.0.16-rc3_ubuntu_trusty64.deb", - "name" : "apm_planner_2.0.16-rc3_ubuntu_trusty64.deb", - "version": "2.0.16-rc3", + "url": "http://firmware.diydrones.com/Tools/APMPlanner/beta/apm_planner_2.0.18-rc2_ubuntu_trusty64.deb", + "name" : "apm_planner_2.0.18-rc2_ubuntu_trusty64.deb", + "version": "2.0.18-rc2", "sha" : "", - "date" : "2015-02-06" + "date" : "2015-06-17" } ] } diff --git a/libs/lib/sdl/include/SDL2/SDL_platform.h b/libs/lib/sdl/include/SDL2/SDL_platform.h index 83381be918..c43f4b54a0 100644 --- a/libs/lib/sdl/include/SDL2/SDL_platform.h +++ b/libs/lib/sdl/include/SDL2/SDL_platform.h @@ -114,10 +114,10 @@ #define __SOLARIS__ 1 #endif -#if defined(WIN32) || defined(_WIN32) || defined(__CYGWIN__) || defined(__MINGW32__) +#if defined(WIN32) || defined(_WIN32) || defined(__CYGWIN__) /* Try to find out if we're compiling for WinRT or non-WinRT */ /* If _USING_V110_SDK71_ is defined it means we are using the v110_xp or v120_xp toolset. */ -#if (defined(_MSC_VER) && (_MSC_VER >= 1700) && !_USING_V110_SDK71_) /* _MSC_VER==1700 for MSVC 2012 */ +#if defined(__MINGW32__) || (defined(_MSC_VER) && (_MSC_VER >= 1700) && !_USING_V110_SDK71_) /* _MSC_VER==1700 for MSVC 2012 */ #include #if WINAPI_FAMILY_PARTITION(WINAPI_PARTITION_DESKTOP) #undef __WINDOWS__ diff --git a/libs/lib/sdl/msvc/lib/SDL2.dll b/libs/lib/sdl/msvc/lib/SDL2.dll index a65b22ce9d..18d707f481 100644 Binary files a/libs/lib/sdl/msvc/lib/SDL2.dll and b/libs/lib/sdl/msvc/lib/SDL2.dll differ diff --git a/libs/lib/sdl/msvc/lib/SDL2.lib b/libs/lib/sdl/msvc/lib/SDL2.lib index 6faf131fb9..88452a9042 100644 Binary files a/libs/lib/sdl/msvc/lib/SDL2.lib and b/libs/lib/sdl/msvc/lib/SDL2.lib differ diff --git a/libs/lib/sdl/msvc/lib/SDL2main.lib b/libs/lib/sdl/msvc/lib/SDL2main.lib index 699f8f6d80..ad65ae091b 100644 Binary files a/libs/lib/sdl/msvc/lib/SDL2main.lib and b/libs/lib/sdl/msvc/lib/SDL2main.lib differ diff --git a/libs/lib/sdl/msvc/lib/SDL2test.lib b/libs/lib/sdl/msvc/lib/SDL2test.lib index e4d409b4cc..09e11c655d 100644 Binary files a/libs/lib/sdl/msvc/lib/SDL2test.lib and b/libs/lib/sdl/msvc/lib/SDL2test.lib differ diff --git a/libs/opmapcontrol/src/core/opmaps.cpp b/libs/opmapcontrol/src/core/opmaps.cpp index 7fe8392b06..8a875469cc 100644 --- a/libs/opmapcontrol/src/core/opmaps.cpp +++ b/libs/opmapcontrol/src/core/opmaps.cpp @@ -39,7 +39,7 @@ namespace core { OPMaps::OPMaps():RetryLoadTile(2),useMemoryCache(true) { accessmode=AccessMode::ServerAndCache; - Language=LanguageType::PortuguesePortugal; + Language=LanguageType::English; LanguageStr=LanguageType().toShortString(Language); Cache::Instance(); @@ -221,6 +221,9 @@ namespace core { tT.stop(); if( (reply->error()!=QNetworkReply::NoError)) { +#ifdef DEBUG_GMAPS + qDebug() << " Download Tile Network error: " << reply->errorString(); +#endif errorvars.lock(); ++diag.networkerrors; errorvars.unlock(); diff --git a/libs/opmapcontrol/src/core/providerstrings.cpp b/libs/opmapcontrol/src/core/providerstrings.cpp index be3e50df75..59f7518d45 100644 --- a/libs/opmapcontrol/src/core/providerstrings.cpp +++ b/libs/opmapcontrol/src/core/providerstrings.cpp @@ -42,10 +42,10 @@ namespace core { // VersionGoogleLabels = "h@132"; // VersionGoogleTerrain = "t@125,r@132"; // Google version strings - VersionGoogleMap = "m@284"; - VersionGoogleSatellite = "163"; - VersionGoogleLabels = "h@284"; - VersionGoogleTerrain = "t@132,r@284"; + VersionGoogleMap = "m@301"; + VersionGoogleSatellite = "171"; + VersionGoogleLabels = "h@301"; + VersionGoogleTerrain = "t@132,r@301"; SecGoogleWord = "Galileo"; // Google (China) version strings diff --git a/libs/opmapcontrol/src/core/urlfactory.cpp b/libs/opmapcontrol/src/core/urlfactory.cpp index ca361ce208..7c26bf046c 100644 --- a/libs/opmapcontrol/src/core/urlfactory.cpp +++ b/libs/opmapcontrol/src/core/urlfactory.cpp @@ -111,7 +111,7 @@ namespace core { qDebug()<<"Correct GoogleVersion"; #endif //DEBUG_URLFACTORY setIsCorrectGoogleVersions(true); - QString url = "https://maps.google.com"; + QString url = "http://maps.google.com"; qheader.setUrl(QUrl(url)); qheader.setRawHeader("User-Agent",UserAgent); @@ -129,7 +129,7 @@ namespace core { return; } QString html=QString(reply->readAll()); - QRegExp reg("\"*https://mt0.google.com/vt/lyrs=m@(\\d*)",Qt::CaseInsensitive); + QRegExp reg("\"*http://mt0.google.com/vt/lyrs=m@(\\d*)",Qt::CaseInsensitive); if(reg.indexIn(html)!=-1) { QStringList gc=reg.capturedTexts(); @@ -140,7 +140,7 @@ namespace core { #endif //DEBUG_URLFACTORY } - reg=QRegExp("\"*https://mt0.google.com/vt/lyrs=h@(\\d*)",Qt::CaseInsensitive); + reg=QRegExp("\"*http://mt0.google.com/vt/lyrs=h@(\\d*)",Qt::CaseInsensitive); if(reg.indexIn(html)!=-1) { QStringList gc=reg.capturedTexts(); @@ -150,7 +150,7 @@ namespace core { qDebug()<<"TryCorrectGoogleVersions, VersionGoogleLabels: "< $NEW_SPEC" or die $!; while () { @@ -49,6 +49,16 @@ close SPEC_OUT; print "done\n"; +print "Copying diydrones.repo ... "; +$REPO_FILE = "${WORK_PATH}/redhat/diydrones.repo"; +system "cp ${REPO_FILE} ${RPMBUILD_ROOT}/SOURCES"; +print "done\n"; + +print "Copying diydrones-repo.spec ... "; +$OLD_SPEC = "${WORK_PATH}/redhat/diydrones-repo.spec"; +system "cp ${OLD_SPEC} ${RPMBUILD_ROOT}/SPECS"; +print "done\n"; + sub get_release { my $vs = shift; $vs =~ /^(\d+(?:\.\d+)+)-(.+)$/; diff --git a/src/QGC.h b/src/QGC.h index 0aa6a43d74..4b3e37c64f 100644 --- a/src/QGC.h +++ b/src/QGC.h @@ -35,7 +35,9 @@ #define define2string(x) define2string_p(x) /* Windows fixes */ -#ifdef _MSC_VER +#if defined(_MSC_VER) && _MSC_VER>=1800 + #include +#elif defined(_MSC_VER) /* Needed define for Eigen */ //#define NOMINMAX #include diff --git a/src/audio/AlsaAudio.cc b/src/audio/AlsaAudio.cc index faf6e77da4..9a16ebd5a6 100644 --- a/src/audio/AlsaAudio.cc +++ b/src/audio/AlsaAudio.cc @@ -178,7 +178,7 @@ bool AlsaAudio::alsa_play( QString filename ) snd_pcm_t * AlsaAudio::alsa_open (int channels, int samplerate) { - const char * device = "plughw:0"; + const char * device = "default"; snd_pcm_t *alsa_dev; snd_pcm_hw_params_t *hw_params; snd_pcm_uframes_t buffer_size, xfer_align, start_threshold; diff --git a/src/comm/LinkInterface.h b/src/comm/LinkInterface.h index 14cdee74cc..bf12ee8cc4 100644 --- a/src/comm/LinkInterface.h +++ b/src/comm/LinkInterface.h @@ -51,6 +51,7 @@ class LinkInterface : public QThread SERIAL_LINK, TCP_LINK, UDP_LINK, + UDP_CLIENT_LINK, SIM_LINK, UNKNOWN_LINK }; @@ -79,6 +80,16 @@ class LinkInterface : public QThread */ virtual QString getName() const = 0; + /** + * @brief Get the human readable name of this link + */ + virtual QString getShortName() const = 0; + + /** + * @brief Get the detail of this link (ie. baud, ip addres etc) + */ + virtual QString getDetail() const = 0; + virtual void requestReset() = 0; /** @@ -136,6 +147,7 @@ class LinkInterface : public QThread virtual LinkType getLinkType() { return UNKNOWN_LINK; } + public slots: /** @@ -219,6 +231,12 @@ public slots: void error(LinkInterface* link,QString errorstring); + /** + * @brief Sends an update message when the link parameters are changed + * + */ + void linkChanged(LinkInterface* link); + protected: static const int dataRateBufferSize = 20; ///< Specify how many data points to capture for data rate calculations. diff --git a/src/comm/LinkManager.cc b/src/comm/LinkManager.cc index 475f647b60..832a32f91c 100644 --- a/src/comm/LinkManager.cc +++ b/src/comm/LinkManager.cc @@ -29,13 +29,15 @@ This file is part of the APM_PLANNER project * */ - +#include "LinkManagerFactory.h" #include "LinkManager.h" #include "PxQuadMAV.h" #include "SlugsMAV.h" #include "ArduPilotMegaMAV.h" #include "UASManager.h" +#include "serialconnection.h" #include "UDPLink.h" +#include "UDPClientLink.h" #include "TCPLink.h" #include "UASObject.h" #include @@ -43,6 +45,17 @@ This file is part of the APM_PLANNER project #include #include + +LinkManager* LinkManager::instance() +{ + static LinkManager* _instance = 0; + if(_instance == 0) + { + _instance = new LinkManager(); + } + return _instance; +} + LinkManager::LinkManager(QObject *parent) : QObject(parent) { @@ -57,14 +70,11 @@ LinkManager::LinkManager(QObject *parent) : QTimer::singleShot(500, this, SLOT(reloadSettings())); } - void LinkManager::reloadSettings() { loadSettings(); //Check to see if we have a single serial and single UDP connection, since they are the defaults - /*//LinkManager::instance()->addSerialConnection(); - //LinkManager::instance()->addUdpConnection(QHostAddress::Any,14550);*/ bool foundserial = false; bool foundudp = false; for (QMap::const_iterator i= m_connectionMap.constBegin();i!=m_connectionMap.constEnd();i++) @@ -80,13 +90,14 @@ void LinkManager::reloadSettings() } if (!foundserial) { - addSerialConnection(); + LinkManagerFactory::addSerialConnection(); } if (!foundudp) { - addUdpConnection(QHostAddress::Any,14550); + LinkManagerFactory::addUdpConnection(QHostAddress::Any,14550); } } + void LinkManager::stopLogging() { if (!m_mavlinkLoggingEnabled) @@ -125,13 +136,13 @@ void LinkManager::loadSettings() baud = 115200; } - addSerialConnection(port,baud); + LinkManagerFactory::addSerialConnection(port,baud); } else if (type == "UDP_LINK") { int port = settings.value("port").toInt(); - int linkid = addUdpConnection(QHostAddress::Any,port); - UDPLink *iface = qobject_cast(m_connectionMap.value(linkid)); + int linkid = LinkManagerFactory::addUdpConnection(QHostAddress::Any,port); + UDPLink *iface = qobject_cast(getLink(linkid)); int hostcount = settings.beginReadArray("HOSTS"); for (int j=0;jaddHost(tr("%1:%2").arg(host, port)); } - settings.endArray(); + settings.endArray(); // HOSTS } else if (type == "TCP_LINK") { QString host = settings.value("host").toString(); int port = settings.value("port").toInt(); bool asServer = settings.value("asServer").toBool(); - addTcpConnection(QHostAddress(host),port,asServer); + LinkManagerFactory::addTcpConnection(QHostAddress(host),port,asServer); + } + else if (type == "UDP_CLIENT_LINK") + { + QString host = settings.value("host").toString(); + int port = settings.value("port").toInt(); + LinkManagerFactory::addUdpClientConnection(QHostAddress(host),port); } } - + settings.endArray(); // HOSTS int portsize = settings.beginReadArray("PORTBAUDPAIRS"); for (int i=0;igetPort()); } + else if (i.value()->getLinkType() == LinkInterface::UDP_CLIENT_LINK) + { + UDPClientLink *link = qobject_cast(i.value()); + settings.setValue("type","UDP_CLIENT_LINK"); + settings.setValue("host",link->getHostAddress().toString()); + settings.setValue("port",link->getPort()); + } else if (i.value()->getLinkType() == LinkInterface::TCP_LINK) { TCPLink *link = qobject_cast(i.value()); @@ -203,7 +227,7 @@ void LinkManager::saveSettings() settings.setValue("asServer",link->isServer()); } } - settings.endArray(); + settings.endArray(); // LINKS settings.beginWriteArray("PORTBAUDPAIRS"); index = 0; for (QMap::const_iterator i=m_portToBaudMap.constBegin();i!=m_portToBaudMap.constEnd();i++) @@ -212,10 +236,11 @@ void LinkManager::saveSettings() settings.setValue("port",i.key()); settings.setValue("baud",i.value()); } - settings.endArray(); + settings.endArray(); // PORTBAUDPAIRS settings.endGroup(); settings.sync(); } + void LinkManager::setLogSubDirectory(QString dir) { if (!dir.startsWith("/")) @@ -236,6 +261,7 @@ void LinkManager::setLogSubDirectory(QString dir) logdir.mkdir(m_logSubDir.mid(1)); } } + void LinkManager::enableLogging(bool enabled) { @@ -267,20 +293,12 @@ void LinkManager::startLogging() m_mavlinkProtocol->startLogging(logFileName); } -int LinkManager::addSerialConnection() + +MAVLinkProtocol* LinkManager::getProtocol() const { - //Add with defaults - SerialConnection *connection = new SerialConnection(); - connect(connection,SIGNAL(bytesReceived(LinkInterface*,QByteArray)),m_mavlinkProtocol,SLOT(receiveBytes(LinkInterface*,QByteArray))); - connect(connection,SIGNAL(connected(LinkInterface*)),this,SLOT(linkConnected(LinkInterface*))); - connect(connection,SIGNAL(disconnected(LinkInterface*)),this,SLOT(linkDisonnected(LinkInterface*))); - connect(connection,SIGNAL(error(LinkInterface*,QString)),this,SLOT(linkErrorRec(LinkInterface*,QString))); - connect(connection,SIGNAL(timeoutTriggered(LinkInterface*)),this,SLOT(linkTimeoutTriggered(LinkInterface*))); - m_connectionMap.insert(connection->getId(),connection); - emit newLink(connection->getId()); - saveSettings(); - return connection->getId(); + return m_mavlinkProtocol; } + LinkInterface::LinkType LinkManager::getLinkType(int linkid) { if (!m_connectionMap.contains(linkid)) @@ -290,57 +308,19 @@ LinkInterface::LinkType LinkManager::getLinkType(int linkid) return m_connectionMap.value(linkid)->getLinkType(); } -int LinkManager::addSerialConnection(QString port,int baud) -{ - SerialConnection *connection = new SerialConnection(); - connect(connection,SIGNAL(bytesReceived(LinkInterface*,QByteArray)),m_mavlinkProtocol,SLOT(receiveBytes(LinkInterface*,QByteArray))); - connect(connection,SIGNAL(connected(LinkInterface*)),this,SLOT(linkConnected(LinkInterface*))); - connect(connection,SIGNAL(disconnected(LinkInterface*)),this,SLOT(linkDisonnected(LinkInterface*))); - connect(connection,SIGNAL(error(LinkInterface*,QString)),this,SLOT(linkErrorRec(LinkInterface*,QString))); - connect(connection,SIGNAL(timeoutTriggered(LinkInterface*)),this,SLOT(linkTimeoutTriggered(LinkInterface*))); - connection->setPortName(port); - connection->setBaudRate(baud); - m_connectionMap.insert(connection->getId(),connection); - //emit newLink(connection); - emit newLink(connection->getId()); - saveSettings(); - return connection->getId(); - -} -int LinkManager::addUdpConnection(QHostAddress addr,int port) -{ - UDPLink* udpLink = new UDPLink(addr,port); - connect(udpLink,SIGNAL(bytesReceived(LinkInterface*,QByteArray)),m_mavlinkProtocol,SLOT(receiveBytes(LinkInterface*,QByteArray))); - connect(udpLink,SIGNAL(connected(LinkInterface*)),this,SLOT(linkConnected(LinkInterface*))); - connect(udpLink,SIGNAL(disconnected(LinkInterface*)),this,SLOT(linkDisonnected(LinkInterface*))); - connect(udpLink,SIGNAL(error(LinkInterface*,QString)),this,SLOT(linkErrorRec(LinkInterface*,QString))); - m_connectionMap.insert(udpLink->getId(),udpLink); - emit newLink(udpLink->getId()); - saveSettings(); - udpLink->connect(); - return udpLink->getId(); -} -int LinkManager::addTcpConnection(QHostAddress addr,int port,bool asServer) +void LinkManager::addLink(LinkInterface *link) { - TCPLink *tcplink = new TCPLink(addr,port,asServer); - connect(tcplink,SIGNAL(bytesReceived(LinkInterface*,QByteArray)),m_mavlinkProtocol,SLOT(receiveBytes(LinkInterface*,QByteArray))); - connect(tcplink,SIGNAL(connected(LinkInterface*)),this,SLOT(linkConnected(LinkInterface*))); - connect(tcplink,SIGNAL(disconnected(LinkInterface*)),this,SLOT(linkDisonnected(LinkInterface*))); - m_connectionMap.insert(tcplink->getId(),tcplink); - emit newLink(tcplink->getId()); - saveSettings(); - if (asServer) - { - tcplink->connect(); - } - return tcplink->getId(); + m_connectionMap.insert(link->getId(),link); + emit newLink(link->getId()); +// saveSettings(); } -void LinkManager::addLink(LinkInterface *link) +LinkInterface* LinkManager::getLink(int linkId) { - m_connectionMap.insert(link->getId(),link); + return m_connectionMap.value(linkId, 0); } + void LinkManager::removeLink(LinkInterface *link) { // This is called with a LINK_ID not an interface. needs mor rework @@ -370,6 +350,7 @@ bool LinkManager::connectLink(int index) } return false; } + void LinkManager::disconnectLink(int index) { if (m_connectionMap.contains(index)) @@ -377,56 +358,14 @@ void LinkManager::disconnectLink(int index) m_connectionMap.value(index)->disconnect(); } } -void LinkManager::modifyTcpConnection(int index,QHostAddress addr,int port,bool asServer) -{ - if (!m_connectionMap.contains(index)) - { - return; - } - TCPLink *iface = qobject_cast(m_connectionMap.value(index)); - if (!iface) - { - return; - } - iface->setHostAddress(addr); - iface->setPort(port); - iface->setAsServer(asServer); - emit linkChanged(index); - saveSettings(); -} -void LinkManager::modifySerialConnection(int index,QString port,int baud) +void LinkManager::linkUpdated(LinkInterface *link) { - if (!m_connectionMap.contains(index)) - { - return; - } - SerialLinkInterface *iface = qobject_cast(m_connectionMap.value(index)); - if (!iface) - { - return; - } - iface->setPortName(port); - if (baud != -1) - { - iface->setBaudRate(baud); - m_portToBaudMap[port] = baud; - } - else - { - //Check the map, if we've had a baud rate on that port before, use it - int newbaud = iface->getBaudRate(); - if (m_portToBaudMap.contains(port)) - { - newbaud = m_portToBaudMap.value(port); - } - iface->setBaudRate(newbaud); - m_portToBaudMap[port] = newbaud; - } - - emit linkChanged(index); - saveSettings(); + emit linkChanged(link); + emit linkChanged(link->getId()); + saveSettings(); // [todo] may need to verify if this is needed always (refactor to link objects) } + QString LinkManager::getLinkName(int linkid) { if (!m_connectionMap.contains(linkid)) @@ -436,109 +375,45 @@ QString LinkManager::getLinkName(int linkid) return m_connectionMap.value(linkid)->getName(); } -QString LinkManager::getSerialLinkPort(int linkid) +QString LinkManager::getLinkShortName(int linkid) { if (!m_connectionMap.contains(linkid)) { return ""; } - SerialLinkInterface *iface = qobject_cast(m_connectionMap.value(linkid)); - if (!iface) - { - return ""; - } - return iface->getPortName(); -} -bool LinkManager::getLinkConnected(int linkid) -{ - if (!m_connectionMap.contains(linkid)) - { - return ""; - } - return m_connectionMap.value(linkid)->isConnected(); + return m_connectionMap.value(linkid)->getShortName(); } -int LinkManager::getUdpLinkPort(int linkid) +QString LinkManager::getLinkDetail(int linkid) { if (!m_connectionMap.contains(linkid)) { - return 0; - } - UDPLink *iface = qobject_cast(m_connectionMap.value(linkid)); - if (!iface) - { - return 0; - } - return iface->getPort(); -} -int LinkManager::getTcpLinkPort(int linkid) -{ - if (!m_connectionMap.contains(linkid)) - { - return 0; - } - TCPLink *iface = qobject_cast(m_connectionMap.value(linkid)); - if (!iface) - { - return 0; - } - return iface->getPort(); -} - -QHostAddress LinkManager::getTcpLinkHost(int linkid) -{ - if (!m_connectionMap.contains(linkid)) - { - return QHostAddress::Null; - } - TCPLink *iface = qobject_cast(m_connectionMap.value(linkid)); - if (!iface) - { - return QHostAddress::Null; + return ""; } - return iface->getHostAddress(); -} - -bool LinkManager::isTcpServer(int linkid) -{ - if (!m_connectionMap.contains(linkid)) - return false; - - TCPLink *iface = qobject_cast(m_connectionMap.value(linkid)); - if (!iface) - return false; - return iface->isServer(); + return m_connectionMap.value(linkid)->getDetail(); } -void LinkManager::setUdpLinkPort(int linkid, int port) +QString LinkManager::getSerialLinkPort(int linkid) { if (!m_connectionMap.contains(linkid)) { - return; + return ""; } - UDPLink *iface = qobject_cast(m_connectionMap.value(linkid)); + SerialLinkInterface *iface = qobject_cast(m_connectionMap.value(linkid)); if (!iface) { - return; + return ""; } - iface->setPort(port); - emit linkChanged(linkid); - saveSettings(); + return iface->getPortName(); } -void LinkManager::addUdpHost(int linkid,QString hostname) +bool LinkManager::getLinkConnected(int linkid) { if (!m_connectionMap.contains(linkid)) { - return; - } - UDPLink *iface = qobject_cast(m_connectionMap.value(linkid)); - if (!iface) - { - return; + return ""; } - iface->addHost(hostname); - saveSettings(); + return m_connectionMap.value(linkid)->isConnected(); } int LinkManager::getSerialLinkBaud(int linkid) @@ -554,6 +429,7 @@ int LinkManager::getSerialLinkBaud(int linkid) } return iface->getBaudRate(); } + QList LinkManager::getCurrentPorts() { QList m_portList; @@ -574,11 +450,6 @@ QList LinkManager::getCurrentPorts() return m_portList; } -void LinkManager::removeSerialConnection(int index) -{ - -} - void LinkManager::receiveMessage(LinkInterface* link,mavlink_message_t message) { emit messageReceived(link,message); @@ -724,8 +595,8 @@ UASInterface* LinkManager::createUAS(MAVLinkProtocol* mavlink, LinkInterface* li UASManager::instance()->addUAS(uas); return uas; - } + UASObject *LinkManager::getUasObject(int uasid) { if (m_uasObjectMap.contains(uasid)) @@ -735,66 +606,12 @@ UASObject *LinkManager::getUasObject(int uasid) return 0; } -void LinkManager::setSerialParityType(int index,int parity) -{ - if (!m_connectionMap.contains(index)) - { - return; - } - SerialLinkInterface *iface = qobject_cast(m_connectionMap.value(index)); - if (!iface) - { - return; - } - iface->setParityType(parity); -} - -void LinkManager::setSerialFlowType(int index,int flow) -{ - if (!m_connectionMap.contains(index)) - { - return; - } - SerialLinkInterface *iface = qobject_cast(m_connectionMap.value(index)); - if (!iface) - { - return; - } - iface->setFlowType(flow); -} -void LinkManager::setSerialDataBits(int index,int bits) -{ - if (!m_connectionMap.contains(index)) - { - return; - } - SerialLinkInterface *iface = qobject_cast(m_connectionMap.value(index)); - if (!iface) - { - return; - } - iface->setDataBitsType(bits); -} - -void LinkManager::setSerialStopBits(int index,int bits) -{ - if (!m_connectionMap.contains(index)) - { - return; - } - SerialLinkInterface *iface = qobject_cast(m_connectionMap.value(index)); - if (!iface) - { - return; - } - iface->setStopBitsType(bits); -} - void LinkManager::protocolStatusMessageRec(QString title,QString text) { emit protocolStatusMessage(title,text); QLOG_DEBUG() << "Protocol Status Message:" << title << text; } + void LinkManager::linkConnected(LinkInterface* link) { emit linkChanged(link->getId()); @@ -804,10 +621,12 @@ void LinkManager::linkDisonnected(LinkInterface* link) { emit linkChanged(link->getId()); } + void LinkManager::linkErrorRec(LinkInterface *link,QString errorstring) { emit linkError(link->getId(),errorstring); } + void LinkManager::linkTimeoutTriggered(LinkInterface *link) { //Link has had a timeout @@ -815,6 +634,7 @@ void LinkManager::linkTimeoutTriggered(LinkInterface *link) //emit linkError(link->getId(),"Connected to link, but unable to receive any mavlink packets, (link is silent). Disconnecting"); //link->disconnect(); } + void LinkManager::disableTimeouts(int index) { if (!m_connectionMap.contains(index)) @@ -832,6 +652,7 @@ void LinkManager::enableTimeouts(int index) } m_connectionMap.value(index)->enableTimeouts(); } + void LinkManager::disableAllTimeouts() { for (QMap::const_iterator i = m_connectionMap.constBegin(); i != m_connectionMap.constEnd();i++) diff --git a/src/comm/LinkManager.h b/src/comm/LinkManager.h index c0c00e87ef..43eb1032ef 100644 --- a/src/comm/LinkManager.h +++ b/src/comm/LinkManager.h @@ -43,7 +43,6 @@ This file is part of the APM_PLANNER project * The mavlink decoder lives in its own thread * the UAS Class lives in the UI thread */ -#include "serialconnection.h" #include "MAVLinkDecoder.h" #include "MAVLinkProtocol.h" //#include "MAVLinkProtocol.h" @@ -56,40 +55,25 @@ class LinkManager : public QObject Q_OBJECT public: explicit LinkManager(QObject *parent = 0); - static LinkManager* instance() - { - static LinkManager* _instance = 0; - if(_instance == 0) - { - _instance = new LinkManager(); - } - return _instance; - } + static LinkManager* instance(); ~LinkManager(); + void disableTimeouts(int index); void enableTimeouts(int index); void disableAllTimeouts(); void enableAllTimeouts(); - void loadSettings(); - void saveSettings(); - int addSerialConnection(QString port,int baud); - int addSerialConnection(); - int addUdpConnection(QHostAddress addr,int port); - int addTcpConnection(QHostAddress addr,int port,bool asServer); - void modifySerialConnection(int index,QString port,int baud = -1); - void modifyTcpConnection(int index,QHostAddress addr,int port,bool asServer); - void setSerialParityType(int index,int parity); - void setSerialFlowType(int index,int flow); - void setSerialDataBits(int index,int bits); - void setSerialStopBits(int index,int bits); - void removeSerialConnection(int index); + + MAVLinkProtocol* getProtocol() const; bool connectLink(int index); void disconnectLink(int index); + UASInterface* getUas(int id); UASInterface* createUAS(MAVLinkProtocol* mavlink, LinkInterface* link, int sysid, mavlink_heartbeat_t* heartbeat, QObject* parent=NULL); + void addLink(LinkInterface *link); QList getLinks(); + LinkInterface* getLink(int linkId); // Remove a link based on instance void removeLink(LinkInterface *link); // Remove a link based on unique id @@ -97,15 +81,13 @@ class LinkManager : public QObject LinkInterface::LinkType getLinkType(int linkid); bool getLinkConnected(int linkid); - QString getSerialLinkPort(int linkid); - QString getLinkName(int linkid); - int getSerialLinkBaud(int linkid); - int getUdpLinkPort(int linkid); - int getTcpLinkPort(int linkid); - QHostAddress getTcpLinkHost(int linkid); - bool isTcpServer(int linkid); - void setUdpLinkPort(int linkid, int port); - void addUdpHost(int linkid,QString hostname); + + QString getSerialLinkPort(int linkid); // [TODO] remove + QString getLinkName(int linkid); // [TODO] remove + QString getLinkShortName(int linkid); // [TODO] remove + QString getLinkDetail(int linkid); // [TODO] remove + int getSerialLinkBaud(int linkid); // [TODO] remove + QList getCurrentPorts(); void stopLogging(); void startLogging(); @@ -114,14 +96,18 @@ class LinkManager : public QObject UASObject *getUasObject(int uasid); QMap m_uasObjectMap; // [TODO] make private - void addSimObject(uint8_t sysid,UASObject *obj); - void removeSimObject(uint8_t sysid); + void addSimObject(uint8_t sysid,UASObject *obj); // [TODO] remove + void removeSimObject(uint8_t sysid); // [TODO] remove signals: //void newLink(LinkInterface* link); void newLink(int linkid); void protocolStatusMessage(QString title,QString text); void linkChanged(int linkid); + + /** @brief aggregated signal for when link status changes */ + void linkChanged(LinkInterface *link); + void linkError(int linkid, QString message); void messageReceived(LinkInterface* link,mavlink_message_t message); @@ -130,6 +116,7 @@ public slots: void protocolStatusMessageRec(QString title,QString text); void enableLogging(bool enabled); void reloadSettings(); + void linkUpdated(LinkInterface* link); private slots: void linkConnected(LinkInterface* link); @@ -137,6 +124,10 @@ private slots: void linkErrorRec(LinkInterface* link,QString error); void linkTimeoutTriggered(LinkInterface*); +private: + void loadSettings(); + void saveSettings(); + private: QMap m_connectionMap; QMap m_uasMap; diff --git a/src/comm/LinkManagerFactory.cpp b/src/comm/LinkManagerFactory.cpp new file mode 100644 index 0000000000..4ce6cae047 --- /dev/null +++ b/src/comm/LinkManagerFactory.cpp @@ -0,0 +1,81 @@ +#include "LinkManagerFactory.h" +#include "serialconnection.h" +#include "UDPLink.h" +#include "UDPClientLink.h" +#include "TCPLink.h" + + +void LinkManagerFactory::connectLinkSignals(LinkInterface *link, LinkManager *lmgr) +{ + connect(link,SIGNAL(bytesReceived(LinkInterface*,QByteArray)),lmgr->getProtocol(),SLOT(receiveBytes(LinkInterface*,QByteArray))); + connect(link,SIGNAL(connected(LinkInterface*)),lmgr,SLOT(linkConnected(LinkInterface*))); + connect(link,SIGNAL(disconnected(LinkInterface*)),lmgr,SLOT(linkDisonnected(LinkInterface*))); + connect(link,SIGNAL(error(LinkInterface*,QString)),lmgr,SLOT(linkErrorRec(LinkInterface*,QString))); + connect(link, SIGNAL(linkChanged(LinkInterface*)),lmgr,SLOT(linkUpdated(LinkInterface*))); +} + +int LinkManagerFactory::addSerialConnection() +{ + LinkManager *lmgr = LinkManager::instance(); + SerialConnection *link = new SerialConnection(); + connectLinkSignals(link, lmgr); + + connect(link,SIGNAL(timeoutTriggered(LinkInterface*)),lmgr,SLOT(linkTimeoutTriggered(LinkInterface*))); + + lmgr->addLink(link); + return link->getId(); +} + +int LinkManagerFactory::addSerialConnection(QString port,int baud) +{ + LinkManager *lmgr = LinkManager::instance(); + SerialConnection *link = new SerialConnection(); + connectLinkSignals(link, lmgr); + + connect(link,SIGNAL(timeoutTriggered(LinkInterface*)),lmgr,SLOT(linkTimeoutTriggered(LinkInterface*))); + + link->setPortName(port); + link->setBaudRate(baud); + + lmgr->addLink(link); + return link->getId(); + +} +int LinkManagerFactory::addUdpConnection(QHostAddress addr,int port) +{ + LinkManager *lmgr = LinkManager::instance(); + UDPLink* link = new UDPLink(addr,port); + connectLinkSignals(link, lmgr); + + lmgr->addLink(link); + link->connect(); + return link->getId(); + +} + +int LinkManagerFactory::addUdpClientConnection(QHostAddress addr,int port) +{ + LinkManager *lmgr = LinkManager::instance(); + UDPClientLink* link = new UDPClientLink(addr,port); + connectLinkSignals(link, lmgr); + + lmgr->addLink(link); + return link->getId(); +} + +int LinkManagerFactory::addTcpConnection(QHostAddress addr,int port,bool asServer) +{ + LinkManager *lmgr = LinkManager::instance(); + + TCPLink *link = new TCPLink(addr,port,asServer); + + connectLinkSignals(link, lmgr); + + lmgr->addLink(link); + if (asServer) + { + link->connect(); + } + return link->getId(); +} + diff --git a/src/comm/LinkManagerFactory.h b/src/comm/LinkManagerFactory.h new file mode 100644 index 0000000000..5a2c25b513 --- /dev/null +++ b/src/comm/LinkManagerFactory.h @@ -0,0 +1,27 @@ +#ifndef LINKMANAGERFACTORY_H +#define LINKMANAGERFACTORY_H + +#include "LinkManager.h" +#include + +class LinkManagerFactory : public QObject +{ +// Q_OBJECT +public: +// explicit LinkManagerFactory(QObject *parent = 0); +// ~LinkManagerFactory(); + + // Serial Links + static int addSerialConnection(QString port,int baud); + static int addSerialConnection(); + + // IP Links + static int addUdpConnection(QHostAddress addr,int port); + static int addUdpClientConnection(QHostAddress addr,int port); + static int addTcpConnection(QHostAddress addr,int port,bool asServer); + +private: + static void connectLinkSignals(LinkInterface *link, LinkManager *lmgr); +}; + +#endif // LINKMANAGERFACTORY_H diff --git a/src/comm/MAVLinkDecoder.cc b/src/comm/MAVLinkDecoder.cc index b27a5dbf6a..de20a2a148 100644 --- a/src/comm/MAVLinkDecoder.cc +++ b/src/comm/MAVLinkDecoder.cc @@ -82,19 +82,21 @@ MAVLinkDecoder::~MAVLinkDecoder() mavlink_field_info_t MAVLinkDecoder::getFieldInfo(QString msgname,QString fieldname) { + mavlink_field_info_t fieldInfo; for (int i=0;i<256;i++) { if (msgname == messageInfo[i].name) { - for (int j=0;j MAVLinkDecoder::getFieldList(QString msgname) if (msgname == messageInfo[i].name) { - for (int j=0;j MAVLinkDecoder::getFieldList(QString msgname) void MAVLinkDecoder::sendMessage(mavlink_message_t msg) { - + Q_UNUSED(msg); } @@ -160,11 +162,27 @@ QList > MAVLinkDecoder::receiveMessage(LinkInterface* li if (QString(messageInfo[msgid].fields[fieldid].name) == QString("time_boot_ms") && messageInfo[msgid].fields[fieldid].type == MAVLINK_TYPE_UINT32_T) { time = *((quint32*)(m+messageInfo[msgid].fields[fieldid].wire_offset)); + + QPair fieldval; + fieldval.first = QString("M%1:%2.%3") + .arg(message.sysid) + .arg(messageInfo[msgid].name) + .arg(messageInfo[msgid].fields[fieldid].name); + fieldval.second = time; + retval.append(fieldval); } else if (QString(messageInfo[msgid].fields[fieldid].name).contains("usec") && messageInfo[msgid].fields[fieldid].type == MAVLINK_TYPE_UINT64_T) { time = *((quint64*)(m+messageInfo[msgid].fields[fieldid].wire_offset)); time = (time+500)/1000; // Scale to milliseconds, round up/down correctly + + QPair fieldval; + fieldval.first = QString("M%1:%2.%3") + .arg(message.sysid) + .arg(messageInfo[msgid].name) + .arg(messageInfo[msgid].fields[fieldid].name); + fieldval.second = *((quint64*)(m+messageInfo[msgid].fields[fieldid].wire_offset)); + retval.append(fieldval); } else { diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index 4a000283f8..48e70686f9 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -37,10 +37,10 @@ This file is part of the APM_PLANNER project #include "LinkManager.h" MAVLinkProtocol::MAVLinkProtocol(): + m_isOnline(true), m_loggingEnabled(false), m_logfile(NULL), - m_connectionManager(NULL), - m_isOnline(true) + m_connectionManager(NULL) { } @@ -307,7 +307,7 @@ void MAVLinkProtocol::handleMessage(quint64 timeid,LinkInterface *link) { if (lastIndex.value(message.sysid).contains(message.compid)) { - if (lastIndex.value(message.sysid).value(message.compid) == -1) + if (lastIndex.value(message.sysid).value(message.compid) == static_cast(-1)) { lastIndex[message.sysid][message.compid] = message.seq; expectedIndex = message.seq; diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc index 88767975b0..83d6cbc545 100644 --- a/src/comm/MAVLinkSimulationLink.cc +++ b/src/comm/MAVLinkSimulationLink.cc @@ -998,6 +998,16 @@ QString MAVLinkSimulationLink::getName() const return name; } +QString MAVLinkSimulationLink::getShortName() const +{ + return name; +} + +QString MAVLinkSimulationLink::getDetail() const +{ + return QString("sim"); +} + qint64 MAVLinkSimulationLink::getConnectionSpeed() const { /* 100 Mbit is reasonable fast and sufficient for all embedded applications */ diff --git a/src/comm/MAVLinkSimulationLink.h b/src/comm/MAVLinkSimulationLink.h index 0755b42927..fb68e2c0ef 100644 --- a/src/comm/MAVLinkSimulationLink.h +++ b/src/comm/MAVLinkSimulationLink.h @@ -66,6 +66,8 @@ class MAVLinkSimulationLink : public LinkInterface qint64 getCurrentOutDataRate() const; QString getName() const; + QString getShortName() const; + QString getDetail() const; int getId() const; int getBaudRate() const; int getBaudRateType() const; diff --git a/src/comm/TCPLink.cc b/src/comm/TCPLink.cc index 7309286779..124ab5c84f 100644 --- a/src/comm/TCPLink.cc +++ b/src/comm/TCPLink.cc @@ -74,6 +74,7 @@ void TCPLink::setHostAddress(QHostAddress hostAddress) } _hostAddress = hostAddress; + emit linkChanged(this); _resetName(); if (reconnect) { @@ -81,11 +82,6 @@ void TCPLink::setHostAddress(QHostAddress hostAddress) } } -void TCPLink::setHostAddress(const QString& hostAddress) -{ - setHostAddress(QHostAddress(hostAddress)); -} - void TCPLink::setPort(int port) { bool reconnect = false; @@ -96,6 +92,7 @@ void TCPLink::setPort(int port) } _port = port; + emit linkChanged(this); _resetName(); if (reconnect) { @@ -116,6 +113,7 @@ void TCPLink::setAsServer(bool asServer) } _asServer = asServer; + emit linkChanged(this); _resetName(); if (reconnect) { @@ -354,6 +352,16 @@ QString TCPLink::getName() const return _name; } +QString TCPLink::getShortName() const +{ + return _hostAddress.toString(); +} + +QString TCPLink::getDetail() const +{ + return QString::number(_port); +} + qint64 TCPLink::getConnectionSpeed() const { return 54000000; // 54 Mbit diff --git a/src/comm/TCPLink.h b/src/comm/TCPLink.h index cfbe87e592..d8b9131d0b 100644 --- a/src/comm/TCPLink.h +++ b/src/comm/TCPLink.h @@ -65,6 +65,8 @@ class TCPLink : public LinkInterface // LinkInterface methods virtual int getId(void) const; virtual QString getName(void) const; + virtual QString getShortName(void) const; + virtual QString getDetail(void) const; virtual bool isConnected(void) const; virtual bool connect(void); virtual bool disconnect(void); @@ -78,7 +80,6 @@ class TCPLink : public LinkInterface LinkType getLinkType() { return TCP_LINK; } public slots: - void setHostAddress(const QString& hostAddress); void setPort(int port); void setAsServer(bool asServer); diff --git a/src/comm/TLogReplayLink.cc b/src/comm/TLogReplayLink.cc index d72935560a..a2b167f14c 100644 --- a/src/comm/TLogReplayLink.cc +++ b/src/comm/TLogReplayLink.cc @@ -25,6 +25,16 @@ QString TLogReplayLink::getName() const { return "AP2SimulationLink"; } + +QString TLogReplayLink::getShortName() const +{ + return "AP2SimLink"; +} + +QString TLogReplayLink::getDetail() const +{ + return "sim"; +} void TLogReplayLink::requestReset() { @@ -265,6 +275,7 @@ void TLogReplayLink::run() msleep(1); } uas->receiveMessage(this,message); + LinkManager::instance()->getUasObject(message.sysid)->messageReceived(this,message); m_mavlinkDecoder->receiveMessage(this,message); if (m_mavlinkInspector) { diff --git a/src/comm/TLogReplayLink.h b/src/comm/TLogReplayLink.h index bab099c02e..7918949853 100644 --- a/src/comm/TLogReplayLink.h +++ b/src/comm/TLogReplayLink.h @@ -18,6 +18,8 @@ class TLogReplayLink : public LinkInterface bool isPaused(); int getId() const; QString getName() const; + QString getShortName() const; + QString getDetail() const; void requestReset(); bool isConnected() const; qint64 getConnectionSpeed() const; diff --git a/src/comm/UDPClientLink.cc b/src/comm/UDPClientLink.cc new file mode 100644 index 0000000000..11ef59240b --- /dev/null +++ b/src/comm/UDPClientLink.cc @@ -0,0 +1,302 @@ +/*===================================================================== + +APM Planner 2.0 Open Source Ground Control Station + +(c) 2015 APMPLANNER2 PROJECT + +This file is part of the APMPLANNER2 project + + APMPLANNER2 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. + + APMPLANNER2 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 APMPLANNER2. If not, see . + +======================================================================*/ + +/** + * @file + * @brief Definition of UDP Client connection for unmanned vehicles + * + */ + +#include "QsLog.h" +#include "UDPClientLink.h" +#include "LinkManager.h" +#include "QGC.h" + +#include +#include +#include +#include +#include + +UDPClientLink::UDPClientLink(QHostAddress host, quint16 port) : + _targetHost(host), + _port(port), + _packetsReceived(false) +{ + // Set unique ID and add link to the list of links + _linkId = getNextLinkId(); + setName(tr("UDP Client (%1:%2)").arg(_targetHost.toString()).arg(_port)); + QLOG_INFO() << "UDP Created " << _name; + + QObject::connect(&_socket, SIGNAL(readyRead()), this, SLOT(readBytes())); + + QObject::connect(&_socket, SIGNAL(error(QAbstractSocket::SocketError)), this, SLOT(_socketError(QAbstractSocket::SocketError))); + QObject::connect(&_socket, SIGNAL(disconnected()), this, SLOT(_socketDisconnected())); +} + +UDPClientLink::~UDPClientLink() +{ + disconnect(); + this->deleteLater(); +} + +/** + * @brief Runs the thread + * + **/ +void UDPClientLink::run() +{ + exec(); +} + +void UDPClientLink::setAddress(QHostAddress host) +{ + bool reconnect = false; + if(isConnected()) + { + disconnect(); + reconnect = true; + } + _targetHost = host; + setName(tr("UDP Client (%1:%2)").arg(_targetHost.toString()).arg(_port)); + emit linkChanged(this); + + if(reconnect) + { + connect(); + } +} + +void UDPClientLink::setPort(int port) +{ + bool reconnect = false; + if(isConnected()) + { + disconnect(); + reconnect = true; + } + _port = port; + setName(tr("UDP Client (%1:%2)").arg(_targetHost.toString()).arg(_port)); + emit linkChanged(this); + + if(reconnect) + { + connect(); + } +} + +void UDPClientLink::writeBytes(const char* data, qint64 size) +{ + if (!_socket.isOpen()) + return; + +// _socket.writeDatagram(data,size,_targetHost,_port); + _socket.write(data, size); + + // Log the amount and time written out for future data rate calculations. + QMutexLocker dataRateLocker(&dataRateMutex); + logDataRateToBuffer(outDataWriteAmounts, outDataWriteTimes, &outDataIndex, size, QDateTime::currentMSecsSinceEpoch()); +} + +/** + * @brief Read a number of bytes from the interface. + * + * @param data Pointer to the data byte array to write the bytes to + * @param maxLength The maximum number of bytes to write + **/ +void UDPClientLink::readBytes() +{ + while (_socket.bytesAvailable()) + { + _packetsReceived = true; + QByteArray datagram; + datagram.resize(_socket.bytesAvailable()); + + _socket.read(datagram.data(), datagram.size()); + + emit bytesReceived(this, datagram); + + // Log this data reception for this timestep + QMutexLocker dataRateLocker(&dataRateMutex); + logDataRateToBuffer(inDataWriteAmounts, inDataWriteTimes, &inDataIndex, datagram.length(), QDateTime::currentMSecsSinceEpoch()); + } +} + + +/** + * @brief Get the number of bytes to read. + * + * @return The number of bytes to read + **/ +qint64 UDPClientLink::bytesAvailable() +{ + return _socket.bytesAvailable(); +} + +/** + * @brief Disconnect the connection. + * + * @return True if connection has been disconnected, false if connection couldn't be disconnected. + **/ +bool UDPClientLink::disconnect() +{ + QLOG_INFO() << "UDP disconnect"; + quit(); + wait(); + + _socket.close(); + _packetsReceived = true; + + emit disconnected(); + emit connected(false); + emit disconnected(this); + emit linkChanged(this); + return false; +} + +/** + * @brief Connect the connection. + * + * @return True if connection has been established, false if connection couldn't be established. + **/ +bool UDPClientLink::connect() +{ + if (_socket.isOpen()) + disconnect(); + QLOG_INFO() << "UDPClientLink::UDP connect " << _targetHost.toString() << ":" << _port; + bool connected = _hardwareConnect(); + if (connected){ + start(NormalPriority); + } + return connected; +} + +bool UDPClientLink::_hardwareConnect() +{ + + if (!_targetHost.isNull() && _port!=0) { + QLOG_INFO() << "Connected UDP Client socket:" << _targetHost.toString(); + _socket.connectToHost(_targetHost,_port); + _socket.write(QByteArray("HELLO")); // Force Trigger connection. + emit connected(true); + emit connected(this); + emit connected(); + QTimer::singleShot(5000, this, SLOT(_sendTriggerMessage())); + return true; + } else { + QLOG_ERROR() << "connect failed! " << _targetHost.toString() << ":" << _port + << " err:" << _socket.error() << ": " << _socket.errorString(); + emit connected(false); + emit disconnected(this); + emit disconnected(); + } + return false; +} + +void UDPClientLink::_sendTriggerMessage() +{ + if (!_packetsReceived){ + QLOG_DEBUG() << "Send UDP Client HELLO" << _targetHost.toString(); + _socket.write("HELLO"); + QTimer::singleShot(5000, this, SLOT(_sendTriggerMessage())); + } +} + +void UDPClientLink::_socketError(QAbstractSocket::SocketError socketError) +{ + Q_UNUSED(socketError); + emit communicationError(getName(), "UDP Client error on socket: " + _socket.errorString()); +} + +void UDPClientLink::_socketDisconnected() +{ + qDebug() << _name << ": disconnected"; + _socket.close(); + + emit disconnected(); + emit connected(false); + emit disconnected(this); + emit linkChanged(this); +} +/** + * @brief Check if connection is active. + * + * @return True if link is connected, false otherwise. + **/ +bool UDPClientLink::isConnected() const +{ + return _socket.isOpen(); +} + +int UDPClientLink::getId() const +{ + return _linkId; +} + +QHostAddress UDPClientLink::getHostAddress() const +{ + return _targetHost; +} + +quint16 UDPClientLink::getPort() const +{ + return _port; +} + +QString UDPClientLink::getName() const +{ + return _name; +} + +QString UDPClientLink::getShortName() const +{ + return _targetHost.toString(); +} + +QString UDPClientLink::getDetail() const +{ + return QString::number(_port); +} + +void UDPClientLink::setName(QString name) +{ + _name = name; + emit nameChanged(_name); + emit linkChanged(this); +} + + +qint64 UDPClientLink::getConnectionSpeed() const +{ + return 54000000; // 54 Mbit +} + +qint64 UDPClientLink::getCurrentInDataRate() const +{ + return 0; +} + +qint64 UDPClientLink::getCurrentOutDataRate() const +{ + return 0; +} diff --git a/src/comm/UDPClientLink.h b/src/comm/UDPClientLink.h new file mode 100644 index 0000000000..f24cd48d01 --- /dev/null +++ b/src/comm/UDPClientLink.h @@ -0,0 +1,104 @@ +/*===================================================================== + +APM PLANNER 2.0 Open Source Ground Control Station + +(c) 2015 APMPLANNER PROJECT + +This file is part of the QGROUNDCONTROL project + + APMPLANNER 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. + + APMPLANNER 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 APMPLANNER. If not, see . + +======================================================================*/ + +#ifndef UDPCLIENTLINK_H +#define UDPCLIENTLINK_H + +#include +#include +#include +#include +#include +#include +#include + +class UDPClientLink : public LinkInterface +{ + Q_OBJECT + //Q_INTERFACES(UDPLinkInterface:LinkInterface) + +public: + UDPClientLink(QHostAddress host, quint16 port); + + ~UDPClientLink(); + void disableTimeouts() { } + void enableTimeouts() { } + void requestReset() { } + + bool isConnected() const; + qint64 bytesAvailable(); + + /** + * @brief The human readable port name + */ + QString getName() const; + QString getShortName() const; + QString getDetail() const; + QHostAddress getHostAddress() const; + quint16 getPort() const; + + // Extensive statistics for scientific purposes + qint64 getConnectionSpeed() const; + qint64 getCurrentInDataRate() const; + qint64 getCurrentOutDataRate() const; + + void run(); + + int getId() const; + + LinkType getLinkType() { return UDP_CLIENT_LINK; } + +public slots: + void setAddress(QHostAddress host); + void setPort(int port); + + void readBytes(); + void writeBytes(const char* data, qint64 length); + + bool connect(); + bool disconnect(); + +protected slots: + void _socketError(QAbstractSocket::SocketError socketError); + void _socketDisconnected(); + +private slots: + void _sendTriggerMessage(); + +private: // Helper Methods + void setName(QString name); + bool _hardwareConnect(); + void _resetName(); + +private: + QString _name; + QHostAddress _targetHost; + quint16 _port; + int _linkId; + QUdpSocket _socket; + bool _packetsReceived; + + QMutex _dataMutex; +}; + +#endif // UDPCLIENTLINK_H diff --git a/src/comm/UDPLink.cc b/src/comm/UDPLink.cc index e91c16fced..992f1c59c1 100644 --- a/src/comm/UDPLink.cc +++ b/src/comm/UDPLink.cc @@ -77,8 +77,10 @@ void UDPLink::setAddress(QHostAddress host) disconnect(); reconnect = true; } - this->host = host; - if(reconnect) + this->host = host; + emit linkChanged(this); + + if(reconnect) { connect(); } @@ -95,6 +97,8 @@ void UDPLink::setPort(int port) this->port = port; this->name = tr("UDP Link (port:%1)").arg(this->port); emit nameChanged(this->name); + emit linkChanged(this); + if(reconnect) { connect(); @@ -141,6 +145,7 @@ void UDPLink::addHost(const QString& host) ports.append(port); } } + emit linkChanged(this); } void UDPLink::removeHost(const QString& hostname) @@ -390,10 +395,21 @@ QString UDPLink::getName() const return name; } +QString UDPLink::getShortName() const +{ + return host.toString(); +} + +QString UDPLink::getDetail() const +{ + return QString::number(port); +} + void UDPLink::setName(QString name) { this->name = name; emit nameChanged(this->name); + emit linkChanged(this); } diff --git a/src/comm/UDPLink.h b/src/comm/UDPLink.h index 90b545d7a9..4c8fee3f1f 100644 --- a/src/comm/UDPLink.h +++ b/src/comm/UDPLink.h @@ -46,7 +46,7 @@ class UDPLink : public LinkInterface public: UDPLink(QHostAddress host = QHostAddress::Any, quint16 port = 14550); - //UDPLink(QHostAddress host = "239.255.76.67", quint16 port = 7667); + ~UDPLink(); void disableTimeouts() { } void enableTimeouts() { } @@ -63,6 +63,8 @@ class UDPLink : public LinkInterface * @brief The human readable port name */ QString getName() const; + QString getShortName() const; + QString getDetail() const; int getBaudRate() const; int getBaudRateType() const; int getFlowType() const; diff --git a/src/comm/serialconnection.cc b/src/comm/serialconnection.cc index 45aca430b7..8dc554d302 100644 --- a/src/comm/serialconnection.cc +++ b/src/comm/serialconnection.cc @@ -119,7 +119,7 @@ void SerialConnection::timeoutTimerTick() bool SerialConnection::setPortName(QString portName) { m_portName = portName; - emit updateLink(this); + emit linkChanged(this); writeSettings(); return true; } @@ -127,7 +127,7 @@ bool SerialConnection::setPortName(QString portName) bool SerialConnection::setBaudRate(int baud) { m_baud = baud; - emit updateLink(this); + emit linkChanged(this); writeSettings(); return true; } @@ -251,7 +251,7 @@ void SerialConnection::loadSettings() { m_baud = 115200; } - emit updateLink(this); + emit linkChanged(this); } void SerialConnection::writeSettings() { @@ -286,7 +286,13 @@ bool SerialConnection::connect() QObject::connect(m_port, SIGNAL(error(QSerialPort::SerialPortError)), this, SLOT(portError(QSerialPort::SerialPortError)), Qt::UniqueConnection); +#ifdef Q_OS_MACX + // temp fix Qt5.4.1 issue on OSX + // http://code.qt.io/cgit/qt/qtserialport.git/commit/?id=687dfa9312c1ef4894c32a1966b8ac968110b71e + m_port->setPortName("/dev/cu." + m_portName); +#else m_port->setPortName(m_portName); +#endif if (!m_port->open(QIODevice::ReadWrite)) { @@ -384,6 +390,15 @@ QString SerialConnection::getName() const return m_portName; } +QString SerialConnection::getShortName() const +{ + return m_portName; +} + +QString SerialConnection::getDetail() const +{ + return QString::number(m_baud); +} bool SerialConnection::isConnected() const { @@ -441,10 +456,10 @@ bool SerialConnection::setBaudRateString(QString rate) bool ok; int intrate = rate.toInt(&ok); if (!ok) { - emit updateLink(this); + emit linkChanged(this); return false; } - emit updateLink(this); + emit linkChanged(this); return setBaudRate(intrate); } diff --git a/src/comm/serialconnection.h b/src/comm/serialconnection.h index 102b40e4b6..710ea085d8 100644 --- a/src/comm/serialconnection.h +++ b/src/comm/serialconnection.h @@ -50,6 +50,8 @@ class SerialConnection : public SerialLinkInterface void enableTimeouts(); int getId() const; QString getName() const; + QString getShortName() const; + QString getDetail() const; void requestReset(); bool isConnected() const; qint64 getConnectionSpeed() const; @@ -90,7 +92,6 @@ public slots: bool setBaudRateString(QString baud); signals: - void updateLink(LinkInterface *link); void timeoutTriggered(LinkInterface*); private slots: diff --git a/src/configuration.h b/src/configuration.h index 64573cfc26..73b7356dd7 100644 --- a/src/configuration.h +++ b/src/configuration.h @@ -15,7 +15,7 @@ #define WITH_TEXT_TO_SPEECH 1 #define QGC_APPLICATION_NAME "APM Planner" -#define QGC_APPLICATION_VERSION "v2.0.17" +#define QGC_APPLICATION_VERSION "v2.0.18-rc2" #define APP_DATA_DIRECTORY "/apmplanner2" #define LOG_DIRECTORY "/dataflashLogs" #define PARAMETER_DIRECTORY "/parameters" @@ -50,7 +50,7 @@ namespace QGC { const QString APPNAME = "APMPLANNER2"; const QString COMPANYNAME = "DIYDRONES"; -const int APPLICATIONVERSION = 2017; // 2.0.17 [TODO] we should deprecate this version definition +const int APPLICATIONVERSION = 2018; // 2.0.18 [TODO] we should deprecate this version definition inline void close(){ GlobalObject* global = GlobalObject::sharedInstance(); @@ -120,6 +120,14 @@ const int APPLICATIONVERSION = 2017; // 2.0.17 [TODO] we should deprecate this v return GlobalObject::sharedInstance()->shareDirectory(); } + inline QRegExp paramSplitRegExp() { + return QRegExp("\t|,|="); + } + + inline QRegExp paramLineSplitRegExp() { + return QRegExp("\r|\n"); + } + } #endif // QGC_CONFIGURATION_H diff --git a/src/globalobject.cc b/src/globalobject.cc index 37373d34af..4b6f4678b3 100644 --- a/src/globalobject.cc +++ b/src/globalobject.cc @@ -77,7 +77,6 @@ QString GlobalObject::defaultAppDataDirectory() { QString homeDir = QStandardPaths::writableLocation(QStandardPaths::HomeLocation); QString appHomeDir = homeDir + APP_DATA_DIRECTORY; - makeDirectory(appHomeDir); return appHomeDir; } @@ -101,7 +100,6 @@ QString GlobalObject::defaultLogDirectory() { QString homeDir = QStandardPaths::writableLocation(QStandardPaths::HomeLocation); QString logHomeDir = homeDir + APP_DATA_DIRECTORY + LOG_DIRECTORY; - makeDirectory(logHomeDir); return logHomeDir; } @@ -125,7 +123,6 @@ QString GlobalObject::defaultMAVLinkLogDirectory() { QString homeDir = QStandardPaths::writableLocation(QStandardPaths::HomeLocation); QString logHomeDir = homeDir + APP_DATA_DIRECTORY + MAVLINK_LOG_DIRECTORY; - makeDirectory(logHomeDir); return logHomeDir; } @@ -149,7 +146,7 @@ QString GlobalObject::defaultParameterDirectory() { QString homeDir = QStandardPaths::writableLocation(QStandardPaths::HomeLocation); QString paramHomeDir = homeDir + APP_DATA_DIRECTORY + PARAMETER_DIRECTORY; - makeDirectory(paramHomeDir); return paramHomeDir; + return paramHomeDir; } QString GlobalObject::parameterDirectory() @@ -172,7 +169,6 @@ QString GlobalObject::defaultMissionDirectory() { QString homeDir = QStandardPaths::writableLocation(QStandardPaths::HomeLocation); QString missionDir = homeDir + APP_DATA_DIRECTORY + MISSION_DIRECTORY; - makeDirectory(missionDir); return missionDir; } diff --git a/src/input/JoystickInput.cc b/src/input/JoystickInput.cc index 484bd4b235..c682ee02f0 100644 --- a/src/input/JoystickInput.cc +++ b/src/input/JoystickInput.cc @@ -54,6 +54,7 @@ JoystickInput::JoystickInput() : hatValue(sdlJoystickMax*2), valuesTicks(0) { +// init(); loadSettings(); } @@ -63,12 +64,25 @@ JoystickInput::~JoystickInput() done.store(1); } +const QString JoystickInput::getActiveJoystickId() +{ + char joystickId[64]; + + SDL_JoystickGetGUIDString(SDL_JoystickGetGUID(joystick), joystickId, sizeof(joystickId)); + return QString::fromStdString(joystickId); +} + +const int JoystickInput::getNumberOfButtons() const +{ + return SDL_JoystickNumButtons(joystick); +} + void JoystickInput::loadSettings() { // Load defaults from settings QSettings settings; settings.sync(); - settings.beginGroup("QGC_JOYSTICK_INPUT"); + settings.beginGroup("QGC_JOYSTICK_INPUT_" + getActiveJoystickId()); xAxis = (settings.value("X_AXIS_MAPPING", xAxis).toInt()); xReversed = (settings.value("X_REVERSED", xReversed).toBool()); yAxis = (settings.value("Y_AXIS_MAPPING", yAxis).toInt()); @@ -82,11 +96,11 @@ void JoystickInput::loadSettings() settings.endGroup(); } -void JoystickInput::storeSettings() const +void JoystickInput::storeSettings() { // Store settings QSettings settings; - settings.beginGroup("QGC_JOYSTICK_INPUT"); + settings.beginGroup("QGC_JOYSTICK_INPUT_" + getActiveJoystickId()); settings.setValue("X_AXIS_MAPPING", xAxis); settings.setValue("X_REVERSED", xReversed); settings.setValue("Y_AXIS_MAPPING", yAxis); @@ -202,6 +216,7 @@ void JoystickInput::init() joystickName = QString(SDL_JoystickName(joystick)); QLOG_INFO() << "Opened" << joystickName; + loadSettings(); emit joystickSelected(joystickName); #if 0 diff --git a/src/input/JoystickInput.h b/src/input/JoystickInput.h index d49849a0e5..bbf3842721 100644 --- a/src/input/JoystickInput.h +++ b/src/input/JoystickInput.h @@ -59,6 +59,10 @@ class JoystickInput : public QThread const QString& getName() const; + const QString getActiveJoystickId(); + + const int getNumberOfButtons() const; + /** * @brief Load joystick settings */ @@ -67,7 +71,7 @@ class JoystickInput : public QThread /** * @brief Store joystick settings */ - void storeSettings() const; + void storeSettings(); int getMappingThrustAxis() const { diff --git a/src/uas/ArduPilotMegaMAV.cc b/src/uas/ArduPilotMegaMAV.cc index 08dca5463e..a5a5ee6e7c 100644 --- a/src/uas/ArduPilotMegaMAV.cc +++ b/src/uas/ArduPilotMegaMAV.cc @@ -151,7 +151,7 @@ QString ApmPlane::stringForMode(int aMode) case RESERVED_14: return "Reserved"; default: - return "Undefined: " + QString::number(aMode); + return QString().sprintf("Mode (%d)", aMode); } } @@ -191,7 +191,7 @@ QString ApmCopter::stringForMode(int aMode) { return "Circle"; break; case POSITION: - return "Reserved"; // Marked as reserved as not supported since AC3.2 + return QString().sprintf("Position (%d)", aMode); break; case LAND: return "Land"; @@ -218,7 +218,7 @@ QString ApmCopter::stringForMode(int aMode) { return "Flip"; break; default: - return "Undefined"; + return QString().sprintf("Mode (%d)", aMode); } } @@ -266,9 +266,8 @@ QString ApmRover::stringForMode(int aMode) { case RESERVED_12: case RESERVED_13: case RESERVED_14: - return "Reserved"; default: - return "Undefined"; + return QString().sprintf("Mode (%d)", aMode); } } diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 9026344407..4d1830f8ed 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -2670,9 +2670,12 @@ void UAS::requestParameter(int component, const QString& parameter) if (parameter.length() > MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN) { emit textMessageReceived(uasId, 0, 255, QString("QGC WARNING: Parameter name %1 is more than %2 bytes long. This might lead to errors and mishaps!").arg(parameter).arg(MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN-1)); + return; } memcpy(read.param_id, parameter.toStdString().c_str(), qMax(parameter.length(), MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN)); - read.param_id[15] = '\0'; // Enforce null termination + if (parameter.length() < MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN){ + read.param_id[parameter.length()] = '\0'; // Enforce null termination + } read.target_system = uasId; read.target_component = component; mavlink_msg_param_request_read_encode(systemId, componentId, &msg, &read); diff --git a/src/uas/UAS.h b/src/uas/UAS.h index e4b19dc929..0d16b9d442 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -257,12 +257,16 @@ class UAS : public UASInterface QString getGpsFixString() { switch(m_gps_fix){ + case 5: + return "3D+RTK"; + case 4: + return "3D+DGPS"; case 3: return "3D"; case 2: return "2D"; case 1: - return "1"; + return "NO FIX"; default: return "."; } diff --git a/src/ui/AP2DataPlot2D.cpp b/src/ui/AP2DataPlot2D.cpp index 37320197d8..fa505700b1 100644 --- a/src/ui/AP2DataPlot2D.cpp +++ b/src/ui/AP2DataPlot2D.cpp @@ -391,11 +391,13 @@ void AP2DataPlot2D::plotMouseMove(QMouseEvent *evt) } else if (graph->data()->contains(key)) { - newresult.append(m_graphClassMap.keys()[i] + ": " + QString::number(graph->data()->value(key).value,'f',4) + ((i == m_graphClassMap.keys().size()-1) ? "" : "\n")); + QString str = QString().sprintf( "%.4g", graph->data()->value(key).value); + newresult.append(m_graphClassMap.keys()[i] + ": " + str + ((i == m_graphClassMap.keys().size()-1) ? "" : "\n")); } else if (graph->data()->lowerBound(key) != graph->data()->constEnd()) { - newresult.append(m_graphClassMap.keys()[i] + ": " + QString::number((graph->data()->lowerBound(key).value().value),'f',4) + ((i == m_graphClassMap.keys().size()-1) ? "" : "\n")); + QString str = QString().sprintf( "%.4g", graph->data()->lowerBound(key).value().value); + newresult.append(m_graphClassMap.keys()[i] + ": " + str + ((i == m_graphClassMap.keys().size()-1) ? "" : "\n")); } else { @@ -1262,21 +1264,27 @@ void AP2DataPlot2D::threadDone(int errors,MAV_TYPE type) //It's an integer! switch (type) { - case MAV_TYPE_QUADROTOR: + case MAV_TYPE_QUADROTOR: + case MAV_TYPE_HEXAROTOR: + case MAV_TYPE_OCTOROTOR: + case MAV_TYPE_HELICOPTER: + case MAV_TYPE_TRICOPTER: { mode = ApmCopter::stringForMode(modeint); } break; - case MAV_TYPE_FIXED_WING: + case MAV_TYPE_FIXED_WING: { mode = ApmPlane::stringForMode(modeint); } break; - case MAV_TYPE_GROUND_ROVER: + case MAV_TYPE_GROUND_ROVER: { mode = ApmRover::stringForMode(modeint); } break; + default: + mode = QString().sprintf("Mode (%d)", modeint); } } QLOG_DEBUG() << "Mode change at index" << index << "to" << mode; diff --git a/src/ui/AP2DataPlot2DModel.cc b/src/ui/AP2DataPlot2DModel.cc index dda966d74f..3ba866a846 100644 --- a/src/ui/AP2DataPlot2DModel.cc +++ b/src/ui/AP2DataPlot2DModel.cc @@ -35,6 +35,7 @@ This file is part of the APM_PLANNER project #include #include #include +#include /* * This model holds everything in memory in a sqlite database. * There are two system tables, then unlimited number of message tables. @@ -269,34 +270,72 @@ QMap AP2DataPlot2DModel::getModeValues() { //No mode? QLOG_DEBUG() << "Graph loaded with no mode table. Running anyway, but text modes will not be available"; + modequery.prepare("SELECT * FROM 'HEARTBEAT';"); + if (!modequery.exec()) + { + QLOG_DEBUG() << "Graph loaded with no heartbeat either. No modes available"; + } } - else + QString lastmode = ""; + MAV_TYPE foundtype = MAV_TYPE_GENERIC; + bool custom_mode = false; + + while (modequery.next()) { - while (modequery.next()) + QSqlRecord record = modequery.record(); + quint64 index = record.value(0).toLongLong(); + QString mode = ""; + if (record.contains("Mode")) { - QSqlRecord record = modequery.record(); - quint64 index = record.value(0).toLongLong(); - QString mode = ""; - if (record.contains("Mode")) + mode = record.value("Mode").toString(); + } + else if (record.contains("custom_mode")) + { + custom_mode = true; + int modeint = record.value("custom_mode").toString().toInt(); + if (foundtype == MAV_TYPE_GENERIC) { - mode = record.value("Mode").toString(); + int type = record.value("type").toString().toInt(); + foundtype = static_cast(type); } - bool ok = false; - int modeint = mode.toInt(&ok); - if (!ok) + if (foundtype == MAV_TYPE_FIXED_WING) { - if (record.contains("ModeNum")) - { - mode = record.value("ModeNum").toString(); - } - else - { - QLOG_DEBUG() << "Unable to determine Mode number in log" << record.value("Mode").toString(); - mode = record.value("Mode").toString(); - } + mode = ApmPlane::stringForMode(modeint); + } + else if (foundtype == MAV_TYPE_QUADROTOR || foundtype == MAV_TYPE_COAXIAL || foundtype == MAV_TYPE_HELICOPTER || \ + foundtype == MAV_TYPE_HEXAROTOR || foundtype == MAV_TYPE_OCTOROTOR || foundtype == MAV_TYPE_TRICOPTER) + { + mode = ApmCopter::stringForMode(modeint); + } + else if (foundtype == MAV_TYPE_GROUND_ROVER) + { + mode = ApmRover::stringForMode(modeint); + } + else + { + mode = QString::number(static_cast(modeint)); } + } + bool ok = false; + int modeint = mode.toInt(&ok); + if (!ok && !custom_mode) + { + if (record.contains("ModeNum")) + { + mode = record.value("ModeNum").toString(); + } + else + { + QLOG_DEBUG() << "Unable to determine Mode number in log" << record.value("Mode").toString(); + mode = record.value("Mode").toString(); + } + } + if (lastmode != mode) + { retval.insert(index,mode); + lastmode = mode; } + } return retval; } @@ -684,9 +723,17 @@ QString AP2DataPlot2DModel::makeCreateTableString(QString tablename, QString for { mktable.append("," + name + " integer"); } + else if (typeCode == 'q') //int64_t + { + mktable.append("," + name + " integer"); + } + else if (typeCode == 'Q') //uint64_t + { + mktable.append("," + name + " integer"); + } else { - //QLOG_DEBUG() << "AP2DataPlotThread::makeCreateTableString(): NEW UNKNOWN VALUE" << typeCode; + QLOG_DEBUG() << "AP2DataPlotThread::makeCreateTableString(): NEW UNKNOWN VALUE" << typeCode; } } mktable.append(");"); diff --git a/src/ui/AP2DataPlotThread.cc b/src/ui/AP2DataPlotThread.cc index b75f51b417..9bdff58c21 100644 --- a/src/ui/AP2DataPlotThread.cc +++ b/src/ui/AP2DataPlotThread.cc @@ -69,15 +69,8 @@ bool AP2DataPlotThread::isMainThread() return QThread::currentThread() == QCoreApplication::instance()->thread(); } -void AP2DataPlotThread::loadBinaryLog() +void AP2DataPlotThread::loadBinaryLog(QFile &logfile) { - QFile logfile(m_fileName); - if (!logfile.open(QIODevice::ReadOnly)) - { - emit error("Unable to open log file"); - return; - } - QByteArray block; int paramtype = -1; QMap typeToLengthMap; @@ -205,7 +198,7 @@ void AP2DataPlotThread::loadBinaryLog() } else if (typeCode == 'h') //int16_t { - quint16 val; + qint16 val; packetstream >> val; linetoemit += "," + QString::number(val,'f',0); valuepairlist.append(QPair(labelstrsplit.at(j),val)); @@ -326,6 +319,20 @@ void AP2DataPlotThread::loadBinaryLog() linetoemit += "," + QString::number(val,'f',0); valuepairlist.append(QPair(labelstrsplit.at(j),val)); } + else if (typeCode == 'q') + { + qint64 val; + packetstream >> val; + linetoemit += "," + QString::number(val,'f',0); + valuepairlist.append(QPair(labelstrsplit.at(j),val)); + } + else if (typeCode == 'Q') + { + quint64 val; + packetstream >> val; + linetoemit += "," + QString::number(val,'f',0); + valuepairlist.append(QPair(labelstrsplit.at(j),val)); + } else { //Unknown! @@ -384,16 +391,8 @@ void AP2DataPlotThread::loadBinaryLog() return; } } -void AP2DataPlotThread::loadAsciiLog() +void AP2DataPlotThread::loadAsciiLog(QFile &logfile) { - - QFile logfile(m_fileName); - if (!logfile.open(QIODevice::ReadOnly)) - { - emit error("Unable to open log file"); - return; - } - m_loadedLogType = MAV_TYPE_GENERIC; int index = 500; QMap nameToTypeString; @@ -574,15 +573,8 @@ void AP2DataPlotThread::loadAsciiLog() return; } } -void AP2DataPlotThread::loadTLog() +void AP2DataPlotThread::loadTLog(QFile &logfile) { - QFile logfile(m_fileName); - if (!logfile.open(QIODevice::ReadOnly)) - { - emit error("Unable to open log file"); - return; - } - m_loadedLogType = MAV_TYPE_GENERIC; int bytesize = 0; @@ -770,20 +762,31 @@ void AP2DataPlotThread::run() emit startLoad(); m_stop = false; m_errorCount = 0; + qint64 msecs = QDateTime::currentMSecsSinceEpoch(); + + QFile logfile(m_fileName); + if (!logfile.open(QIODevice::ReadOnly)) + { + emit error("Unable to open log file (" + m_fileName + ")"); + return; + } + + QLOG_DEBUG() << "AP2DataPlotThread::run(): Log loading start -" << logfile.size() << "bytes"; + if (m_fileName.toLower().endsWith(".bin")) { //It's a binary file - loadBinaryLog(); + loadBinaryLog(logfile); } else if (m_fileName.toLower().endsWith(".log")) { //It's a ascii log. - loadAsciiLog(); + loadAsciiLog(logfile); } else if (m_fileName.toLower().endsWith(".tlog")) { //It's a tlog - loadTLog(); + loadTLog(logfile); } else { @@ -792,23 +795,14 @@ void AP2DataPlotThread::run() } - qint64 msecs = QDateTime::currentMSecsSinceEpoch(); - QFile logfile(m_fileName); - if (!logfile.open(QIODevice::ReadOnly)) - { - emit error("Unable to open log file"); - return; - } - - QLOG_DEBUG() << "AP2DataPlotThread::run(): Log loading finished, pos:" << logfile.pos() << "filesize:" << logfile.size(); if (m_stop) { - QLOG_ERROR() << "Plot Log loading was canceled after" << (QDateTime::currentMSecsSinceEpoch() - msecs) / 1000.0 << "seconds"; + QLOG_ERROR() << "Plot Log loading was canceled after" << (QDateTime::currentMSecsSinceEpoch() - msecs) / 1000.0 << "seconds -" << logfile.pos() << "of" << logfile.size() << "bytes"; emit error("Log loading Canceled"); } else { - QLOG_INFO() << "Plot Log loading took" << (QDateTime::currentMSecsSinceEpoch() - msecs) / 1000.0 << "seconds"; + QLOG_INFO() << "Plot Log loading took" << (QDateTime::currentMSecsSinceEpoch() - msecs) / 1000.0 << "seconds -" << logfile.pos() << "of" << logfile.size() << "bytes used"; emit done(m_errorCount,m_loadedLogType); } } diff --git a/src/ui/AP2DataPlotThread.h b/src/ui/AP2DataPlotThread.h index 26860585bb..f353562a94 100644 --- a/src/ui/AP2DataPlotThread.h +++ b/src/ui/AP2DataPlotThread.h @@ -60,9 +60,9 @@ class AP2DataPlotThread : public QThread bool isMainThread(); void loadDataFieldsFromValues(); - void loadBinaryLog(); - void loadAsciiLog(); - void loadTLog(); + void loadBinaryLog(QFile &logfile); + void loadAsciiLog(QFile &logfile); + void loadTLog(QFile &logfile); private: QString m_fileName; diff --git a/src/ui/ApmToolBar.cc b/src/ui/ApmToolBar.cc index ec0f07fd60..a773ab224b 100644 --- a/src/ui/ApmToolBar.cc +++ b/src/ui/ApmToolBar.cc @@ -54,6 +54,16 @@ APMToolBar::APMToolBar(QWindow *parent): engine()->addImportPath(QGC::shareDirectory() +"/qml"); //For installed linux builds setSource(url); QLOG_DEBUG() << "QML Status:" << status(); + if (status() == QQuickView::Error) + { + QString errorstr = ""; + for (int i=0;irootContext()->setContextProperty("globalObj", this); @@ -120,8 +130,6 @@ void APMToolBar::activeUasSet(UASInterface *uas) // [TODO} Add active MAV to diplay here m_uas = uas; - - connect(m_uas,SIGNAL(armingChanged(bool)), this,SLOT(armingChanged(bool))); connect(m_uas,SIGNAL(armingChanged(int, QString)), @@ -161,14 +169,10 @@ void APMToolBar::activeUasSet(UASInterface *uas) { if (LinkManager::instance()->getLinkConnected(linkidlist.at(i))) { - //This link is connected - if (LinkManager::instance()->getLinkType(linkidlist.at(i)) == LinkInterface::SERIAL_LINK) - { - //We only want to attach the toolbar to a serial link. - m_currentLinkId = linkidlist.at(i); - updateLinkDisplay(m_currentLinkId); - break; - } + // Show only the first actve link for a UAS + m_currentLinkId = linkidlist.at(i); + updateLinkDisplay(m_currentLinkId); + break; } } } @@ -302,19 +306,23 @@ void APMToolBar::updateLinkDisplay(int linkid) QLOG_TRACE() << "APMToolBar::updateLinkDisplay called with non current link. Current:" << m_currentLinkId << "called:" << linkid; return; } - QString port = LinkManager::instance()->getSerialLinkPort(linkid); - int baud = LinkManager::instance()->getSerialLinkBaud(linkid); - bool connected = LinkManager::instance()->getLinkConnected(linkid); + QObject *object = rootObject(); if (!object) { - QLOG_FATAL() << "APMToolBar::updateLinkDisplay, null QML root object. FIXME"; + QLOG_FATAL() << "APMToolBar::updateLinkDisplay, null QML root object."; return; } - object->setProperty("baudrateLabel", QString::number(baud)); - object->setProperty("linkNameLabel", port); + LinkManager* lm = LinkManager::instance(); + QString linkName = lm->getLinkShortName(linkid); + QString linkDetail = lm->getLinkDetail(linkid); + + object->setProperty("linkDetailLabel", linkDetail); + object->setProperty("linkNameLabel", linkName); + + bool connected = LinkManager::instance()->getLinkConnected(linkid); setConnection(connected); - QLOG_DEBUG() << "APMToolBar: updateLinkDisplay" << port << baud << connected; + QLOG_DEBUG() << "APMToolBar: updateLinkDisplay" << linkName <<":"<< linkDetail <<" connected:" << connected; } void APMToolBar::newLinkCreated(int linkid) diff --git a/src/ui/CommConfigurationWindow.cc b/src/ui/CommConfigurationWindow.cc index f3726c6990..cb578dadd7 100644 --- a/src/ui/CommConfigurationWindow.cc +++ b/src/ui/CommConfigurationWindow.cc @@ -48,6 +48,7 @@ This file is part of the QGROUNDCONTROL project #include "MAVLinkProtocol.h" #include "MAVLinkSettingsWidget.h" #include "QGCUDPLinkConfiguration.h" +#include "QGCUDPClientLinkConfiguration.h" #include "QGCTCPLinkConfiguration.h" #include "LinkManager.h" #include "MainWindow.h" @@ -87,6 +88,7 @@ CommConfigurationWindow::CommConfigurationWindow(int linkid, ProtocolInterface* // add link types ui.linkType->addItem(tr("Serial"), QGC_LINK_SERIAL); ui.linkType->addItem(tr("UDP"), QGC_LINK_UDP); + ui.linkType->addItem(tr("UDP Client"), QGC_LINK_UDP_CLIENT); ui.linkType->addItem(tr("TCP"), QGC_LINK_TCP); // ui.linkType->addItem(tr("Simulation"), QGC_LINK_SIMULATION); // [TODO] left as key where to add simulation mode @@ -145,6 +147,13 @@ CommConfigurationWindow::CommConfigurationWindow(int linkid, ProtocolInterface* ui.linkGroupBox->setTitle(tr("TCP Link")); ui.linkType->setCurrentIndex(ui.linkType->findData(QGC_LINK_TCP)); } + else if (LinkManager::instance()->getLinkType(linkid) == LinkInterface::UDP_CLIENT_LINK) + { + QWidget* conf = new QGCUDPClientLinkConfiguration(linkid, this); + ui.linkScrollArea->setWidget(conf); + ui.linkGroupBox->setTitle(tr("UDP Client Link")); + ui.linkType->setCurrentIndex(ui.linkType->findData(QGC_LINK_UDP_CLIENT)); + } // Display the widget this->window()->setWindowTitle(tr("Settings for ") + LinkManager::instance()->getLinkName(linkid)); @@ -171,9 +180,8 @@ void CommConfigurationWindow::linkUpdate(int linkid) return; } - if (LinkManager::instance()->getLinkType(linkid) == LinkInterface::TCP_LINK - && LinkManager::instance()->isTcpServer(linkid)) - { + TCPLink* tcpLink = dynamic_cast(LinkManager::instance()->getLink(linkid)); + if (tcpLink) { this->window()->close(); } diff --git a/src/ui/CommConfigurationWindow.h b/src/ui/CommConfigurationWindow.h index b638b232c7..b274868422 100644 --- a/src/ui/CommConfigurationWindow.h +++ b/src/ui/CommConfigurationWindow.h @@ -43,6 +43,7 @@ This file is part of the QGROUNDCONTROL project enum qgc_link_t { QGC_LINK_SERIAL, QGC_LINK_UDP, + QGC_LINK_UDP_CLIENT, QGC_LINK_TCP, QGC_LINK_SIMULATION, QGC_LINK_FORWARDING, diff --git a/src/ui/JoystickWidget.cc b/src/ui/JoystickWidget.cc index e41ce9a4d4..75f70f032b 100644 --- a/src/ui/JoystickWidget.cc +++ b/src/ui/JoystickWidget.cc @@ -6,33 +6,28 @@ #include "UASManager.h" + JoystickWidget::JoystickWidget(JoystickInput* joystick, QWidget *parent) : QDialog(parent), - m_ui(new Ui::JoystickWidget) + m_ui(new Ui::JoystickWidget), + m_buttonPressedMessage("") { m_ui->setupUi(this); - clearKeys(); - this->joystick = joystick; - m_ui->rollMapSpinBox->setValue(joystick->getMappingXAxis()); - m_ui->pitchMapSpinBox->setValue(joystick->getMappingYAxis()); - m_ui->yawMapSpinBox->setValue(joystick->getMappingYawAxis()); - m_ui->throttleMapSpinBox->setValue(joystick->getMappingThrustAxis()); + for (int i = 0; i < joystick->getNumberOfButtons(); ++i) { + addJoystickButtonLabel(i); + } - m_ui->rollInvertCheckBox->setChecked(joystick->getXReversed()); - m_ui->pitchInvertCheckBox->setChecked(joystick->getYReversed()); - m_ui->yawInvertCheckBox->setChecked(joystick->getYawReversed()); - m_ui->throttleInvertCheckBox->setChecked(joystick->getThrustReversed()); + clearKeys(); + this->joystick = joystick; - m_ui->autoMapSpinBox->setValue(joystick->getMappingAutoButton()); - m_ui->stabilizeMapSpinBox->setValue(joystick->getMappingStabilizeButton()); + updateMappings(); connect(this->joystick, SIGNAL(joystickChanged(double,double,double,double,int,int,int)), this, SLOT(updateJoystick(double,double,double,double,int,int,int))); connect(this->joystick, SIGNAL(xChanged(double)), this, SLOT(setX(double))); connect(this->joystick, SIGNAL(yChanged(double)), this, SLOT(setY(double))); connect(this->joystick, SIGNAL(yawChanged(double)), this, SLOT(setZ(double))); connect(this->joystick, SIGNAL(thrustChanged(double)), this, SLOT(setThrottle(double))); - connect(this->joystick, SIGNAL(buttonPressed(int)), this, SLOT(pressKey(int))); connect(this->joystick, SIGNAL(hatDirectionChanged(int, int)), this, SLOT(setHat(int, int))); connect(m_ui->rollMapSpinBox, SIGNAL(valueChanged(int)), this->joystick, SLOT(setMappingXAxis(int))); @@ -48,6 +43,9 @@ JoystickWidget::JoystickWidget(JoystickInput* joystick, QWidget *parent) : connect(m_ui->autoMapSpinBox, SIGNAL(valueChanged(int)), this->joystick, SLOT(setMappingAutoButton(int))); connect(m_ui->stabilizeMapSpinBox, SIGNAL(valueChanged(int)), this->joystick, SLOT(setMappingStabilizeButton(int))); + connect(this, SIGNAL(accepted()), this, SLOT(storeMapping())); + connect(this, SIGNAL(rejected()), this, SLOT(restoreMapping())); + if (joystick) { connect(joystick, SIGNAL(joystickSelected(const QString&)), @@ -76,13 +74,35 @@ JoystickWidget::~JoystickWidget() delete m_ui; } +void JoystickWidget::addJoystickButtonLabel(int i) +{ + QLabel* button = new QLabel(m_ui->groupBox); + button->setObjectName("button" + QString::number(i)); + button->setEnabled(true); + QSizePolicy sizePolicy(QSizePolicy::Minimum, QSizePolicy::Minimum); + sizePolicy.setHorizontalStretch(0); + sizePolicy.setVerticalStretch(0); + sizePolicy.setHeightForWidth(button->sizePolicy().hasHeightForWidth()); + button->setSizePolicy(sizePolicy); + button->setAlignment(Qt::AlignCenter); + button->setText(QString::number(i)); + m_ui->verticalLayout->addWidget(button); + m_buttonList.append(button); + m_buttonStates[i] = false; +} + void JoystickWidget::joystickSelected(const QString& name) { - m_ui->joystickButton->setText(name); - m_ui->joystickButton->setFlat(false); - m_ui->joystickButton->setCheckable(true); - m_ui->joystickButton->setChecked(false); + m_ui->joystickLabel->setText(name); + m_ui->joystickButton->setEnabled(true); joystick->setActiveUAS(NULL); + updateMappings(); + + m_buttonList.clear(); + for (int i = 0; i < joystick->getNumberOfButtons(); ++i) { + addJoystickButtonLabel(i); + } + clearKeys(); } void JoystickWidget::joystickEnabled(bool checked) @@ -90,11 +110,13 @@ void JoystickWidget::joystickEnabled(bool checked) if (checked) { QLOG_INFO() << "Enabling joystick"; + m_ui->joystickButton->setText("Deactivate"); joystick->setActiveUAS(UASManager::instance()->getActiveUAS()); } else { QLOG_INFO() << "Disabling joystick"; + m_ui->joystickButton->setText("Activate"); joystick->setActiveUAS(NULL); } } @@ -105,6 +127,33 @@ void JoystickWidget::activeUASSet(UASInterface* uas) joystick->setActiveUAS(uas); } +void JoystickWidget::updateMappings() +{ + m_ui->rollMapSpinBox->setValue(joystick->getMappingXAxis()); + m_ui->pitchMapSpinBox->setValue(joystick->getMappingYAxis()); + m_ui->yawMapSpinBox->setValue(joystick->getMappingYawAxis()); + m_ui->throttleMapSpinBox->setValue(joystick->getMappingThrustAxis()); + + m_ui->rollInvertCheckBox->setChecked(joystick->getXReversed()); + m_ui->pitchInvertCheckBox->setChecked(joystick->getYReversed()); + m_ui->yawInvertCheckBox->setChecked(joystick->getYawReversed()); + m_ui->throttleInvertCheckBox->setChecked(joystick->getThrustReversed()); + + m_ui->autoMapSpinBox->setValue(joystick->getMappingAutoButton()); + m_ui->stabilizeMapSpinBox->setValue(joystick->getMappingStabilizeButton()); +} + +void JoystickWidget::storeMapping() +{ + joystick->storeSettings(); +} + +void JoystickWidget::restoreMapping() +{ + joystick->loadSettings(); + updateMappings(); +} + void JoystickWidget::updateJoystick(double roll, double pitch, double yaw, double thrust, int xHat, int yHat, int buttons) { setX(pitch); @@ -113,10 +162,12 @@ void JoystickWidget::updateJoystick(double roll, double pitch, double yaw, doubl setThrottle(thrust); setHat(xHat, yHat); - for (int i = 0; i < 10; ++i) + for (int i = 0; i < m_buttonList.size(); ++i) { - if (buttons & (1<button0->setStyleSheet(colorstyle); - m_ui->button1->setStyleSheet(colorstyle); - m_ui->button2->setStyleSheet(colorstyle); - m_ui->button3->setStyleSheet(colorstyle); - m_ui->button4->setStyleSheet(colorstyle); - m_ui->button5->setStyleSheet(colorstyle); - m_ui->button6->setStyleSheet(colorstyle); - m_ui->button7->setStyleSheet(colorstyle); - m_ui->button8->setStyleSheet(colorstyle); - m_ui->button9->setStyleSheet(colorstyle); - m_ui->button10->setStyleSheet(colorstyle); + for (int i = 0; i < m_buttonList.size(); ++i) { + m_buttonList[i]->setStyleSheet(colorstyle); + } } -void JoystickWidget::pressKey(int key) +void JoystickWidget::buttonStateChanged(const int key, const bool pressed) { QString colorstyle; - QColor buttonStyleColor = QColor(20, 200, 20); + QColor buttonStyleColor = pressed ? QColor(20, 200, 20) : QColor(200, 20, 20); colorstyle = QString("QLabel { border: 1px solid #EEEEEE; border-radius: 4px; padding: 0px; margin: 0px; background-color: %1;}").arg(buttonStyleColor.name()); - switch(key) { - case 0: - m_ui->button0->setStyleSheet(colorstyle); - break; - case 1: - m_ui->button1->setStyleSheet(colorstyle); - break; - case 2: - m_ui->button2->setStyleSheet(colorstyle); - break; - case 3: - m_ui->button3->setStyleSheet(colorstyle); - break; - case 4: - m_ui->button4->setStyleSheet(colorstyle); - break; - case 5: - m_ui->button5->setStyleSheet(colorstyle); - break; - case 6: - m_ui->button6->setStyleSheet(colorstyle); - break; - case 7: - m_ui->button7->setStyleSheet(colorstyle); - break; - case 8: - m_ui->button8->setStyleSheet(colorstyle); - break; - case 9: - m_ui->button9->setStyleSheet(colorstyle); - break; - case 10: - m_ui->button10->setStyleSheet(colorstyle); - break; + + if (key < m_buttonList.size()) { + m_buttonList[key]->setStyleSheet(colorstyle); + } + + if (pressed) { + m_buttonPressedMessage = tr("Key %1 pressed").arg(key); + } else { + m_buttonPressedMessage = ""; } - QTimer::singleShot(100, this, SLOT(clearKeys())); - updateStatus(tr("Key %1 pressed").arg(key)); } void JoystickWidget::updateStatus(const QString& status) diff --git a/src/ui/JoystickWidget.h b/src/ui/JoystickWidget.h index 9963d51ee1..14451f6511 100644 --- a/src/ui/JoystickWidget.h +++ b/src/ui/JoystickWidget.h @@ -32,6 +32,7 @@ This file is part of the PIXHAWK project #define JOYSTICKWIDGET_H #include +#include #include "JoystickInput.h" #include "UASInterface.h" @@ -75,7 +76,7 @@ public slots: /** @brief Clear keys */ void clearKeys(); /** @brief Joystick keys, as labeled on the joystick */ - void pressKey(int key); + void buttonStateChanged(const int key, const bool pressed); /** @brief Update status string */ void updateStatus(const QString& status); @@ -86,6 +87,11 @@ protected slots: void activeUASSet(UASInterface*); +private slots: + void storeMapping(); + + void restoreMapping(); + protected: /** @brief UI change event */ virtual void changeEvent(QEvent *e); @@ -93,6 +99,16 @@ protected slots: private: Ui::JoystickWidget *m_ui; JoystickInput* joystick; ///< Reference to the joystick + + int m_buttonStates[11]; + + QString m_buttonPressedMessage; + + QList m_buttonList; + + void addJoystickButtonLabel(int i); + + void updateMappings(); }; #endif // JOYSTICKWIDGET_H diff --git a/src/ui/JoystickWidget.ui b/src/ui/JoystickWidget.ui index 0acc51fa13..f40e8c13fe 100644 --- a/src/ui/JoystickWidget.ui +++ b/src/ui/JoystickWidget.ui @@ -74,215 +74,6 @@ 3 - - - - true - - - - 0 - 0 - - - - 0 - - - Qt::AlignCenter - - - - - - - true - - - - 0 - 0 - - - - 1 - - - Qt::AlignCenter - - - - - - - true - - - - 0 - 0 - - - - 2 - - - Qt::AlignCenter - - - - - - - true - - - - 0 - 0 - - - - 3 - - - Qt::AlignCenter - - - - - - - true - - - - 0 - 0 - - - - 4 - - - Qt::AlignCenter - - - - - - - true - - - - 0 - 0 - - - - 5 - - - Qt::AlignCenter - - - - - - - true - - - - 0 - 0 - - - - 6 - - - Qt::AlignCenter - - - - - - - true - - - - 0 - 0 - - - - 7 - - - Qt::AlignCenter - - - - - - - true - - - - 0 - 0 - - - - 8 - - - Qt::AlignCenter - - - - - - - true - - - - 0 - 0 - - - - 9 - - - Qt::AlignCenter - - - - - - - true - - - - 0 - 0 - - - - 10 - - - Qt::AlignCenter - - - @@ -478,7 +269,7 @@ - + @@ -487,13 +278,23 @@ - + No joystick detected yet.. waiting - + + + + + false + + Activate + + + true + false @@ -501,7 +302,7 @@ false - true + false diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index ca6f9a893d..e7c5d906a5 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -57,6 +57,7 @@ This file is part of the QGROUNDCONTROL project #include "TerminalConsole.h" #include "AP2DataPlot2D.h" #include "MissionElevationDisplay.h" +#include "LinkManagerFactory.h" #ifdef QGC_OSG_ENABLED #include "Q3DWidgetFactory.h" @@ -1610,9 +1611,11 @@ void MainWindow::connectCommonActions() ui.actionSerial->setData(LinkInterface::SERIAL_LINK); ui.actionTCP->setData(LinkInterface::TCP_LINK); ui.actionUDP->setData(LinkInterface::UDP_LINK); + ui.actionUDPClient->setData(LinkInterface::UDP_CLIENT_LINK); connect(ui.actionSerial,SIGNAL(triggered()),this,SLOT(addLink())); connect(ui.actionTCP,SIGNAL(triggered()),this,SLOT(addLink())); connect(ui.actionUDP,SIGNAL(triggered()),this,SLOT(addLink())); + connect(ui.actionUDPClient,SIGNAL(triggered()),this,SLOT(addLink())); connect(ui.actionAdvanced_Mode,SIGNAL(triggered(bool)),this,SLOT(setAdvancedMode(bool))); // Connect internal actions @@ -1784,15 +1787,19 @@ void MainWindow::addLink() int newid = 0; if (send->data() == LinkInterface::SERIAL_LINK) { - newid = LinkManager::instance()->addSerialConnection(); + newid = LinkManagerFactory::addSerialConnection(); } else if (send->data() == LinkInterface::TCP_LINK) { - newid = LinkManager::instance()->addTcpConnection(QHostAddress::LocalHost,5555,false); + newid = LinkManagerFactory::addTcpConnection(QHostAddress::LocalHost,5555,false); } else if (send->data() == LinkInterface::UDP_LINK) { - newid = LinkManager::instance()->addUdpConnection(QHostAddress::LocalHost,5555); + newid = LinkManagerFactory::addUdpConnection(QHostAddress::LocalHost,14550); + } + else if (send->data() == LinkInterface::UDP_CLIENT_LINK) + { + newid = LinkManagerFactory::addUdpClientConnection(QHostAddress("192.168.4.1"),14550); } addLink(newid); for (int i=0;iactions().size();i++) @@ -1831,7 +1838,11 @@ void MainWindow::addLink(int linkid) void MainWindow::linkError(int linkid,QString errorstring) { - QMessageBox::information(this,"Link Error",errorstring); + QWidget* parent = QApplication::activeWindow(); + if (!parent) { + parent = this; + } + QMessageBox::information(parent,"Link Error",errorstring); } void MainWindow::simulateLink(bool simulate) { diff --git a/src/ui/MainWindow.ui b/src/ui/MainWindow.ui index f72d3db7c4..318944602b 100644 --- a/src/ui/MainWindow.ui +++ b/src/ui/MainWindow.ui @@ -51,7 +51,7 @@ 0 0 1080 - 22 + 27 @@ -86,6 +86,7 @@ + @@ -588,6 +589,14 @@ Ctrl+Shift+M + + + UDP Client + + + UDP Client + + diff --git a/src/ui/MissionElevationDisplay.cpp b/src/ui/MissionElevationDisplay.cpp index 307d2876ad..e5cf5cf81a 100644 --- a/src/ui/MissionElevationDisplay.cpp +++ b/src/ui/MissionElevationDisplay.cpp @@ -178,21 +178,21 @@ void MissionElevationDisplay::updateDisplay() if (m_waypointList.count() == 0) return; - m_totalDistance = plotElevationGraph(ElevationGraphMissionId, m_homeAltOffset); + m_totalDistance = plotElevationGraph(m_waypointList.values(), ElevationGraphMissionId, m_homeAltOffset); addWaypointLabels(); } -void MissionElevationDisplay::updateElevationGraph(double averageResolution) +void MissionElevationDisplay::updateElevationGraph(QList waypointList, double averageResolution) { if (m_waypointList.count() == 0) return; - int distance = plotElevationGraph(ElevationGraphElevationId, 0.0); + int distance = plotElevationGraph(waypointList, ElevationGraphElevationId, 0.0); ui->resolutionLabel->setText(QString::number(averageResolution)+"(m)"); if (distance > m_totalDistance) m_totalDistance = distance; } -int MissionElevationDisplay::plotElevationGraph(int graphId, double homeAltOffset) +int MissionElevationDisplay::plotElevationGraph(QList waypointList, int graphId, double homeAltOffset) { Waypoint* previousWp = NULL; double totalDistance = 0.0; @@ -201,7 +201,7 @@ int MissionElevationDisplay::plotElevationGraph(int graphId, double homeAltOffse QCPGraph* graph = customplot->graph(graphId); graph->clearData(); - foreach(Waypoint* wp, m_waypointList){ + foreach(Waypoint* wp, waypointList){ double lower = 0.0, upper = 0.0; QCPRange xRange = customplot->xAxis->range(); QCPRange yRange = customplot->yAxis->range(); @@ -272,7 +272,7 @@ void MissionElevationDisplay::addWaypointLabels() customPlot->addItem(itemTracer); QCPItemText *itemText = new QCPItemText(customPlot); - itemText->setText(QString::number(wp->getId())); + itemText->setText("WP" + (QString::number(wp->getId())) + " (+" + (QString::number(distance,'f', 1)) +"m)"); itemText->position->setParentAnchor(itemTracer->position); double adjustedAlt = 0.0; diff --git a/src/ui/MissionElevationDisplay.h b/src/ui/MissionElevationDisplay.h index ba2c3b1bcb..0258e22a5f 100644 --- a/src/ui/MissionElevationDisplay.h +++ b/src/ui/MissionElevationDisplay.h @@ -52,14 +52,14 @@ private slots: void currentWaypointChanged(quint16 waypointId); void updateDisplay(); void updateElevationData(); - void updateElevationGraph(double averageResolution); + void updateElevationGraph(QList waypointList, double averageResolution); void setHomeAltOffset(); void useHomeAltOffset(bool state); void showInfoBox(); void sampleValueChanged(); private: - int plotElevationGraph(int graphId, double homeAltOffset); + int plotElevationGraph(QList waypointList, int graphId, double homeAltOffset); double distanceBetweenLatLng(double lat1, double lon1, double lat2, double lon2); double getHomeAlt(Waypoint* wp); void addWaypointLabels(); diff --git a/src/ui/QGCParamWidget.cc b/src/ui/QGCParamWidget.cc index 58ae3742e5..9159b97d16 100644 --- a/src/ui/QGCParamWidget.cc +++ b/src/ui/QGCParamWidget.cc @@ -172,7 +172,7 @@ QString QGCParamWidget::summaryInfoFromFile(const QString &filename) QString paramString = file.readAll(); file.close(); - QStringList paramSplit = paramString.split("\n"); + QStringList paramSplit = paramString.split(QGC::paramLineSplitRegExp()); foreach (QString paramLine, paramSplit) { if (paramLine.startsWith("#")) { @@ -206,9 +206,9 @@ bool QGCParamWidget::loadParamsFromFile(const QString &filename,ParamFileType ty QString line = paramfile.readLine(); if (!line.startsWith("#")) { - if (line.indexOf(",") != -1) + if (line.indexOf(QGC::paramSplitRegExp()) != -1) { - setParameter(1,line.split(",")[0],line.split(",")[1].toFloat()); + setParameter(1,line.split(QGC::paramSplitRegExp())[0],line.split(QGC::paramSplitRegExp())[1].toFloat()); } } } diff --git a/src/ui/QGCSettingsWidget.cc b/src/ui/QGCSettingsWidget.cc index 33a17e5db8..6f51ed0733 100644 --- a/src/ui/QGCSettingsWidget.cc +++ b/src/ui/QGCSettingsWidget.cc @@ -67,11 +67,13 @@ void QGCSettingsWidget::showEvent(QShowEvent *evt) ui->appDataDirEdit->setText((QGC::appDataDirectory())); ui->paramDirEdit->setText(QGC::parameterDirectory()); ui->mavlinkLogDirEdit->setText((QGC::MAVLinkLogDirectory())); + ui->missionsDirEdit->setText((QGC::missionDirectory())); connect(ui->logDirSetButton, SIGNAL(clicked()), this, SLOT(setLogDir())); connect(ui->appDirSetButton, SIGNAL(clicked()), this, SLOT(setAppDataDir())); connect(ui->paramDirSetButton, SIGNAL(clicked()), this, SLOT(setParamDir())); connect(ui->mavlinkDirSetButton, SIGNAL(clicked()), this, SLOT(setMAVLinkLogDir())); + connect(ui->missionsSetButton, SIGNAL(clicked()), this, SLOT(setMissionsDir())); // Style MainWindow::QGC_MAINWINDOW_STYLE style = (MainWindow::QGC_MAINWINDOW_STYLE)MainWindow::instance()->getStyle(); @@ -120,7 +122,7 @@ QGCSettingsWidget::~QGCSettingsWidget() void QGCSettingsWidget::setLogDir() { - QFileDialog dlg(this); + QFileDialog dlg(this, "Set log output directory"); dlg.setFileMode(QFileDialog::Directory); dlg.setDirectory(QGC::logDirectory()); @@ -134,7 +136,7 @@ void QGCSettingsWidget::setLogDir() void QGCSettingsWidget::setMAVLinkLogDir() { - QFileDialog dlg(this); + QFileDialog dlg(this, "Set tlog output directory"); dlg.setFileMode(QFileDialog::Directory); dlg.setDirectory(QGC::MAVLinkLogDirectory()); @@ -148,7 +150,7 @@ void QGCSettingsWidget::setMAVLinkLogDir() void QGCSettingsWidget::setParamDir() { - QFileDialog dlg(this); + QFileDialog dlg(this, "Set parameters directory"); dlg.setFileMode(QFileDialog::Directory); dlg.setDirectory(QGC::parameterDirectory()); @@ -162,7 +164,7 @@ void QGCSettingsWidget::setParamDir() void QGCSettingsWidget::setAppDataDir() { - QFileDialog dlg(this); + QFileDialog dlg(this, "Set application data directory"); dlg.setFileMode(QFileDialog::Directory); dlg.setDirectory(QGC::appDataDirectory()); @@ -174,6 +176,20 @@ void QGCSettingsWidget::setAppDataDir() } } +void QGCSettingsWidget::setMissionsDir() +{ + QFileDialog dlg(this, "Set missions directory"); + dlg.setFileMode(QFileDialog::Directory); + dlg.setDirectory(QGC::missionDirectory()); + + if(dlg.exec() == QDialog::Accepted) { + QDir dir = dlg.directory(); + QString name = dir.absolutePath(); + QGC::setMissionDirectory(name); + ui->missionsDirEdit->setText(name); + } +} + void QGCSettingsWidget::setActiveUAS(UASInterface *uas) { if (m_uas){ diff --git a/src/ui/QGCSettingsWidget.h b/src/ui/QGCSettingsWidget.h index 73af1468f5..d990e58ab4 100644 --- a/src/ui/QGCSettingsWidget.h +++ b/src/ui/QGCSettingsWidget.h @@ -23,6 +23,7 @@ private slots: void setMAVLinkLogDir(); void setParamDir(); void setAppDataDir(); + void setMissionsDir(); void ratesChanged(); void setBetaRelease(bool state); diff --git a/src/ui/QGCSettingsWidget.ui b/src/ui/QGCSettingsWidget.ui index be16258f39..7b99023434 100644 --- a/src/ui/QGCSettingsWidget.ui +++ b/src/ui/QGCSettingsWidget.ui @@ -27,75 +27,18 @@ General Settings - - - - 0 + + + + Qt::Horizontal - - - - Mute all audio output - - - - :/files/images/status/audio-volume-muted.svg:/files/images/status/audio-volume-muted.svg - - - - - - - Automatically reconnect last link on application startup - - - - :/files/images/devices/network-wireless.svg:/files/images/devices/network-wireless.svg - - - - - - - Lowers all update rates to save battery power - - - Enable low power mode - - - - - - - Show Docked Widget title bars when NOT in advanced Mode. - - - false - - - - - - - Enable GCS Heartbeat transmit (For GCS_Failsafe) - - - - - - - Enable Mavlink Logging (tlog) - - - - - - - Enable Beta Release Updates - - - - + + + 40 + 20 + + + @@ -137,89 +80,122 @@ - + File Locations - - - - - - - App Data - - - - - - - - - - Set... - - - - - - - Log output: - - - - - - - - - - Set... - - - - - - - tlog output: - - - - - - - - - - Set... - - - - - - - Parameters - - - - - - - - - - Set... - - - - + + + + + Log output: + + + + + + + tlog output: + + + + + + + App Data + + + + + + + Missions + + + + + + + false + + + + + + + false + + + + + + + Set... + + + + + + + false + + + + + + + Set... + + + + + + + false + + + + + + + Parameters + + + + + + + Set... + + + + + + + false + + + + + + + Set... + + + + + + + Set... + + - - + + Qt::Horizontal @@ -231,8 +207,8 @@ - - + + Qt::Horizontal @@ -244,21 +220,78 @@ - - - - Qt::Horizontal - - - - 40 - 20 - + + + + 0 - + + + + Mute all audio output + + + + :/files/images/status/audio-volume-muted.svg:/files/images/status/audio-volume-muted.svg + + + + + + + Automatically reconnect last link on application startup + + + + :/files/images/devices/network-wireless.svg:/files/images/devices/network-wireless.svg + + + + + + + Lowers all update rates to save battery power + + + Enable low power mode + + + + + + + Show Docked Widget title bars when NOT in advanced Mode. + + + false + + + + + + + Enable GCS Heartbeat transmit (For GCS_Failsafe) + + + + + + + Enable Mavlink Logging (tlog) + + + + + + + Enable Beta Release Updates + + + + - - + + Qt::Vertical @@ -270,8 +303,8 @@ - - + + Qt::Vertical @@ -284,14 +317,13 @@ - layoutWidget groupBox groupBox_2 horizontalSpacer horizontalSpacer_2 horizontalSpacer_3 - verticalSpacer verticalSpacer_2 + verticalSpacer diff --git a/src/ui/QGCTCPLinkConfiguration.cc b/src/ui/QGCTCPLinkConfiguration.cc index 3b8fe78d38..00a9ef44cd 100644 --- a/src/ui/QGCTCPLinkConfiguration.cc +++ b/src/ui/QGCTCPLinkConfiguration.cc @@ -11,18 +11,28 @@ QGCTCPLinkConfiguration::QGCTCPLinkConfiguration(int linkid, QWidget *parent) : { ui->setupUi(this); m_linkId = linkid; - uint16_t port = LinkManager::instance()->getTcpLinkPort(linkid); + uint16_t port = getTcpLink()->getPort(); ui->portSpinBox->setValue(port); - QString addr = LinkManager::instance()->getTcpLinkHost(linkid).toString(); + QString addr = getTcpLink()->getHostAddress().toString(); ui->hostAddressLineEdit->setText(addr); - ui->asServerCheckBox->setChecked(LinkManager::instance()->isTcpServer(linkid)); - connect(ui->portSpinBox,SIGNAL(valueChanged(int)),this,SLOT(valuesChanged())); - connect(ui->hostAddressLineEdit,SIGNAL(textChanged(QString)),this,SLOT(valuesChanged())); + ui->asServerCheckBox->setChecked(getTcpLink()->isServer()); + connect(ui->portSpinBox,SIGNAL(editingFinished()),this,SLOT(valuesChanged())); + connect(ui->hostAddressLineEdit,SIGNAL(editingFinished()),this,SLOT(valuesChanged())); connect(ui->asServerCheckBox,SIGNAL(stateChanged(int)),this,SLOT(valuesChanged())); } + void QGCTCPLinkConfiguration::valuesChanged() { - LinkManager::instance()->modifyTcpConnection(m_linkId,QHostAddress(ui->hostAddressLineEdit->text()),ui->portSpinBox->value(),ui->asServerCheckBox->isChecked()); + LinkManager *lm = LinkManager::instance(); + + TCPLink *iface = qobject_cast(lm->getLink(m_linkId)); + if (!iface) + { + return; + } + iface->setHostAddress(QHostAddress(ui->hostAddressLineEdit->text())); + iface->setPort(ui->portSpinBox->value()); + iface->setAsServer(ui->asServerCheckBox->isChecked()); } QGCTCPLinkConfiguration::~QGCTCPLinkConfiguration() @@ -41,3 +51,8 @@ void QGCTCPLinkConfiguration::changeEvent(QEvent *e) break; } } + +TCPLink* QGCTCPLinkConfiguration::getTcpLink() const +{ + return dynamic_cast(LinkManager::instance()->getLink(m_linkId)); +} diff --git a/src/ui/QGCTCPLinkConfiguration.h b/src/ui/QGCTCPLinkConfiguration.h index 980ac43c75..76c06c31f4 100644 --- a/src/ui/QGCTCPLinkConfiguration.h +++ b/src/ui/QGCTCPLinkConfiguration.h @@ -20,13 +20,15 @@ class QGCTCPLinkConfiguration : public QWidget public slots: void valuesChanged(); -protected: - void changeEvent(QEvent *e); - int m_linkId; +private: + void changeEvent(QEvent *e); + TCPLink* getTcpLink() const; private: Ui::QGCTCPLinkConfiguration *ui; + int m_linkId; + }; #endif // QGCTCPLINKCONFIGURATION_H diff --git a/src/ui/QGCUDPClientLinkConfiguration.cc b/src/ui/QGCUDPClientLinkConfiguration.cc new file mode 100644 index 0000000000..dc269c631a --- /dev/null +++ b/src/ui/QGCUDPClientLinkConfiguration.cc @@ -0,0 +1,60 @@ +#include + +#include "QGCUDPClientLinkConfiguration.h" +#include "ui_QGCUDPClientLinkConfiguration.h" +#include "LinkManager.h" +QGCUDPClientLinkConfiguration::QGCUDPClientLinkConfiguration(int linkid, QWidget *parent) : + QWidget(parent), + ui(new Ui::QGCUDPClientLinkConfiguration) +{ + m_linkId = linkid; + ui->setupUi(this); + ui->portSpinBox->setValue(getUdpClientLink()->getPort()); + ui->ipAddressLineEdit->setText("192.168.4.1"); + connect(LinkManager::instance(),SIGNAL(linkChanged(int)),this,SLOT(linkChanged(int))); + connect(ui->portSpinBox, SIGNAL(valueChanged(int)), this, SLOT(portValueChanged(int))); + connect(ui->ipAddressLineEdit, SIGNAL(textChanged(QString)), this, SLOT(hostValueChanged(QString))); +} + +QGCUDPClientLinkConfiguration::~QGCUDPClientLinkConfiguration() +{ + delete ui; +} + +void QGCUDPClientLinkConfiguration::changeEvent(QEvent *e) +{ + QWidget::changeEvent(e); + switch (e->type()) { + case QEvent::LanguageChange: + ui->retranslateUi(this); + break; + default: + break; + } +} +void QGCUDPClientLinkConfiguration::portValueChanged(int value) +{ + getUdpClientLink()->setPort(value); +} + +void QGCUDPClientLinkConfiguration::hostValueChanged(QString host) +{ + getUdpClientLink()->setAddress(QHostAddress(host)); +} + +void QGCUDPClientLinkConfiguration::linkChanged(int linkid) +{ + if (m_linkId != linkid) + { + return; + } + disconnect(ui->portSpinBox, SIGNAL(valueChanged(int)), this, SLOT(portValueChanged(int))); + disconnect(ui->ipAddressLineEdit, SIGNAL(editingFinished()), this, SLOT(portValueChanged(int))); + ui->portSpinBox->setValue(getUdpClientLink()->getPort()); + connect(ui->portSpinBox, SIGNAL(valueChanged(int)), this, SLOT(portValueChanged(int))); +} + +UDPClientLink* QGCUDPClientLinkConfiguration::getUdpClientLink() const +{ + return dynamic_cast(LinkManager::instance()->getLink(m_linkId)); +} diff --git a/src/ui/QGCUDPClientLinkConfiguration.h b/src/ui/QGCUDPClientLinkConfiguration.h new file mode 100644 index 0000000000..6de57541f2 --- /dev/null +++ b/src/ui/QGCUDPClientLinkConfiguration.h @@ -0,0 +1,35 @@ +#ifndef QGCUDPCLIENTLINKCONFIGURATION_H +#define QGCUDPCLIENTLINKCONFIGURATION_H + +#include + +#include "UDPClientLink.h" + +namespace Ui +{ +class QGCUDPClientLinkConfiguration; +} + +class QGCUDPClientLinkConfiguration : public QWidget +{ + Q_OBJECT + +public: + explicit QGCUDPClientLinkConfiguration(int linkid, QWidget *parent = 0); + ~QGCUDPClientLinkConfiguration(); + +public slots: + void portValueChanged(int value); + void hostValueChanged(QString host); + void linkChanged(int linkid); + +private: + void changeEvent(QEvent *e); + UDPClientLink* getUdpClientLink() const; + +private: + Ui::QGCUDPClientLinkConfiguration *ui; + int m_linkId; +}; + +#endif // QGCUDPCLIENTLINKCONFIGURATION_H diff --git a/src/ui/QGCUDPClientLinkConfiguration.ui b/src/ui/QGCUDPClientLinkConfiguration.ui new file mode 100644 index 0000000000..56fb5681af --- /dev/null +++ b/src/ui/QGCUDPClientLinkConfiguration.ui @@ -0,0 +1,48 @@ + + + QGCUDPClientLinkConfiguration + + + + 0 + 0 + 400 + 300 + + + + Form + + + + + + 1000 + + + 100000 + + + + + + + Address + + + + + + + Port + + + + + + + + + + + diff --git a/src/ui/QGCUDPLinkConfiguration.cc b/src/ui/QGCUDPLinkConfiguration.cc index bd12c3b3b4..e649bc8e0d 100644 --- a/src/ui/QGCUDPLinkConfiguration.cc +++ b/src/ui/QGCUDPLinkConfiguration.cc @@ -9,7 +9,7 @@ QGCUDPLinkConfiguration::QGCUDPLinkConfiguration(int linkid, QWidget *parent) : { m_linkId = linkid; ui->setupUi(this); - ui->portSpinBox->setValue(LinkManager::instance()->getUdpLinkPort(linkid)); + ui->portSpinBox->setValue(getUdpLink()->getPort()); connect(LinkManager::instance(),SIGNAL(linkChanged(int)),this,SLOT(linkChanged(int))); connect(ui->portSpinBox, SIGNAL(valueChanged(int)), this, SLOT(portValueChanged(int))); connect(ui->addIPButton, SIGNAL(clicked()), this, SLOT(addHost())); @@ -33,7 +33,7 @@ void QGCUDPLinkConfiguration::changeEvent(QEvent *e) } void QGCUDPLinkConfiguration::portValueChanged(int value) { - LinkManager::instance()->setUdpLinkPort(m_linkId,value); + getUdpLink()->setPort(value); } void QGCUDPLinkConfiguration::addHost() @@ -44,7 +44,7 @@ void QGCUDPLinkConfiguration::addHost() "localhost:14555", &ok); if (ok && !hostName.isEmpty()) { - LinkManager::instance()->addUdpHost(m_linkId,hostName); + getUdpLink()->addHost(hostName); } } void QGCUDPLinkConfiguration::linkChanged(int linkid) @@ -54,6 +54,11 @@ void QGCUDPLinkConfiguration::linkChanged(int linkid) return; } disconnect(ui->portSpinBox, SIGNAL(valueChanged(int)), this, SLOT(portValueChanged(int))); - ui->portSpinBox->setValue(LinkManager::instance()->getUdpLinkPort(m_linkId)); + ui->portSpinBox->setValue(getUdpLink()->getPort()); connect(ui->portSpinBox, SIGNAL(valueChanged(int)), this, SLOT(portValueChanged(int))); } + +UDPLink* QGCUDPLinkConfiguration::getUdpLink() const +{ + return dynamic_cast(LinkManager::instance()->getLink(m_linkId)); +} diff --git a/src/ui/QGCUDPLinkConfiguration.h b/src/ui/QGCUDPLinkConfiguration.h index cb3a8a62d3..c88be3eb41 100644 --- a/src/ui/QGCUDPLinkConfiguration.h +++ b/src/ui/QGCUDPLinkConfiguration.h @@ -22,14 +22,14 @@ public slots: void addHost(); void portValueChanged(int value); void linkChanged(int linkid); -protected: - void changeEvent(QEvent *e); - //UDPLink* link; ///< UDP link instance this widget configures - int m_linkId; +private: + void changeEvent(QEvent *e); + UDPLink* getUdpLink() const; private: Ui::QGCUDPLinkConfiguration *ui; + int m_linkId; }; #endif // QGCUDPLINKCONFIGURATION_H diff --git a/src/ui/SerialConfigurationWindow.cc b/src/ui/SerialConfigurationWindow.cc index da8711fb53..16d5b598db 100644 --- a/src/ui/SerialConfigurationWindow.cc +++ b/src/ui/SerialConfigurationWindow.cc @@ -41,7 +41,7 @@ This file is part of the QGROUNDCONTROL project SerialConfigurationWindow::SerialConfigurationWindow(int linkid, QWidget *parent, Qt::WindowFlags flags) : QWidget(parent, flags), userConfigured(false) { - m_linkid = linkid; + m_linkId = linkid; ui.setupUi(this); @@ -59,9 +59,6 @@ SerialConfigurationWindow::SerialConfigurationWindow(int linkid, QWidget *parent // Connect the current UAS action = new QAction(QIcon(":/files/images/devices/network-wireless.svg"), "", this); - setLinkName(LinkManager::instance()->getSerialLinkPort(linkid)); - - // Set up baud rates ui.baudRate->clear(); @@ -119,13 +116,18 @@ SerialConfigurationWindow::SerialConfigurationWindow(int linkid, QWidget *parent setupPortList(); // Load current link config - QString linkportname = LinkManager::instance()->getSerialLinkPort(linkid); + SerialLinkInterface* serialLink = getSerialInterfaceLink(); + QString linkportname = ""; + int linkbaudrate = -1; + if (serialLink) { + linkportname = serialLink->getName(); + setLinkName(linkportname); + linkbaudrate = serialLink->getBaudRate(); + } int portid = ui.portName->findText(linkportname); - - int linkbaudrate = LinkManager::instance()->getSerialLinkBaud(linkid); int baudid = ui.baudRate->findText(QString::number(linkbaudrate)); - QLOG_DEBUG() << "Baud rate:" << LinkManager::instance()->getSerialLinkBaud(linkid) << "Expected:" << baudid; + QLOG_DEBUG() << "Baud rate:" << linkbaudrate << "Expected:" << baudid; if (baudid == -1) { if (linkbaudrate != -1 && linkbaudrate != 0) @@ -176,7 +178,7 @@ SerialConfigurationWindow::SerialConfigurationWindow(int linkid, QWidget *parent // Make sure that a change in the link name will be reflected in the UI connect(LinkManager::instance(),SIGNAL(linkChanged(int)),this,SLOT(linkChanged(int))); - linkChanged(m_linkid); + linkChanged(m_linkId); // Connect the individual user interface inputs connect(ui.portName, SIGNAL(editTextChanged(QString)), this, SLOT(setPortName(QString))); @@ -240,7 +242,7 @@ SerialConfigurationWindow::SerialConfigurationWindow(int linkid, QWidget *parent } void SerialConfigurationWindow::linkChanged(int linkid) { - if (linkid != m_linkid) + if (linkid != m_linkId) { return; } @@ -323,19 +325,20 @@ void SerialConfigurationWindow::configureCommunication() void SerialConfigurationWindow::setupPortList() { - //if (!link) return; + SerialLinkInterface* serialLink = getSerialInterfaceLink(); + if (!serialLink){ + return; + } - QLOG_DEBUG() << "SCW: Link is" << (LinkManager::instance()->getLinkConnected(this->m_linkid) ? "connected" : "disconnected"); + QLOG_DEBUG() << "SCW: Link is" << (serialLink->isConnected() ? "connected" : "disconnected"); // Get the ports available on this system - QList ports = LinkManager::instance()->getCurrentPorts(); - + QList ports = serialLink->getCurrentPorts(); - QString storedName = LinkManager::instance()->getSerialLinkPort(m_linkid); + QString storedName = serialLink->getPortName(); QString currentName = ui.portName->currentText(); - if (ui.portName->currentText() != ui.portName->itemText(ui.portName->currentIndex())) { //Custom text entered @@ -375,11 +378,11 @@ void SerialConfigurationWindow::enableFlowControl(bool flow) { if(flow) { - LinkManager::instance()->setSerialFlowType(m_linkid,1); + getSerialInterfaceLink()->setFlowType(1); } else { - LinkManager::instance()->setSerialFlowType(m_linkid,0); + getSerialInterfaceLink()->setFlowType(0); } } @@ -387,7 +390,7 @@ void SerialConfigurationWindow::setParityNone(bool accept) { if (accept) { - LinkManager::instance()->setSerialParityType(m_linkid,0); + getSerialInterfaceLink()->setParityType(0); } //if (accept) link->setParityType(0); } @@ -396,7 +399,7 @@ void SerialConfigurationWindow::setParityOdd(bool accept) { if (accept) { - LinkManager::instance()->setSerialParityType(m_linkid,1); + getSerialInterfaceLink()->setParityType(1); } } @@ -404,17 +407,17 @@ void SerialConfigurationWindow::setParityEven(bool accept) { if (accept) { - LinkManager::instance()->setSerialParityType(m_linkid,2); + getSerialInterfaceLink()->setParityType(2); } } void SerialConfigurationWindow::setDataBits(int bits) { - LinkManager::instance()->setSerialDataBits(m_linkid,bits); + getSerialInterfaceLink()->setDataBitsType(bits); } void SerialConfigurationWindow::setStopBits(int bits) { - LinkManager::instance()->setSerialStopBits(m_linkid,bits); + getSerialInterfaceLink()->setStopBitsType(bits); } void SerialConfigurationWindow::setPortName(QString port) @@ -424,7 +427,7 @@ void SerialConfigurationWindow::setPortName(QString port) #endif port = port.remove(" "); - LinkManager::instance()->modifySerialConnection(m_linkid,port); + getSerialInterfaceLink()->setPortName(port); userConfigured = true; } @@ -446,7 +449,7 @@ void SerialConfigurationWindow::setBaudRateString(QString baud) if (!ok) { disconnect(ui.baudRate,SIGNAL(currentIndexChanged(QString)),this,SLOT(setBaudRateString(QString))); - ui.baudRate->setCurrentText(QString::number(LinkManager::instance()->getSerialLinkBaud(m_linkid))); + ui.baudRate->setCurrentText(QString::number(LinkManager::instance()->getSerialLinkBaud(m_linkId))); connect(ui.baudRate,SIGNAL(currentIndexChanged(QString)),this,SLOT(setBaudRateString(QString))); return; } @@ -458,5 +461,15 @@ void SerialConfigurationWindow::setBaudRateString(QString baud) port = port.split("-").first(); #endif port = port.remove(" "); - LinkManager::instance()->modifySerialConnection(m_linkid,port,baud.toInt()); + + SerialLinkInterface *link = getSerialInterfaceLink(); + if (link){ + link->setPortName(port); + link->setBaudRate(baud.toInt()); + } +} + +SerialLinkInterface* SerialConfigurationWindow::getSerialInterfaceLink() const +{ + return dynamic_cast(LinkManager::instance()->getLink(m_linkId)); } diff --git a/src/ui/SerialConfigurationWindow.h b/src/ui/SerialConfigurationWindow.h index f11aa6f411..675ade861d 100644 --- a/src/ui/SerialConfigurationWindow.h +++ b/src/ui/SerialConfigurationWindow.h @@ -64,19 +64,21 @@ public slots: void setStopBits(int bits); void setupPortList(); void setAdvancedSettings(bool visible); + private slots: void connectionStateChanged(bool connected); void linkChanged(int linkid); void setBaudRateString(QString baud); -protected: + +private: void showEvent(QShowEvent* event); void hideEvent(QHideEvent* event); - bool userConfigured; ///< Switch to detect if current values are user-selected and shouldn't be overriden + SerialLinkInterface* getSerialInterfaceLink() const; private: - Ui::serialSettings ui; - int m_linkid; + int m_linkId; + bool userConfigured; ///< Switch to detect if current values are user-selected and shouldn't be overriden QAction* action; QTimer* portCheckTimer; diff --git a/src/ui/WaypointEditableView.cc b/src/ui/WaypointEditableView.cc index 70053a7c6e..8a11cfc0c6 100644 --- a/src/ui/WaypointEditableView.cc +++ b/src/ui/WaypointEditableView.cc @@ -161,6 +161,8 @@ WaypointEditableView::WaypointEditableView(Waypoint* wp, QWidget* parent) : connect(m_ui->upButton, SIGNAL(clicked()), this, SLOT(moveUp())); connect(m_ui->downButton, SIGNAL(clicked()), this, SLOT(moveDown())); + connect(m_ui->topButton, SIGNAL(clicked()), this, SLOT(moveTop())); + connect(m_ui->bottomButton, SIGNAL(clicked()), this, SLOT(moveBottom())); connect(m_ui->removeButton, SIGNAL(clicked()), this, SLOT(remove())); connect(m_ui->autoContinue, SIGNAL(stateChanged(int)), this, SLOT(changedAutoContinue(int))); @@ -180,6 +182,16 @@ void WaypointEditableView::moveDown() emit moveDownWaypoint(wp); } +void WaypointEditableView::moveTop() +{ + emit moveTopWaypoint(wp); +} + +void WaypointEditableView::moveBottom() +{ + emit moveBottomWaypoint(wp); +} + void WaypointEditableView::remove() { diff --git a/src/ui/WaypointEditableView.h b/src/ui/WaypointEditableView.h index 2360f8ced6..394d2cbba6 100644 --- a/src/ui/WaypointEditableView.h +++ b/src/ui/WaypointEditableView.h @@ -86,6 +86,9 @@ class WaypointEditableView : public QWidget public slots: void moveUp(); void moveDown(); + void moveTop(); + void moveBottom(); + void remove(); /** @brief Waypoint matching this widget has been deleted */ void deleted(QObject* waypoint); @@ -125,7 +128,9 @@ protected slots: signals: void moveUpWaypoint(Waypoint*); void moveDownWaypoint(Waypoint*); - void removeWaypoint(Waypoint*); + void moveTopWaypoint(Waypoint*); + void moveBottomWaypoint(Waypoint*); + void removeWaypoint(Waypoint*); void changeCurrentWaypoint(quint16); void setYaw(double); diff --git a/src/ui/WaypointEditableView.ui b/src/ui/WaypointEditableView.ui index 9aec328804..777603f07f 100644 --- a/src/ui/WaypointEditableView.ui +++ b/src/ui/WaypointEditableView.ui @@ -287,6 +287,58 @@ QPushButton:pressed { + + + + + 0 + 0 + + + + Qt::NoFocus + + + Move to top + + + Move to top + + + + + + + :/files/images/actions/go-top.svg:/files/images/actions/go-top.svg + + + + + + + + 0 + 0 + + + + Qt::NoFocus + + + Move to bottom + + + Move to bottom + + + + + + + :/files/images/actions/go-bottom.svg:/files/images/actions/go-bottom.svg + + + @@ -311,10 +363,10 @@ QPushButton:pressed { Qt::NoFocus - Move Up in List + Move up - Move Up in List + Move up @@ -349,10 +401,10 @@ QPushButton:pressed { Qt::NoFocus - Move Down in List + Move down - Move Down in List + Move down diff --git a/src/ui/WaypointList.cc b/src/ui/WaypointList.cc index c791e83f4e..78871480ae 100644 --- a/src/ui/WaypointList.cc +++ b/src/ui/WaypointList.cc @@ -601,6 +601,8 @@ void WaypointList::waypointEditableListChanged() wpEditableViews.insert(wp, wpview); connect(wpview, SIGNAL(moveDownWaypoint(Waypoint*)), this, SLOT(moveDown(Waypoint*))); connect(wpview, SIGNAL(moveUpWaypoint(Waypoint*)), this, SLOT(moveUp(Waypoint*))); + connect(wpview, SIGNAL(moveTopWaypoint(Waypoint*)), this, SLOT(moveTop(Waypoint*))); + connect(wpview, SIGNAL(moveBottomWaypoint(Waypoint*)), this, SLOT(moveBottom(Waypoint*))); connect(wpview, SIGNAL(removeWaypoint(Waypoint*)), this, SLOT(removeWaypoint(Waypoint*))); //connect(wpview, SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointChanged(quint16))); connect(wpview, SIGNAL(changeCurrentWaypoint(quint16)), this, SLOT(currentWaypointEditableChanged(quint16))); @@ -662,8 +664,44 @@ void WaypointList::moveDown(Waypoint* wp) } } +void WaypointList::moveTop(Waypoint* wp) +{ + const QList &waypoints = WPM->getWaypointEditableList(); + + //get the current position of wp in the local storage + int i; + for (i = 0; i < waypoints.count(); i++) { + if (waypoints[i] == wp) + break; + } + + // if wp was found and its not the first entry, move it + // For APM the first entry is WP1 + if (i < waypoints.count() && i > 1) { + WPM->moveWaypoint(i, 1); + } +} + +void WaypointList::moveBottom(Waypoint* wp) +{ + const QList &waypoints = WPM->getWaypointEditableList(); + + //get the current position of wp in the local storage + int i; + // For APM entries start at WP1 + for (i = 1; i < waypoints.count(); i++) { + if (waypoints[i] == wp) + break; + } + + // if wp was found and its not the last entry, move it + if (i < waypoints.count()-1) { + WPM->moveWaypoint(i, waypoints.count()-1); + } +} + void WaypointList::removeWaypoint(Waypoint* wp) -{ +{ if (wp && (wp->getId() > 0)){ // APM use WP0 as home so do not remove it WPM->removeWaypoint(wp->getId()); } diff --git a/src/ui/WaypointList.h b/src/ui/WaypointList.h index 580a2b8e2f..0e53fb632e 100644 --- a/src/ui/WaypointList.h +++ b/src/ui/WaypointList.h @@ -114,6 +114,8 @@ public slots: // Waypoint operations void moveUp(Waypoint* wp); void moveDown(Waypoint* wp); + void moveTop(Waypoint* wp); + void moveBottom(Waypoint* wp); void removeWaypoint(Waypoint* wp); void parameterChanged(int uas, int component, QString parameterName, QVariant value); diff --git a/src/ui/WaypointList.ui b/src/ui/WaypointList.ui index 915584d320..2d515722f0 100644 --- a/src/ui/WaypointList.ui +++ b/src/ui/WaypointList.ui @@ -68,6 +68,9 @@ m + + 999.000000000000000 + @@ -136,6 +139,9 @@ Save Mission + + Ctrl+S + @@ -176,12 +182,15 @@ Read all waypoints from the MAV. Clears the list on this computer. - Get + Read :/files/images/status/software-update-available.svg:/files/images/status/software-update-available.svg + + Ctrl+R + @@ -251,6 +260,9 @@ Load Mission + + Ctrl+L + @@ -290,6 +302,9 @@ :/files/images/devices/network-wireless.svg:/files/images/devices/network-wireless.svg + + Ctrl+W + @@ -400,6 +415,9 @@ :/files/images/actions/go-jump.svg:/files/images/actions/go-jump.svg + + Ctrl+R + diff --git a/src/ui/configuration/AccelCalibrationConfig.cc b/src/ui/configuration/AccelCalibrationConfig.cc index 948f83ab09..2fdd9985de 100644 --- a/src/ui/configuration/AccelCalibrationConfig.cc +++ b/src/ui/configuration/AccelCalibrationConfig.cc @@ -24,15 +24,18 @@ This file is part of the APM_PLANNER project #include "GAudioOutput.h" #include "MainWindow.h" + +const char* COUNTDOWN_STRING = "

Calibrate MAV%03d
Time remaining until timeout: %d

"; + AccelCalibrationConfig::AccelCalibrationConfig(QWidget *parent) : AP2ConfigWidget(parent), - m_muted(false) + m_muted(false), + m_isCalibrating(false), + m_countdownCount(CALIBRATION_TIMEOUT_SEC) { ui.setupUi(this); connect(ui.calibrateAccelButton,SIGNAL(clicked()),this,SLOT(calibrateButtonClicked())); - connect(ui.levelAccelButton,SIGNAL(clicked()),this,SLOT(levelButtonClicked())); m_accelAckCount=0; - m_isLeveling = false; initConnections(); //coutdownLabel connect(&m_countdownTimer,SIGNAL(timeout()),this,SLOT(countdownTimerTick())); @@ -43,7 +46,7 @@ AccelCalibrationConfig::~AccelCalibrationConfig() } void AccelCalibrationConfig::countdownTimerTick() { - ui.coutdownLabel->setText("Time remaining until timeout: " + QString::number(m_countdownCount--)); + ui.coutdownLabel->setText(QString().sprintf(COUNTDOWN_STRING, m_uas->getUASID(), m_countdownCount--)); if (m_countdownCount <= 0) { ui.coutdownLabel->setText("Command timed out, please try again"); @@ -74,31 +77,7 @@ void AccelCalibrationConfig::activeUASSet(UASInterface *uas) } void AccelCalibrationConfig::uasConnected() { - if (m_uas->isFixedWing()) - { - //Show fixed wing level stuff here - ui.levelAccelButton->setVisible(true); - ui.levelOutputLabel->setVisible(true); - ui.levelDescLabel->setVisible(true); - } - else if (m_uas->isMultirotor()) - { - ui.levelAccelButton->setVisible(false); - ui.levelOutputLabel->setVisible(false); - ui.levelDescLabel->setVisible(false); - } - else if (m_uas->isGroundRover()) - { - ui.levelAccelButton->setVisible(false); - ui.levelOutputLabel->setVisible(false); - ui.levelDescLabel->setVisible(false); - } - else - { - ui.levelAccelButton->setVisible(false); - ui.levelOutputLabel->setVisible(false); - ui.levelDescLabel->setVisible(false); - } + } void AccelCalibrationConfig::uasDisconnected() @@ -106,47 +85,18 @@ void AccelCalibrationConfig::uasDisconnected() } -void AccelCalibrationConfig::levelButtonClicked() +void AccelCalibrationConfig::calibrateButtonClicked() { if (!m_uas) { showNullMAVErrorMessageBox(); return; } - // Mute Audio until calibrated to avoid HeartBeat Warning message - if (GAudioOutput::instance()->isMuted() == false) { - GAudioOutput::instance()->mute(true); - m_muted = true; - } - m_uas->getLinks()->at(0)->disableTimeouts(); - MainWindow::instance()->toolBar().stopAnimation(); + ui.outputLabel->clear(); - MAV_CMD command = MAV_CMD_PREFLIGHT_CALIBRATION; - int confirm = 0; - float param1 = 1.0; - float param2 = 0.0; - float param3 = 0.0; - float param4 = 0.0; - float param5 = 0.0; - float param6 = 0.0; - float param7 = 0.0; - int component = 1; - m_uas->executeCommand(command, confirm, param1, param2, param3, param4, param5, param6, param7, component); - if (m_muted) { // turns audio backon, when you leave the page - GAudioOutput::instance()->mute(false); - m_muted = false; - } - m_isLeveling = true; -} + m_isCalibrating = true; // this is to guard against showing unwanted GCS Text Messages. -void AccelCalibrationConfig::calibrateButtonClicked() -{ - if (!m_uas) - { - showNullMAVErrorMessageBox(); - return; - } // Mute Audio until calibrated to avoid HeartBeat Warning message if (GAudioOutput::instance()->isMuted() == false) { GAudioOutput::instance()->mute(true); @@ -169,14 +119,14 @@ void AccelCalibrationConfig::calibrateButtonClicked() float param7 = 0.0; int component = 1; m_uas->executeCommand(command, confirm, param1, param2, param3, param4, param5, param6, param7, component); - m_countdownCount = 40; - ui.coutdownLabel->setText("Time remaining until timeout: 30"); + m_countdownCount = CALIBRATION_TIMEOUT_SEC; + ui.coutdownLabel->setText(QString().sprintf(COUNTDOWN_STRING, m_uas->getUASID(), m_countdownCount--)); m_countdownTimer.start(1000); } else if (m_accelAckCount <= 10) { m_uas->executeCommandAck(m_accelAckCount++,true); - m_countdownCount = 40; + m_countdownCount = CALIBRATION_TIMEOUT_SEC; } else { @@ -200,6 +150,8 @@ void AccelCalibrationConfig::calibrateButtonClicked() void AccelCalibrationConfig::hideEvent(QHideEvent *evt) { + Q_UNUSED(evt); + if (m_muted) { // turns audio backon, when you leave the page GAudioOutput::instance()->mute(false); m_muted = false; @@ -219,26 +171,26 @@ void AccelCalibrationConfig::hideEvent(QHideEvent *evt) } void AccelCalibrationConfig::uasTextMessageReceived(int uasid, int componentid, int severity, QString text) { - //command received: " Severity 1 - //Place APM Level and press any key" severity 5 - if (m_isLeveling) + Q_UNUSED(uasid); + Q_UNUSED(componentid); + + if ((severity == 5 /*SEVERITY_USER_RESPONSE*/)||(severity == 3 /*SEVERITY_HIGH*/)) { - ui.levelOutputLabel->setText(text); - if (text.toLower().contains("success")) + //This is a calibration instruction + if (!m_isCalibrating || text.startsWith("PreArm:") || text.startsWith("EKF") || text.startsWith("Arm")) { - m_isLeveling = false; + // Don't show these warning messages + return; } - } - if (severity == 5) - { - //This is a calibration instruction + if (text.contains("Place") && text.contains ("and press any key")) { //Instruction if (m_accelAckCount == 0) { //Calibration Sucessful\r" - ui.calibrateAccelButton->setText("Continue"); + ui.calibrateAccelButton->setText("Continue\nPress SpaceBar"); + ui.calibrateAccelButton->setShortcut(QKeySequence(Qt::Key_Space)); } ui.outputLabel->setText(text); m_accelAckCount++; @@ -256,6 +208,8 @@ void AccelCalibrationConfig::uasTextMessageReceived(int uasid, int componentid, MainWindow::instance()->toolBar().startAnimation(); m_accelAckCount = 0; ui.calibrateAccelButton->setText("Calibrate\nAccelerometer"); + ui.calibrateAccelButton->setShortcut(QKeySequence()); + m_isCalibrating = false; } else { diff --git a/src/ui/configuration/AccelCalibrationConfig.h b/src/ui/configuration/AccelCalibrationConfig.h index 9f10ad7324..e7c59baebf 100644 --- a/src/ui/configuration/AccelCalibrationConfig.h +++ b/src/ui/configuration/AccelCalibrationConfig.h @@ -40,6 +40,7 @@ class AccelCalibrationConfig : public AP2ConfigWidget { Q_OBJECT + static const int CALIBRATION_TIMEOUT_SEC = 40; public: explicit AccelCalibrationConfig(QWidget *parent = 0); ~AccelCalibrationConfig(); @@ -51,13 +52,13 @@ private slots: void uasTextMessageReceived(int uasid, int componentid, int severity, QString text); void uasConnected(); void uasDisconnected(); - void levelButtonClicked(); void countdownTimerTick(); + private: int m_accelAckCount; Ui::AccelCalibrationConfig ui; bool m_muted; - bool m_isLeveling; + bool m_isCalibrating; QTimer m_countdownTimer; int m_countdownCount; }; diff --git a/src/ui/configuration/AccelCalibrationConfig.ui b/src/ui/configuration/AccelCalibrationConfig.ui index da2c68de96..565258a16c 100644 --- a/src/ui/configuration/AccelCalibrationConfig.ui +++ b/src/ui/configuration/AccelCalibrationConfig.ui @@ -6,8 +6,8 @@ 0 0 - 896 - 514 + 397 + 331 @@ -18,53 +18,27 @@ 10 0 - 281 - 31 + 285 + 24 - <h2>Accelerometer Calibration</h2> + <html><head/><body><p><span style=" font-size:x-large; font-weight:600;">3D Accelerometer Calibration</span></p></body></html> false - - - - 440 - 80 - 241 - 201 - - - - - - - true - - - Qt::AlignCenter - - - 50 - 70 + 10 + 30 351 - 401 + 281 - - - - - - - - + @@ -75,6 +49,13 @@ + + + + + + + @@ -101,13 +82,12 @@ - 75 + 120 35 - Calibrate -Accelerometer + Calibrate @@ -140,73 +120,6 @@ Accelerometer - - - - Level your UAV to set default accelerometer offsets (1 axis)<br>This requires you to place your autopilot on a level surface. - - - true - - - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - - 75 - 35 - - - - Level -Accelerometer - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - - - Qt::Vertical - - - - 20 - 40 - - - - diff --git a/src/ui/configuration/AdvParameterList.cc b/src/ui/configuration/AdvParameterList.cc index 6955f1702e..96267249a6 100644 --- a/src/ui/configuration/AdvParameterList.cc +++ b/src/ui/configuration/AdvParameterList.cc @@ -276,7 +276,7 @@ void AdvParameterList::saveButtonClicked() QLOG_DEBUG() << "CREATED:" << fileDialog; fileDialog->setAcceptMode(QFileDialog::AcceptSave); fileDialog->setNameFilter("*.param *.txt"); - fileDialog->selectFile("paramter.param"); + fileDialog->selectFile("parameter.param"); fileDialog->open(this, SLOT(saveDialogAccepted())); connect(fileDialog,SIGNAL(rejected()),SLOT(dialogRejected())); } diff --git a/src/ui/configuration/AdvParameterList.ui b/src/ui/configuration/AdvParameterList.ui index 222aed410b..939119b6cb 100644 --- a/src/ui/configuration/AdvParameterList.ui +++ b/src/ui/configuration/AdvParameterList.ui @@ -116,6 +116,9 @@ Load From File + + Ctrl+L + @@ -123,6 +126,9 @@ Save To File + + Ctrl+S + @@ -174,6 +180,9 @@ Refresh + + Ctrl+R + @@ -181,6 +190,9 @@ Write To + + Ctrl+W + diff --git a/src/ui/configuration/AirspeedConfig.cc b/src/ui/configuration/AirspeedConfig.cc index 14caee4a7b..f781187141 100644 --- a/src/ui/configuration/AirspeedConfig.cc +++ b/src/ui/configuration/AirspeedConfig.cc @@ -139,6 +139,7 @@ void AirspeedConfig::hardwareSelectComboBoxChanged(int index) ui.sensorComboBox->addItem("Analog Sensor"); ui.sensorComboBox->addItem("EagleTree I2C"); ui.sensorComboBox->addItem("MEAS I2C"); + ui.sensorComboBox->addItem("Pixhawk I2C"); ui.sensorComboBox->setEnabled(true); disconnect(ui.pinComboBox,SIGNAL(currentIndexChanged(int)),this,SLOT(pinSelectComboBoxChanged(int))); ui.pinComboBox->clear(); @@ -154,6 +155,7 @@ void AirspeedConfig::hardwareSelectComboBoxChanged(int index) ui.sensorComboBox->addItem("Analog Sensor"); ui.sensorComboBox->addItem("EagleTree I2C"); ui.sensorComboBox->addItem("MEAS I2C"); + ui.sensorComboBox->addItem("Pixhawk I2C"); ui.sensorComboBox->setEnabled(true); disconnect(ui.pinComboBox,SIGNAL(currentIndexChanged(int)),this,SLOT(pinSelectComboBoxChanged(int))); ui.pinComboBox->clear(); @@ -169,6 +171,7 @@ void AirspeedConfig::hardwareSelectComboBoxChanged(int index) ui.sensorComboBox->addItem("Analog Sensor"); ui.sensorComboBox->addItem("EagleTree I2C"); ui.sensorComboBox->addItem("MEAS I2C"); + ui.sensorComboBox->addItem("Pixhawk I2C"); ui.sensorComboBox->setEnabled(true); disconnect(ui.pinComboBox,SIGNAL(currentIndexChanged(int)),this,SLOT(pinSelectComboBoxChanged(int))); ui.pinComboBox->clear(); @@ -239,7 +242,7 @@ void AirspeedConfig::sensorSelectComboBoxChanged(int index) connect(ui.pinComboBox,SIGNAL(currentIndexChanged(int)),this,SLOT(pinSelectComboBoxChanged(int))); } } - else if (index == 2 || index == 3) //I2C Sensor + else if (index == 2 || index == 3 || index == 4) //I2C Sensor { disconnect(ui.pinComboBox,SIGNAL(currentIndexChanged(int)),this,SLOT(pinSelectComboBoxChanged(int))); ui.pinComboBox->clear(); diff --git a/src/ui/configuration/ApmSoftwareConfig.cc b/src/ui/configuration/ApmSoftwareConfig.cc index e468459a24..66bcdf03c2 100644 --- a/src/ui/configuration/ApmSoftwareConfig.cc +++ b/src/ui/configuration/ApmSoftwareConfig.cc @@ -80,11 +80,6 @@ ApmSoftwareConfig::ApmSoftwareConfig(QWidget *parent) : QWidget(parent), m_buttonToConfigWidgetMap[ui.basicPidButton] = m_basicPidConfig; connect(ui.basicPidButton,SIGNAL(clicked()),this,SLOT(activateStackedWidget())); - m_arduCopterPidConfig = new ArduCopterPidConfig(this); - ui.stackedWidget->addWidget(m_arduCopterPidConfig); - m_buttonToConfigWidgetMap[ui.arduCopterPidButton] = m_arduCopterPidConfig; - connect(ui.arduCopterPidButton,SIGNAL(clicked()),this,SLOT(activateStackedWidget())); - m_arduPlanePidConfig = new ArduPlanePidConfig(this); ui.stackedWidget->addWidget(m_arduPlanePidConfig); m_buttonToConfigWidgetMap[ui.arduPlanePidButton] = m_arduPlanePidConfig; @@ -101,6 +96,8 @@ ApmSoftwareConfig::ApmSoftwareConfig(QWidget *parent) : QWidget(parent), connect(ui.plannerConfigButton,SIGNAL(clicked()),this,SLOT(activateStackedWidget())); ui.stackedWidget->setCurrentWidget(m_buttonToConfigWidgetMap[ui.plannerConfigButton]); + connect(ui.arduCopterPidButton, SIGNAL(clicked()), this, SLOT(updateUAS())); + connect(UASManager::instance(),SIGNAL(activeUASSet(UASInterface*)),this,SLOT(activeUASSet(UASInterface*))); activeUASSet(UASManager::instance()->getActiveUAS()); @@ -620,3 +617,45 @@ void ApmSoftwareConfig::parameterChanged(int uas, int component, int parameterCo } } +void ApmSoftwareConfig::updateUAS() +{ + reloadView(); +} + +void ApmSoftwareConfig::reloadView() +{ + if (m_uas){ + // Detects if we are using new copter PIDS or old ones + QVariant returnValue; + if (m_uas->getParamManager()->getParameterValue(1, QString("POS_XY_P"), returnValue)){ + // Use New Copter PID UI + if (m_arduCopterPidConfig){ + ui.stackedWidget->removeWidget(m_arduCopterPidConfig); + delete m_arduCopterPidConfig; + } + if (!m_copterPidConfig){ + m_copterPidConfig = new CopterPidConfig(this); + ui.stackedWidget->addWidget(m_copterPidConfig); + m_buttonToConfigWidgetMap[ui.arduCopterPidButton] = m_copterPidConfig; + connect(ui.arduCopterPidButton,SIGNAL(clicked()),this,SLOT(activateStackedWidget())); + activateStackedWidget(); + QTimer::singleShot(100,m_copterPidConfig, SLOT(refreshButtonClicked())); + } + + } else if (m_uas->getParamManager()->getParameterValue(1, "HLD_LAT_P", returnValue)){ + // Use old ArduCopter PID UI, + if (m_copterPidConfig){ + ui.stackedWidget->removeWidget(m_copterPidConfig); + delete m_copterPidConfig; + } + if (!m_arduCopterPidConfig){ + m_arduCopterPidConfig = new ArduCopterPidConfig(this); + ui.stackedWidget->addWidget(m_arduCopterPidConfig); + m_buttonToConfigWidgetMap[ui.arduCopterPidButton] = m_arduCopterPidConfig; + connect(ui.arduCopterPidButton,SIGNAL(clicked()),this,SLOT(activateStackedWidget())); + activateStackedWidget(); + QTimer::singleShot(100,m_arduCopterPidConfig, SLOT(refreshButtonClicked())); + } + } + } +} diff --git a/src/ui/configuration/ApmSoftwareConfig.h b/src/ui/configuration/ApmSoftwareConfig.h index a237d53ef4..e2ccec58e5 100644 --- a/src/ui/configuration/ApmSoftwareConfig.h +++ b/src/ui/configuration/ApmSoftwareConfig.h @@ -39,6 +39,7 @@ This file is part of the APM_PLANNER project #include "FailSafeConfig.h" #include "AdvancedParamConfig.h" #include "ArduCopterPidConfig.h" +#include "CopterPidConfig.h" #include "ArduPlanePidConfig.h" #include "ArduRoverPidConfig.h" #include "AdvParameterList.h" @@ -70,6 +71,9 @@ private slots: void uasDisconnected(); void apmParamNetworkReplyFinished(); void populateTimerTick(); + void updateUAS(); + void reloadView(); + private: //Parameter from XML @@ -102,6 +106,7 @@ private slots: QPointer m_geoFenceConfig; QPointer m_advancedParamConfig; QPointer m_arduCopterPidConfig; + QPointer m_copterPidConfig; QPointer m_arduPlanePidConfig; QPointer m_arduRoverPidConfig; QPointer m_advParameterList; diff --git a/src/ui/configuration/ApmSoftwareConfig.ui b/src/ui/configuration/ApmSoftwareConfig.ui index fee7884d31..236c518151 100644 --- a/src/ui/configuration/ApmSoftwareConfig.ui +++ b/src/ui/configuration/ApmSoftwareConfig.ui @@ -37,7 +37,7 @@ 0 0 201 - 592 + 598 diff --git a/src/ui/configuration/ArduCopterPidConfig.cc b/src/ui/configuration/ArduCopterPidConfig.cc index bbfa1138c5..5ccd941e42 100644 --- a/src/ui/configuration/ArduCopterPidConfig.cc +++ b/src/ui/configuration/ArduCopterPidConfig.cc @@ -52,8 +52,8 @@ ArduCopterPidConfig::ArduCopterPidConfig(QWidget *parent) : AP2ConfigWidget(pare connect(ui.rateRollIMAXSpinBox,SIGNAL(valueChanged(double)),this,SLOT(rateIMAXChanged(double))); - m_nameToBoxMap["STB_RLL_P"] = ui.stabilPitchPSpinBox; - m_nameToBoxMap["STB_PIT_P"] = ui.stabilRollPSpinBox; + m_nameToBoxMap["STB_RLL_P"] = ui.stabilRollPSpinBox; + m_nameToBoxMap["STB_PIT_P"] = ui.stabilPitchPSpinBox; m_nameToBoxMap["STB_YAW_P"] = ui.stabilYawPSpinBox; m_nameToBoxMap["HLD_LAT_P"] = ui.loiterPidPSpinBox; @@ -87,10 +87,10 @@ ArduCopterPidConfig::ArduCopterPidConfig(QWidget *parent) : AP2ConfigWidget(pare m_nameToBoxMap["THR_ALT_P"] = ui.altitudeHoldPSpinBox; - m_nameToBoxMap["WPNAV_SPEED"] = ui.wpNavLoiterSpeedSpinBox; + m_nameToBoxMap["WPNAV_SPEED"] = ui.wpNavSpeedSpinBox; m_nameToBoxMap["WPNAV_RADIUS"] = ui.wpNavRadiusSpinBox; m_nameToBoxMap["WPNAV_SPEED_DN"] = ui.wpNavSpeedDownSpinBox; - m_nameToBoxMap["WPNAV_LOIT_SPEED"] = ui.wpNavSpeedSpinBox; + m_nameToBoxMap["WPNAV_LOIT_SPEED"] = ui.wpNavLoiterSpeedSpinBox; m_nameToBoxMap["WPNAV_SPEED_UP"] = ui.wpNavSpeedUpSpinBox; //m_nameToBoxMap["TUNE_HIGH"] = ui.ch6MaxSpinBox; diff --git a/src/ui/configuration/ArduCopterPidConfig.ui b/src/ui/configuration/ArduCopterPidConfig.ui index 764f6ef774..606cef77fe 100644 --- a/src/ui/configuration/ArduCopterPidConfig.ui +++ b/src/ui/configuration/ArduCopterPidConfig.ui @@ -6,8 +6,8 @@ 0 0 - 669 - 519 + 904 + 595 @@ -1239,6 +1239,9 @@ Write Params + + Ctrl+W + @@ -1442,6 +1445,9 @@ Refresh Params + + Ctrl+R + diff --git a/src/ui/configuration/ArduPlanePidConfig.ui b/src/ui/configuration/ArduPlanePidConfig.ui index 0fd306aa47..3715d32063 100644 --- a/src/ui/configuration/ArduPlanePidConfig.ui +++ b/src/ui/configuration/ArduPlanePidConfig.ui @@ -949,6 +949,9 @@ Write Params + + Ctrl+W + @@ -980,6 +983,9 @@ Refresh Params + + Ctrl+R + diff --git a/src/ui/configuration/ArduRoverPidConfig.ui b/src/ui/configuration/ArduRoverPidConfig.ui index d4ae2ad650..6d53bd19f9 100644 --- a/src/ui/configuration/ArduRoverPidConfig.ui +++ b/src/ui/configuration/ArduRoverPidConfig.ui @@ -713,6 +713,9 @@ Refresh Params + + Ctrl+R + @@ -720,6 +723,9 @@ Write Params + + Ctrl+W + diff --git a/src/ui/configuration/CopterPidConfig.cc b/src/ui/configuration/CopterPidConfig.cc new file mode 100644 index 0000000000..d6a4d04e25 --- /dev/null +++ b/src/ui/configuration/CopterPidConfig.cc @@ -0,0 +1,351 @@ +/*=================================================================== +APM_PLANNER Open Source Ground Control Station + +(c) 2013 APM_PLANNER PROJECT + +This file is part of the APM_PLANNER project + + APM_PLANNER 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. + + APM_PLANNER 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 APM_PLANNER. If not, see . + +======================================================================*/ + +#include "CopterPidConfig.h" + +#include "QGCCore.h" + +CopterPidConfig::CopterPidConfig(QWidget *parent) : AP2ConfigWidget(parent) +{ + ui.setupUi(this); + + QList widgetList = this->findChildren(); + for (int i=0;i(widgetList[i]) || qobject_cast(widgetList[i]) || qobject_cast(widgetList[i])) + { + widgetList[i]->installEventFilter(QGCMouseWheelEventFilter::getFilter()); + } + } + + m_pitchRollLocked = false; + connect(ui.checkBox,SIGNAL(clicked(bool)),this,SLOT(lockCheckBoxClicked(bool))); + connect(ui.stabilPitchPSpinBox,SIGNAL(valueChanged(double)),this,SLOT(stabilLockedChanged(double))); + connect(ui.stabilRollPSpinBox,SIGNAL(valueChanged(double)),this,SLOT(stabilLockedChanged(double))); + connect(ui.ratePitchPSpinBox,SIGNAL(valueChanged(double)),this,SLOT(ratePChanged(double))); + connect(ui.ratePitchISpinBox,SIGNAL(valueChanged(double)),this,SLOT(rateIChanged(double))); + connect(ui.ratePitchDSpinBox,SIGNAL(valueChanged(double)),this,SLOT(rateDChanged(double))); + connect(ui.ratePitchIMAXSpinBox,SIGNAL(valueChanged(double)),this,SLOT(rateIMAXChanged(double))); + + connect(ui.rateRollPSpinBox,SIGNAL(valueChanged(double)),this,SLOT(ratePChanged(double))); + connect(ui.rateRollISpinBox,SIGNAL(valueChanged(double)),this,SLOT(rateIChanged(double))); + connect(ui.rateRollDSpinBox,SIGNAL(valueChanged(double)),this,SLOT(rateDChanged(double))); + connect(ui.rateRollIMAXSpinBox,SIGNAL(valueChanged(double)),this,SLOT(rateIMAXChanged(double))); + + + m_nameToBoxMap["STB_RLL_P"] = ui.stabilRollPSpinBox; + m_nameToBoxMap["STB_PIT_P"] = ui.stabilPitchPSpinBox; + m_nameToBoxMap["STB_YAW_P"] = ui.stabilYawPSpinBox; + m_nameToBoxMap["POS_XY_P"] = ui.posXYPSpinBox; + + m_nameToBoxMap["RATE_RLL_P"] = ui.rateRollPSpinBox; + m_nameToBoxMap["RATE_RLL_I"] = ui.rateRollISpinBox; + m_nameToBoxMap["RATE_RLL_D"] = ui.rateRollDSpinBox; + m_nameToBoxMap["RATE_RLL_FILT_HZ"] = ui.rateRollFiltHzSpinBox; + m_nameToBoxMap["RATE_RLL_IMAX"] = ui.rateRollIMAXSpinBox; + + m_nameToBoxMap["RATE_PIT_P"] = ui.ratePitchPSpinBox; + m_nameToBoxMap["RATE_PIT_I"] = ui.ratePitchISpinBox; + m_nameToBoxMap["RATE_PIT_D"] = ui.ratePitchDSpinBox; + m_nameToBoxMap["RATE_PIT_FILT_HZ"] = ui.ratePitchFiltHzSpinBox; + m_nameToBoxMap["RATE_PIT_IMAX"] = ui.ratePitchIMAXSpinBox; + + m_nameToBoxMap["RATE_YAW_P"] = ui.rateYawPSpinBox; + m_nameToBoxMap["RATE_YAW_I"] = ui.rateYawISpinBox; + m_nameToBoxMap["RATE_YAW_D"] = ui.rateYawDSpinBox; + m_nameToBoxMap["RATE_YAW_FILT_HZ"] = ui.rateYawFiltHzSpinBox; + m_nameToBoxMap["RATE_YAW_IMAX"] = ui.rateYawIMAXSpinBox; + + m_nameToBoxMap["VEL_XY_P"] = ui.velXYPSpinBox; + m_nameToBoxMap["VEL_XY_I"] = ui.velXYISpinBox; + m_nameToBoxMap["VEL_XY_IMAX"] = ui.velXYIMAXSpinBox; + m_nameToBoxMap["VEL_XY_FILT_HZ"] = ui.velXYFiltHzSpinBox; + + m_nameToBoxMap["ACCEL_Z_P"] = ui.accelZPSpinBox; + m_nameToBoxMap["ACCEL_Z_I"] = ui.accelZISpinBox; + m_nameToBoxMap["ACCEL_Z_IMAX"] = ui.accelZIMAXSpinBox; + m_nameToBoxMap["ACCEL_Z_FILT_HZ"] = ui.accelZFiltHzSpinBox; + + m_nameToBoxMap["VEL_Z_P"] = ui.velZPSpinBox; + + m_nameToBoxMap["POS_Z_P"] = ui.posZPSpinBox; + + m_nameToBoxMap["POS_XY_P"] = ui.posXYPSpinBox; + + m_nameToBoxMap["WPNAV_SPEED"] = ui.wpNavSpeedSpinBox; + m_nameToBoxMap["WPNAV_RADIUS"] = ui.wpNavRadiusSpinBox; + m_nameToBoxMap["WPNAV_SPEED_DN"] = ui.wpNavSpeedDownSpinBox; + m_nameToBoxMap["WPNAV_LOIT_SPEED"] = ui.wpNavLoiterSpeedSpinBox; + m_nameToBoxMap["WPNAV_SPEED_UP"] = ui.wpNavSpeedUpSpinBox; + + //m_nameToBoxMap["TUNE_HIGH"] = ui.ch6MaxSpinBox; + // m_nameToBoxMap["TUNE_LOW"] = ui.ch6MinSpinBox; + + connect(ui.writePushButton,SIGNAL(clicked()),this,SLOT(writeButtonClicked())); + connect(ui.refreshPushButton,SIGNAL(clicked()),this,SLOT(refreshButtonClicked())); + + m_ch6ValueToTextList.append(QPair(0,"CH6_NONE")); + m_ch6ValueToTextList.append(QPair(1,"CH6_STABILIZE_KP")); + m_ch6ValueToTextList.append(QPair(2,"CH6_STABILIZE_KI")); + m_ch6ValueToTextList.append(QPair(3,"CH6_YAW_KP")); + m_ch6ValueToTextList.append(QPair(24,"CH6_YAW_KI")); + m_ch6ValueToTextList.append(QPair(4,"CH6_RATE_KP")); + m_ch6ValueToTextList.append(QPair(5,"CH6_RATE_KI")); + m_ch6ValueToTextList.append(QPair(6,"CH6_YAW_RATE_KP")); + m_ch6ValueToTextList.append(QPair(26,"CH6_YAW_RATE_KD")); + m_ch6ValueToTextList.append(QPair(7,"CH6_THROTTLE_KP")); + m_ch6ValueToTextList.append(QPair(9,"CH6_RELAY")); + m_ch6ValueToTextList.append(QPair(10,"CH6_WP_SPEED")); + m_ch6ValueToTextList.append(QPair(12,"CH6_LOITER_KP")); + m_ch6ValueToTextList.append(QPair(13,"CH6_HELI_EXTERNAL_GYRO")); + m_ch6ValueToTextList.append(QPair(14,"CH6_THR_HOLD_KP")); + m_ch6ValueToTextList.append(QPair(17,"CH6_OPTFLOW_KP")); + m_ch6ValueToTextList.append(QPair(18,"CH6_OPTFLOW_KI")); + m_ch6ValueToTextList.append(QPair(19,"CH6_OPTFLOW_KD")); + m_ch6ValueToTextList.append(QPair(21,"CH6_RATE_KD")); + m_ch6ValueToTextList.append(QPair(22,"CH6_LOITER_RATE_KP")); + m_ch6ValueToTextList.append(QPair(23,"CH6_LOITER_RATE_KD")); + m_ch6ValueToTextList.append(QPair(25,"CH6_ACRO_KP")); + m_ch6ValueToTextList.append(QPair(27,"CH6_LOITER_KI")); + m_ch6ValueToTextList.append(QPair(28,"CH6_LOITER_RATE_KI")); + m_ch6ValueToTextList.append(QPair(29,"CH6_STABILIZE_KD")); + m_ch6ValueToTextList.append(QPair(30,"CH6_AHRS_YAW_KP")); + m_ch6ValueToTextList.append(QPair(31,"CH6_AHRS_KP")); + m_ch6ValueToTextList.append(QPair(32,"CH6_INAV_TC")); + m_ch6ValueToTextList.append(QPair(33,"CH6_THROTTLE_KI")); + m_ch6ValueToTextList.append(QPair(34,"CH6_ACCEL_Z_KP")); + m_ch6ValueToTextList.append(QPair(35,"CH6_ACCEL_Z_KI")); + m_ch6ValueToTextList.append(QPair(36,"CH6_ACCEL_Z_KD")); + m_ch6ValueToTextList.append(QPair(38,"CH6_DECLINATION")); + m_ch6ValueToTextList.append(QPair(39,"CH6_CIRCLE_RATE")); + m_ch6ValueToTextList.append(QPair(41,"CH6_SONAR_GAIN")); + m_ch6ValueToTextList.append(QPair(42,"CH6_EKF_VERTICAL_POS")); + m_ch6ValueToTextList.append(QPair(43,"CH6_EKF_HORIZONTAL_POS")); + m_ch6ValueToTextList.append(QPair(44,"CH6_EKF_ACCEL_NOISE")); + m_ch6ValueToTextList.append(QPair(45,"CH6_RC_FEEL_RP")); + m_ch6ValueToTextList.append(QPair(46,"CH6_RATE_PITCH_KP")); + m_ch6ValueToTextList.append(QPair(47,"CH6_RATE_PITCH_KI")); + m_ch6ValueToTextList.append(QPair(48,"CH6_RATE_PITCH_KD")); + m_ch6ValueToTextList.append(QPair(49,"CH6_RATE_ROLL_KP")); + m_ch6ValueToTextList.append(QPair(50,"CH6_RATE_ROLL_KI")); + m_ch6ValueToTextList.append(QPair(51,"CH6_RATE_ROLL_KD")); + m_ch6ValueToTextList.append(QPair(52,"CH6_RATE_PITCH_FF")); + m_ch6ValueToTextList.append(QPair(53,"CH6_RATE_ROLL_FF")); + m_ch6ValueToTextList.append(QPair(54,"CH6_RATE_YAW_FF")); + m_ch6ValueToTextList.append(QPair(55,"CH6_RATE_MOT_YAW_HEADROOM")); + m_ch6ValueToTextList.append(QPair(56,"CH6_RATE_YAW_FILT")); + + for (int i=0;iaddItem(m_ch6ValueToTextList[i].second); + } + + m_ch78ValueToTextList.append(QPair(0,"Do nothing")); + m_ch78ValueToTextList.append(QPair(2,"Flip")); + m_ch78ValueToTextList.append(QPair(3,"Simple mode")); + m_ch78ValueToTextList.append(QPair(4,"RTL")); + m_ch78ValueToTextList.append(QPair(5,"Save Trim")); + m_ch78ValueToTextList.append(QPair(7,"Save WP")); + m_ch78ValueToTextList.append(QPair(8,"Multi Mode")); + m_ch78ValueToTextList.append(QPair(9,"Camera Trigger")); + m_ch78ValueToTextList.append(QPair(10,"Sonar")); + m_ch78ValueToTextList.append(QPair(11,"Fence")); + m_ch78ValueToTextList.append(QPair(12,"ResetToArmedYaw")); + m_ch78ValueToTextList.append(QPair(13,"Super Simple Mode")); + m_ch78ValueToTextList.append(QPair(14,"Acro Trainer")); + m_ch78ValueToTextList.append(QPair(15,"Sprayer")); + m_ch78ValueToTextList.append(QPair(16,"Auto")); + m_ch78ValueToTextList.append(QPair(17,"Auto Tune")); + m_ch78ValueToTextList.append(QPair(18,"Land")); + m_ch78ValueToTextList.append(QPair(19,"EPM Cargo Gripper")); + m_ch78ValueToTextList.append(QPair(20,"Enable NavEKF")); + m_ch78ValueToTextList.append(QPair(21,"Parachute Enable")); + m_ch78ValueToTextList.append(QPair(22,"Parachute Release")); + m_ch78ValueToTextList.append(QPair(23,"Parachute 3POS switch")); + m_ch78ValueToTextList.append(QPair(24,"Mission Reset")); + m_ch78ValueToTextList.append(QPair(25,"Roll/Pitch FF enable")); + m_ch78ValueToTextList.append(QPair(26,"Accel Limiting Enable")); + m_ch78ValueToTextList.append(QPair(27,"Retract Mount")); + m_ch78ValueToTextList.append(QPair(28,"Relay Pin on/off")); + m_ch78ValueToTextList.append(QPair(29,"Landing Gear Ctrl")); + m_ch78ValueToTextList.append(QPair(30,"Lost Copter Sound")); + m_ch78ValueToTextList.append(QPair(31,"EStop Switch")); + m_ch78ValueToTextList.append(QPair(32,"Motor Interlock on/off")); + m_ch78ValueToTextList.append(QPair(33,"Brake Mode")); + + for (int i=0;iaddItem(m_ch78ValueToTextList[i].second); + ui.ch8OptComboBox->addItem(m_ch78ValueToTextList[i].second); + } + + initConnections(); +} +void CopterPidConfig::lockCheckBoxClicked(bool checked) +{ + m_pitchRollLocked = checked; +} +void CopterPidConfig::stabilLockedChanged(double value) +{ + if (m_pitchRollLocked) + { + disconnect(ui.stabilPitchPSpinBox,SIGNAL(valueChanged(double)),this,SLOT(stabilLockedChanged(double))); + disconnect(ui.stabilRollPSpinBox,SIGNAL(valueChanged(double)),this,SLOT(stabilLockedChanged(double))); + ui.stabilPitchPSpinBox->setValue(value); + ui.stabilRollPSpinBox->setValue(value); + connect(ui.stabilPitchPSpinBox,SIGNAL(valueChanged(double)),this,SLOT(stabilLockedChanged(double))); + connect(ui.stabilRollPSpinBox,SIGNAL(valueChanged(double)),this,SLOT(stabilLockedChanged(double))); + } +} +void CopterPidConfig::ratePChanged(double value) +{ + if (m_pitchRollLocked) + { + disconnect(ui.ratePitchPSpinBox,SIGNAL(valueChanged(double)),this,SLOT(ratePChanged(double))); + disconnect(ui.rateRollPSpinBox,SIGNAL(valueChanged(double)),this,SLOT(ratePChanged(double))); + ui.ratePitchPSpinBox->setValue(value); + ui.rateRollPSpinBox->setValue(value); + connect(ui.ratePitchPSpinBox,SIGNAL(valueChanged(double)),this,SLOT(ratePChanged(double))); + connect(ui.rateRollPSpinBox,SIGNAL(valueChanged(double)),this,SLOT(ratePChanged(double))); + } +} +void CopterPidConfig::rateIChanged(double value) +{ + if (m_pitchRollLocked) + { + disconnect(ui.ratePitchISpinBox,SIGNAL(valueChanged(double)),this,SLOT(rateIChanged(double))); + disconnect(ui.rateRollISpinBox,SIGNAL(valueChanged(double)),this,SLOT(rateIChanged(double))); + ui.ratePitchISpinBox->setValue(value); + ui.rateRollISpinBox->setValue(value); + connect(ui.ratePitchISpinBox,SIGNAL(valueChanged(double)),this,SLOT(rateIChanged(double))); + connect(ui.rateRollISpinBox,SIGNAL(valueChanged(double)),this,SLOT(rateIChanged(double))); + } +} +void CopterPidConfig::rateDChanged(double value) +{ + if (m_pitchRollLocked) + { + disconnect(ui.ratePitchDSpinBox,SIGNAL(valueChanged(double)),this,SLOT(rateDChanged(double))); + disconnect(ui.rateRollDSpinBox,SIGNAL(valueChanged(double)),this,SLOT(rateDChanged(double))); + ui.ratePitchDSpinBox->setValue(value); + ui.rateRollDSpinBox->setValue(value); + connect(ui.ratePitchDSpinBox,SIGNAL(valueChanged(double)),this,SLOT(rateDChanged(double))); + connect(ui.rateRollDSpinBox,SIGNAL(valueChanged(double)),this,SLOT(rateDChanged(double))); + } +} +void CopterPidConfig::rateIMAXChanged(double value) +{ + if (m_pitchRollLocked) + { + disconnect(ui.ratePitchIMAXSpinBox,SIGNAL(valueChanged(double)),this,SLOT(rateIMAXChanged(double))); + disconnect(ui.rateRollIMAXSpinBox,SIGNAL(valueChanged(double)),this,SLOT(rateIMAXChanged(double))); + ui.ratePitchIMAXSpinBox->setValue(value); + ui.rateRollIMAXSpinBox->setValue(value); + connect(ui.ratePitchIMAXSpinBox,SIGNAL(valueChanged(double)),this,SLOT(rateIMAXChanged(double))); + connect(ui.rateRollIMAXSpinBox,SIGNAL(valueChanged(double)),this,SLOT(rateIMAXChanged(double))); + } +} + +CopterPidConfig::~CopterPidConfig() +{ +} +void CopterPidConfig::parameterChanged(int uas, int component, QString parameterName, QVariant value) +{ + if (m_nameToBoxMap.contains(parameterName)) + { + m_nameToBoxMap[parameterName]->setValue(value.toDouble()); + } + else if (parameterName == "TUNE_HIGH") + { + ui.ch6MaxSpinBox->setValue(value.toDouble() / 1000.0); + } + else if (parameterName == "TUNE_LOW") + { + ui.ch6MinSpinBox->setValue(value.toDouble() / 1000.0); + } + else if (parameterName == "TUNE") + { + for (int i=0;isetCurrentIndex(i); + } + } + } + else if (parameterName == "CH7_OPT") + { + for (int i=0;isetCurrentIndex(i); + } + } + } + else if (parameterName == "CH8_OPT") + { + for (int i=0;isetCurrentIndex(i); + } + } + } +} +void CopterPidConfig::writeButtonClicked() +{ + if (!m_uas) + { + showNullMAVErrorMessageBox(); + return; + } + for (QMap::const_iterator i=m_nameToBoxMap.constBegin();i!=m_nameToBoxMap.constEnd();i++) + { + m_uas->getParamManager()->setParameter(1,i.key(),i.value()->value()); + } + + m_uas->getParamManager()->setParameter(1,"TUNE",m_ch6ValueToTextList[ui.ch6OptComboBox->currentIndex()].first); + m_uas->getParamManager()->setParameter(1,"CH7_OPT",m_ch78ValueToTextList[ui.ch7OptComboBox->currentIndex()].first); + m_uas->getParamManager()->setParameter(1,"CH8_OPT",m_ch78ValueToTextList[ui.ch8OptComboBox->currentIndex()].first); + m_uas->getParamManager()->setParameter(1,"TUNE_HIGH",ui.ch6MaxSpinBox->value() * 1000.0); + m_uas->getParamManager()->setParameter(1,"TUNE_LOW",ui.ch6MinSpinBox->value() * 1000.0); +} + +void CopterPidConfig::refreshButtonClicked() +{ + if (!m_uas) + { + showNullMAVErrorMessageBox(); + return; + } + for (QMap::const_iterator i=m_nameToBoxMap.constBegin();i!=m_nameToBoxMap.constEnd();i++) + { + m_uas->getParamManager()->requestParameterUpdate(1,i.key()); + } + m_uas->getParamManager()->requestParameterUpdate(1,"TUNE"); + m_uas->getParamManager()->requestParameterUpdate(1,"CH7_OPT"); + m_uas->getParamManager()->requestParameterUpdate(1,"CH8_OPT"); + m_uas->getParamManager()->requestParameterUpdate(1,"TUNE_HIGH"); + m_uas->getParamManager()->requestParameterUpdate(1,"TUNE_LOW"); +} diff --git a/src/ui/configuration/CopterPidConfig.h b/src/ui/configuration/CopterPidConfig.h new file mode 100644 index 0000000000..6ca41c88d6 --- /dev/null +++ b/src/ui/configuration/CopterPidConfig.h @@ -0,0 +1,65 @@ +/*=================================================================== +APM_PLANNER Open Source Ground Control Station + +(c) 2013 APM_PLANNER PROJECT + +This file is part of the APM_PLANNER project + + APM_PLANNER 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. + + APM_PLANNER 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 APM_PLANNER. If not, see . + +======================================================================*/ + +/** + * @file + * @brief PID configuration for ArduCoptor (multi-rotor air vehicle) + * for version Copter3.3+ + * + * @author Michael Carpenter + * @author Bill Bonney Carpenter + */ + +#ifndef COPTERPIDCONFIG_H +#define COPTERPIDCONFIG_H + +#include +#include "ui_CopterPidConfig.h" + +#include "AP2ConfigWidget.h" + +class CopterPidConfig : public AP2ConfigWidget +{ + Q_OBJECT + +public: + explicit CopterPidConfig(QWidget *parent = 0); + ~CopterPidConfig(); +private slots: + void writeButtonClicked(); + void refreshButtonClicked(); + void parameterChanged(int uas, int component, QString parameterName, QVariant value); + void lockCheckBoxClicked(bool checked); + void stabilLockedChanged(double value); + void ratePChanged(double value); + void rateIChanged(double value); + void rateDChanged(double value); + void rateIMAXChanged(double value); +private: + bool m_pitchRollLocked; + QList > m_ch6ValueToTextList; + QList > m_ch78ValueToTextList; + QMap m_nameToBoxMap; + Ui::CopterPidConfig ui; +}; + +#endif // COPTERPIDCONFIG_H diff --git a/src/ui/configuration/CopterPidConfig.ui b/src/ui/configuration/CopterPidConfig.ui new file mode 100644 index 0000000000..1624a5d58c --- /dev/null +++ b/src/ui/configuration/CopterPidConfig.ui @@ -0,0 +1,1770 @@ + + + CopterPidConfig + + + + 0 + 0 + 677 + 575 + + + + + 0 + 0 + + + + + 200 + 0 + + + + + 721 + 16777215 + + + + Form + + + + + + + + <html><head/><body><p><span style=" font-size:x-large; font-weight:600;">Extended Tuning</span></p></body></html> + + + + + + + QLayout::SetFixedSize + + + + + + + + 0 + 0 + + + + + 150 + 0 + + + + + 150 + 16777215 + + + + Vertical Position + + + + 2 + + + 2 + + + 2 + + + 2 + + + + + QLayout::SetMinimumSize + + + + + + + + 0 + 0 + + + + P + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + + + + 4 + + + -10000.000000000000000 + + + 10000.000000000000000 + + + 0.000100000000000 + + + + + + + + + + + + + + + 0 + 0 + + + + + 150 + 0 + + + + + 150 + 16777215 + + + + Vertical Velocity + + + + 2 + + + 2 + + + 2 + + + 2 + + + + + + + + + P + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + + + + 4 + + + -10000.000000000000000 + + + 10000.000000000000000 + + + 0.000100000000000 + + + + + + + + + + + + + + + + + 0 + 15 + + + + + 16777215 + 15 + + + + Lock Pitch and Roll Values + + + false + + + + + + + Write Params + + + Ctrl+W + + + + + + + + 0 + 0 + + + + + 150 + 0 + + + + + 150 + 16777215 + + + + Rate Roll + + + + 2 + + + 2 + + + 2 + + + 2 + + + + + + + + + + 0 + 0 + + + + P + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 0 + 0 + + + + I + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 0 + 0 + + + + D + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 0 + 0 + + + + + 0 + 0 + + + + I_MAX + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + Filter + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + + + + 4 + + + -10000.000000000000000 + + + 10000.000000000000000 + + + 0.000100000000000 + + + + + + + 4 + + + -10000.000000000000000 + + + 10000.000000000000000 + + + 0.000100000000000 + + + + + + + 4 + + + -10000.000000000000000 + + + 10000.000000000000000 + + + 0.000100000000000 + + + + + + + -10000.000000000000000 + + + 10000.000000000000000 + + + + + + + Hz + + + 0 + + + 1.000000000000000 + + + + + + + + + + + + + + Refresh Params + + + Ctrl+R + + + + + + + + 0 + 0 + + + + + 150 + 0 + + + + + 150 + 16777215 + + + + Rate Yaw + + + + 2 + + + 2 + + + 2 + + + 2 + + + + + + + + + P + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + I + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + D + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + I_MAX + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + Filter + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + + + + 4 + + + -10000.000000000000000 + + + 10000.000000000000000 + + + 0.000100000000000 + + + + + + + 4 + + + -10000.000000000000000 + + + 10000.000000000000000 + + + 0.000100000000000 + + + + + + + 4 + + + -10000.000000000000000 + + + 10000.000000000000000 + + + 0.000100000000000 + + + + + + + -10000.000000000000000 + + + 10000.000000000000000 + + + + + + + Hz + + + 0 + + + 1.000000000000000 + + + + + + + + + + + + + + + + + + + 0 + 0 + + + + Ch6 Opt + + + + + + + + 0 + 0 + + + + + 0 + 0 + + + + + 200 + 22 + + + + + + + + + + + + + 0 + 0 + + + + Min + + + + + + + + 200 + 25 + + + + 4 + + + -10000.000000000000000 + + + 10000.000000000000000 + + + 0.000100000000000 + + + + + + + + + + + + 0 + 0 + + + + Max + + + + + + + + 200 + 25 + + + + 4 + + + -10000.000000000000000 + + + 10000.000000000000000 + + + + + + + + + + + + 0 + 0 + + + + Ch7 Opt + + + + + + + + 0 + 0 + + + + + 0 + 0 + + + + + 200 + 22 + + + + + + + + + + + + + 0 + 0 + + + + Ch8 Opt + + + + + + + + 0 + 0 + + + + + 0 + 0 + + + + + 200 + 22 + + + + + + + + + + + + + 0 + 0 + + + + + 150 + 200 + + + + Vertical Acceleration + + + + 2 + + + 2 + + + 2 + + + 2 + + + + + + + + + P + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + I + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + I_MAX + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + Filter + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + + + + 4 + + + -10000.000000000000000 + + + 10000.000000000000000 + + + 0.000100000000000 + + + + + + + 4 + + + -10000.000000000000000 + + + 10000.000000000000000 + + + 0.000100000000000 + + + + + + + 4 + + + -10000.000000000000000 + + + 10000.000000000000000 + + + + + + + Hz + + + 0 + + + 1.000000000000000 + + + + + + + + + + + + + + + 0 + 0 + + + + + 150 + 70 + + + + + 150 + 16777215 + + + + Stabilize Pitch + + + + 2 + + + 2 + + + 2 + + + 2 + + + + + + + + + P + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + + + + + 0 + 0 + + + + 4 + + + 100.000000000000000 + + + 0.000100000000000 + + + + + + + + + + + + + + Horizontal Position + + + + 2 + + + 2 + + + 2 + + + 2 + + + + + + 40 + 0 + + + + P + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + 4 + + + + + + + + + + + 0 + 0 + + + + + 150 + 70 + + + + + 150 + 16777215 + + + + Stabilize Yaw + + + + 2 + + + 2 + + + 2 + + + 2 + + + + + + + + + P + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + + + + 4 + + + -10000.000000000000000 + + + 10000.000000000000000 + + + 0.000100000000000 + + + + + + + + + + + + + + + 0 + 0 + + + + + 150 + 0 + + + + + 150 + 16777215 + + + + Horizontal Velocity + + + + + + + + + + P + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + I + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + I_MAX + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + Filter + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + + + + 4 + + + -10000.000000000000000 + + + 10000.000000000000000 + + + 0.000100000000000 + + + + + + + 4 + + + -10000.000000000000000 + + + 10000.000000000000000 + + + 0.000100000000000 + + + + + + + 4 + + + -10000.000000000000000 + + + 10000.000000000000000 + + + + + + + Hz + + + 0 + + + 1.000000000000000 + + + + + + + + + + + + + + + 0 + 0 + + + + + 150 + 0 + + + + + 150 + 16777215 + + + + Rate Pitch + + + + 2 + + + 2 + + + 2 + + + 2 + + + + + + + + + P + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + I + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + D + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + I_MAX + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + Filter + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + + + + 4 + + + -10000.000000000000000 + + + 10000.000000000000000 + + + 0.000100000000000 + + + + + + + 4 + + + -10000.000000000000000 + + + 10000.000000000000000 + + + 0.000100000000000 + + + + + + + 4 + + + -10000.000000000000000 + + + 10000.000000000000000 + + + 0.000100000000000 + + + + + + + -10000.000000000000000 + + + 10000.000000000000000 + + + + + + + Hz + + + 0 + + + 1.000000000000000 + + + + + + + + + + + + + + + 0 + 0 + + + + + 150 + 70 + + + + + 150 + 16777215 + + + + Stabilize Roll + + + + 2 + + + 2 + + + 2 + + + 2 + + + + + QLayout::SetMinimumSize + + + + + + + + 0 + 25 + + + + + 16777215 + 25 + + + + P + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + + + + + 0 + 25 + + + + + 16777215 + 25 + + + + 4 + + + -10000.000000000000000 + + + 10000.000000000000000 + + + 0.000100000000000 + + + + + + + + + + + + + + + 0 + 0 + + + + + 150 + 0 + + + + + 150 + 16777215 + + + + WPNav (cm's) + + + + 2 + + + 2 + + + 2 + + + 2 + + + + + + + + + Speed + + + + + + + Radius + + + + + + + Speed Up + + + + + + + speed Down + + + + + + + Loiter Speed + + + + + + + + + + + 0 + + + -10000.000000000000000 + + + 10000.000000000000000 + + + + + + + 0 + + + -10000.000000000000000 + + + 10000.000000000000000 + + + + + + + 0 + + + -10000.000000000000000 + + + 10000.000000000000000 + + + + + + + 0 + + + -10000.000000000000000 + + + 10000.000000000000000 + + + + + + + 0 + + + -10000.000000000000000 + + + 10000.000000000000000 + + + + + + + + + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + diff --git a/src/ui/configuration/FlightModeConfig.ui b/src/ui/configuration/FlightModeConfig.ui index 0529a86eb4..ea834d23cc 100644 --- a/src/ui/configuration/FlightModeConfig.ui +++ b/src/ui/configuration/FlightModeConfig.ui @@ -298,7 +298,10 @@ - Save + Write + + + Ctrl+W diff --git a/src/ui/configuration/PX4FirmwareUploader.cc b/src/ui/configuration/PX4FirmwareUploader.cc index 6f52da8346..4d60e4b10c 100644 --- a/src/ui/configuration/PX4FirmwareUploader.cc +++ b/src/ui/configuration/PX4FirmwareUploader.cc @@ -284,7 +284,15 @@ void PX4FirmwareUploader::kickOffTriggered() m_currentState = INIT; m_port = new QSerialPort(); connect(m_port,SIGNAL(readyRead()),this,SLOT(portReadyRead())); + +#ifdef Q_OS_MACX + // temp fix Qt5.4.1 issue on OSX + // http://code.qt.io/cgit/qt/qtserialport.git/commit/?id=687dfa9312c1ef4894c32a1966b8ac968110b71e + m_port->setPortName("/dev/cu." + m_portToUse); +#else m_port->setPortName(m_portToUse); +#endif + if (!m_port->open(QIODevice::ReadWrite)) { QLOG_ERROR() << "Unable to open port:" << m_port->errorString(); @@ -594,6 +602,8 @@ bool PX4FirmwareUploader::verifyOtp() publicKeyList << "\r\nMIGfMA0GCSqGSIb3DQEBAQUAA4GNADCBiQKBgQDqi8E6EdZ11iE7nAc95bjdUTwd\r\n/gLetSAAx8X9jgjInz5j47DIcDqFVFKEFZWiAc3AxJE/fNrPQey16SfI0FyDAX/U\t\n4jyGIv9w+M1dKgUPI8UdpEMS2w1YnfzW0GO3PX0SBL6pctEIdXr0NGsFFaqU9Yz4\r\nDbgBdR6wBz9qdfRRoQIDAQAB"; // 3DR Public Key 2 publicKeyList << "\r\nMIGfMA0GCSqGSIb3DQEBAQUAA4GNADCBiQKBgQCei8vGvc+jPXfbAAUkiu7o9fiQ\r\nhQ6/xdJcrmsJqa2/Onf4xDk6mMoZaNXNaNsEmE/xdeYgLqbPoivCba7A0YiGzonp\r\nOiEZlfUKJPzXvGSNrKbIDsOrvhPQpFwBEU+hO5usHoWCO5VzN5+wKTpGVZ300ny4\r\nNHbIGjZUF/RUz84lVwIDAQAB"; + // Arsov RC Technology USA + publicKeyList << "\r\nMIGfMA0GCSqGSIb3DQEBAQUAA4GNADCBiQKBgQDJQfXiYHrYIHGadPwDPkUuHnQG\r\nubN4x0UAyvhP1VGzAk/CNYkcSCEKVGbi+Ro3l60xpoiOIR5HfHtzGgzdl+oBu55t\r\n2xjvbJbGIPKkA8pBU10Nj2dg/vjNphWMQpSw2yRdHuqvnrGjOq2fLduS0ITp11ST\r\nRX6cRfeDDXsVkL+XzwIDAQAB"; bool checkCOAsuccess = false; foreach(QString publicKey, publicKeyList){ diff --git a/src/ui/configuration/ParamCompareDialog.cpp b/src/ui/configuration/ParamCompareDialog.cpp index 0062779a82..2d9b668cd7 100644 --- a/src/ui/configuration/ParamCompareDialog.cpp +++ b/src/ui/configuration/ParamCompareDialog.cpp @@ -157,7 +157,7 @@ void ParamCompareDialog::loadParameterFile(const QString &filename) void ParamCompareDialog::populateParamListFromString(QString paramString, QMap* list, QWidget* widget = NULL) { - QStringList paramSplit = paramString.split("\n"); + QStringList paramSplit = paramString.split(QGC::paramLineSplitRegExp()); bool summaryComplete = false; bool summaryShown = false; QString summaryText; @@ -165,7 +165,7 @@ void ParamCompareDialog::populateParamListFromString(QString paramString, QMapstart(RADIO3DR_UPDATE_PORT_TIME); } Radio3DRConfig::~Radio3DRConfig() @@ -87,9 +88,12 @@ void Radio3DRConfig::addBaudComboBoxConfig(QComboBox *comboBox) comboBox->setCurrentIndex(2); } -void Radio3DRConfig::fillPortsInfo(QComboBox &comboxBox) +void Radio3DRConfig::fillPortsInfo(QComboBox &comboBox) { QLOG_TRACE() << "3DR Radio fillPortsInfo "; + QString current = comboBox.itemText(comboBox.currentIndex()); + disconnect(&comboBox,SIGNAL(currentIndexChanged(int)),this,SLOT(setLink(int))); + comboBox.clear(); foreach (const QSerialPortInfo &info, QSerialPortInfo::availablePorts()) { QStringList list; list << info.portName() @@ -99,14 +103,25 @@ void Radio3DRConfig::fillPortsInfo(QComboBox &comboxBox) << (info.vendorIdentifier() ? QString::number(info.vendorIdentifier(), 16) : QString()) << (info.productIdentifier() ? QString::number(info.productIdentifier(), 16) : QString()); - int found = comboxBox.findData(list); + int found = comboBox.findData(list); if ((found == -1)&& (info.manufacturer().contains("FTDI") || info.manufacturer().contains("Silicon Labs"))) { QLOG_INFO() << "Inserting " << list.first(); - comboxBox.insertItem(0,list[0], list); + comboBox.insertItem(0,list[0], list); } else { // Do nothing as the port is already listed } - }} + } + for (int i=0;istart(2000); + m_timer->start(RADIO3DR_UPDATE_PORT_TIME); loadSavedSerialSettings(); MainWindow::instance()->toolBar().disableConnectWidget(true); diff --git a/src/ui/configuration/Radio3DRConfig.h b/src/ui/configuration/Radio3DRConfig.h index 4701586d90..9df4ac244c 100644 --- a/src/ui/configuration/Radio3DRConfig.h +++ b/src/ui/configuration/Radio3DRConfig.h @@ -37,6 +37,7 @@ class Radio3DRSettings; class Radio3DRConfig : public QWidget { Q_OBJECT + static const int RADIO3DR_UPDATE_PORT_TIME = 3000; enum State { none, writeRadioSettings, resetRadioSettings, complete}; @@ -82,7 +83,7 @@ private slots: void resetUI(); void initConnections(); void addBaudComboBoxConfig(QComboBox *comboBox); - void fillPortsInfo(QComboBox &comboxBox); + void fillPortsInfo(QComboBox &comboBox); void addRadioBaudComboBoxConfig(QComboBox &comboBox); void addRadioAirBaudComboBoxConfig(QComboBox &comboBox); void addTxPowerComboBoxConfig(QComboBox &comboBox); diff --git a/src/ui/configuration/Radio3DRSettings.cc b/src/ui/configuration/Radio3DRSettings.cc index e2a75bcba9..c688964b68 100644 --- a/src/ui/configuration/Radio3DRSettings.cc +++ b/src/ui/configuration/Radio3DRSettings.cc @@ -311,7 +311,13 @@ bool Radio3DRSettings::openSerialPort(SerialSettings settings) m_serialPort->close(); +#ifdef Q_OS_MACX + // temp fix Qt5.4.1 issue on OSX + // http://code.qt.io/cgit/qt/qtserialport.git/commit/?id=687dfa9312c1ef4894c32a1966b8ac968110b71e + m_serialPort->setPortName("/dev/cu." + settings.name); +#else m_serialPort->setPortName(settings.name); +#endif if(m_serialPort->open(QIODevice::ReadWrite)){ // Start Reading the settings diff --git a/src/ui/configuration/RadioCalibrationConfig.cc b/src/ui/configuration/RadioCalibrationConfig.cc index 2bd2a0a98f..1c131abefe 100644 --- a/src/ui/configuration/RadioCalibrationConfig.cc +++ b/src/ui/configuration/RadioCalibrationConfig.cc @@ -521,25 +521,59 @@ void RadioCalibrationConfig::writeSettings() bool RadioCalibrationConfig::isRadioControlActive() { - for(int count=0; count< RC_CHANNEL_NUM_MAX; count++){ + // Check the lower 4 channels are active for radio connected. + for(int count=0; count < RC_CHANNEL_LOWER_CONTROL_CH_MAX; count++){ // Any invalid range and we abort. if (!isInRange(rcValue[count], RC_CHANNEL_PWM_MIN, RC_CHANNEL_PWM_MAX)){ + QLOG_ERROR() << QString().sprintf("isRadioControlActive: Error Channel %d out of range: rcValue=%f", + count+1, rcValue[count]); return false; } } + + for(int count=RC_CHANNEL_LOWER_CONTROL_CH_MAX; count < RC_CHANNEL_NUM_MAX; count++){ + if ((rcValue[count]>0.0)){ // Only active channels are validated. + if (!isInRange(rcValue[count], RC_CHANNEL_PWM_MIN, RC_CHANNEL_PWM_MAX)){ + QLOG_ERROR() << QString().sprintf("isRadioControlActive: Error Channel %d out of range: rcValue=%f", + count+1, rcValue[count]); + return false; + } + } + } return true; } bool RadioCalibrationConfig::validRadioSettings() { - for(int count=0; count< RC_CHANNEL_NUM_MAX; count++){ + // Check lower 4 channels have been set correctly zero values not allowed. + // i.e. Aileron (Roll), Elevator (Pitch), Throttle, Rudder (Yaw) + for(int count=0; count< RC_CHANNEL_LOWER_CONTROL_CH_MAX; count++){ // Any invalid range and we abort. if (!isInRange(rcMin[count], RC_CHANNEL_PWM_MIN, RC_CHANNEL_PWM_MAX) ||!isInRange(rcMax[count], RC_CHANNEL_PWM_MIN, RC_CHANNEL_PWM_MAX) ||!isInRange(rcTrim[count], RC_CHANNEL_PWM_MIN, RC_CHANNEL_PWM_MAX)){ + QLOG_ERROR() << QString().sprintf("validRadioSettings: Error Channel %d out of range: rcMin=%f rcMax=%f rcTrim=%f", + count+1, rcMin[count], rcMax[count], rcTrim[count]); return false; } } + + // for channels other than the lower 4 we verify only if non-zero. + for(int count=RC_CHANNEL_LOWER_CONTROL_CH_MAX; count< RC_CHANNEL_NUM_MAX; count++){ + // Only check if we have received a non-zero value on the channel + // that the settings are valid. + if ((rcValue[count]>0.0)){ + // Any invalid range and we abort. + if (!isInRange(rcMin[count], RC_CHANNEL_PWM_MIN, RC_CHANNEL_PWM_MAX) + ||!isInRange(rcMax[count], RC_CHANNEL_PWM_MIN, RC_CHANNEL_PWM_MAX) + ||!isInRange(rcTrim[count], RC_CHANNEL_PWM_MIN, RC_CHANNEL_PWM_MAX)){ + QLOG_ERROR() << QString().sprintf("validRadioSettings: Error Channel %d out of range: rcMin=%f rcMax=%f rcTrim=%f", + count+1, rcMin[count], rcMax[count], rcTrim[count]); + return false; + } + } + } + return true; } diff --git a/src/ui/configuration/RadioCalibrationConfig.h b/src/ui/configuration/RadioCalibrationConfig.h index c2c2a74787..347e6ada92 100644 --- a/src/ui/configuration/RadioCalibrationConfig.h +++ b/src/ui/configuration/RadioCalibrationConfig.h @@ -44,9 +44,10 @@ class RadioCalibrationConfig : public AP2ConfigWidget { Q_OBJECT - static const int RC_CHANNEL_PWM_MIN = 900.0; + static const int RC_CHANNEL_PWM_MIN = 895; // Spektrum DX6i reports 898 on ch7 even though its 6 channels static const int RC_CHANNEL_PWM_MAX = 2100; static const int RC_CHANNEL_NUM_MAX = 8; + static const int RC_CHANNEL_LOWER_CONTROL_CH_MAX = 4; public: explicit RadioCalibrationConfig(QWidget *parent = 0); diff --git a/src/ui/configuration/RangeFinderConfig.cc b/src/ui/configuration/RangeFinderConfig.cc index 9630860007..41409fe06b 100644 --- a/src/ui/configuration/RangeFinderConfig.cc +++ b/src/ui/configuration/RangeFinderConfig.cc @@ -45,7 +45,8 @@ RangeFinderConfig::RangeFinderConfig(QWidget *parent) : AP2ConfigWidget(parent) ui.typeComboBox->addItem("Analog", 1 ); ui.typeComboBox->addItem("APM2-MaxbotixI2C", 2 ); ui.typeComboBox->addItem("APM2-PulsedLightI2C", 3) ; - ui.typeComboBox->addItem("PX4", 4 ); + ui.typeComboBox->addItem("PX4-I2C", 4 ); + ui.typeComboBox->addItem("PX4-PWM", 5 ); ui.functionComboBox->addItem("Linear", 0); ui.functionComboBox->addItem("Inverted", 1); diff --git a/src/ui/configuration/SerialSettingsDialog.cc b/src/ui/configuration/SerialSettingsDialog.cc index a169406c3c..92fbadfcd4 100644 --- a/src/ui/configuration/SerialSettingsDialog.cc +++ b/src/ui/configuration/SerialSettingsDialog.cc @@ -183,9 +183,7 @@ void SettingsDialog::fillPortsInfo(QComboBox &comboBox) if (comboBox.itemText(i) == current) { comboBox.setCurrentIndex(i); - setLink(comboBox.currentIndex()); - connect(&comboBox,SIGNAL(currentIndexChanged(int)),this,SLOT(setLink(int))); - return; + break; } } connect(&comboBox,SIGNAL(currentIndexChanged(int)),this,SLOT(setLink(int))); diff --git a/src/ui/configuration/TerminalConsole.cc b/src/ui/configuration/TerminalConsole.cc index 4375861e2f..4784597cc0 100644 --- a/src/ui/configuration/TerminalConsole.cc +++ b/src/ui/configuration/TerminalConsole.cc @@ -200,7 +200,13 @@ void TerminalConsole::logsButtonClicked() { void TerminalConsole::openSerialPort(const SerialSettings &settings) { +#ifdef Q_OS_MACX + // temp fix Qt5.4.1 issue on OSX + // http://code.qt.io/cgit/qt/qtserialport.git/commit/?id=687dfa9312c1ef4894c32a1966b8ac968110b71e + m_serial->setPortName("/dev/cu." + settings.name); +#else m_serial->setPortName(settings.name); +#endif if (m_serial->open(QIODevice::ReadWrite)) { if (m_serial->setBaudRate(settings.baudRate) && m_serial->setDataBits(settings.dataBits) diff --git a/src/ui/map/QGCMapTool.ui b/src/ui/map/QGCMapTool.ui index 0a8ec30377..4e80dfc48e 100644 --- a/src/ui/map/QGCMapTool.ui +++ b/src/ui/map/QGCMapTool.ui @@ -79,7 +79,7 @@