diff --git a/.gitignore b/.gitignore index ddd3a437bcf567f86a35d64d86f8ecc3575d2098..6d8b9fdf08dab16fe0599a83836aac10be09b326 100644 --- a/.gitignore +++ b/.gitignore @@ -1,32 +1,33 @@ -*Makefile* -tags -build -Info.plist -obj -*.log -*~ -*qtc* -bin/*.exe -bin/*.txt -bin/mac -*pro.user* -qrc_*.cpp -*.Debug -*.Release -tmp -debug -release -qgroundcontrol -mavlinkgen -*.wav -qgroundcontrol.xcodeproj/** -doc/html -doc/doxy.log -deploy/mac -deploy/linux -controller_log* -user_config.pri -*.app +*Makefile* +tags +build +Info.plist +obj +*.log +*~ +*qtc* +bin/*.exe +bin/*.txt +bin/mac +*pro.user* +qrc_*.cpp +*.Debug +*.Release +tmp +debug +release +qgroundcontrol +mavlinkgen +*.wav +qgroundcontrol.xcodeproj/** +doc/html +doc/doxy.log +deploy/mac +deploy/linux +deploy/qgroundcontrol* +controller_log* +user_config.pri +*.app *.ncb *.vcproj @@ -39,4 +40,4 @@ user_config.pri *.project *.cproject *.sln -*.suo \ No newline at end of file +*.suo diff --git a/README b/README index f412e023a23edd2139e4f3832b9602eaa627fc87..aaed5f85b940b885548e8416e804a49ec79b37d9 100644 --- a/README +++ b/README @@ -7,6 +7,8 @@ Files: http://github.com/pixhawk/qgroundcontrol http://github.com/pixhawk/mavlink +Credits: +http://qgroundcontrol.org/credits ********************************************************************************************** * PLEASE NOTE: YOU NEED TO DOWNLOAD THE MAVLINK LIBRARY IN ORDER TO COMPILE THIS APPLICATION * @@ -49,9 +51,9 @@ Linux To build on Linux: -sudo apt-get install phonon libqt4-dev libqt4-phonon-dev \ -libqt4-phonon libphonon-dev libphonon4 phonon-backend-gstreamer \ -qt-creator libsdl1.2-dev libflite1 flite1-dev +sudo apt-get install phonon libqt4-dev \ + libphonon-dev libphonon4 phonon-backend-gstreamer \ +qtcreator libsdl1.2-dev libflite1 flite1-dev build-essential cd directory diff --git a/deploy/mac_create_dmg.sh b/deploy/mac_create_dmg.sh index bcdfe4381a653e1ee7b0af1901be6fa6dfe49cd9..e9c0218b174bc846662c9f1bcd32f37ddaeb1f7f 100644 --- a/deploy/mac_create_dmg.sh +++ b/deploy/mac_create_dmg.sh @@ -1,20 +1,12 @@ #!/bin/sh -# Clean build directories -rm -rf mac -mkdir -p mac -# Change to build directory and compile application -cd .. -make -j4 -# Copy and build the application bundle -cd deploy -cp -r ../bin/mac/qgroundcontrol.app mac/. +cp -r ../../qgroundcontrol-build-desktop/qgroundcontrol.app . -cp -r ../audio mac/qgroundcontrol.app/Contents/MacOs/. -mkdir -p mac/qgroundcontrol.app/Contents/Frameworks/ +cp -r ../audio qgroundcontrol.app/Contents/MacOs/. +mkdir -p qgroundcontrol.app/Contents/Frameworks/ # SDL is not copied by Qt - for whatever reason -cp -r SDL.framework mac/qgroundcontrol.app/Contents/Frameworks/. +cp -r /Library/Frameworks/SDL.framework qgroundcontrol.app/Contents/Frameworks/. echo -e '\n\nStarting to create disk image. This may take a while..\n' -macdeployqt mac/qgroundcontrol.app -dmg -rm -rf mac/qgroundcontrol.app +macdeployqt qgroundcontrol.app -dmg +rm -rf qgroundcontrol.app echo -e '\n\n QGroundControl .DMG file is now ready for publishing\n' diff --git a/images/earth.html b/images/earth.html index c5699c596d2fdf2eff7948b77dc4780c19abaad8..a5056ed44caf84a3536b53f3deef204825c33f0a 100644 --- a/images/earth.html +++ b/images/earth.html @@ -14,7 +14,7 @@ google.load("earth", "1", { 'language': 'en'}); var ge = null; var initialized = false; -var currAircraft = 220; +var currAircraft = 0; var followEnabled = false; var lastLat = 0; @@ -36,6 +36,12 @@ var currViewRange = 50.0; ///<< The current viewing range from this position (in var currTilt = 40.0; ///<< The tilt angle (in degrees) var currFollowTilt = 40.0; var currView = null; +var distanceMode = 0; + +var viewMode = 0; +var lastTilt = currTilt; +var lastRoll = 0; +var lastHeading = 0; var M_PI = 3.14159265; @@ -53,12 +59,63 @@ var trails = []; var trailPlacemarks = []; var trailsVisible = []; var trailColors = []; +var waypoints = []; +//var waypointLines = []; //var trailPlacemarks[id]; var lineStyle; // Aircraft class var planeColor = '6600ffff'; +// Enable / disable dragging +var draggingAllowed = true; + +// Waypoint interaction flags +var dragInfo = null; + +var dragWaypointIndex = ""; +var dragWaypointLatitude = 0; +var dragWaypointLongitude = 0; +var dragWaypointAltitude = 0; +var dragWaypointPending = false; + +// Waypoint creation flags +var newWaypointLatitude = 0; +var newWaypointLongitude = 0; +var newWaypointAltitude = 0; +var newWaypointPending = false; + +var homePlacemark = null; + +function getGlobal(variable) +{ + return variable; +} + +function getDraggingAllowed() +{ + return draggingAllowed; +} + +function setDistanceMode(mode) +{ + distanceMode = mode; +} + +function setDraggingAllowed(allowed) +{ + draggingAllowed = allowed; +} + +function setNewWaypointPending(pending) +{ + newWaypointPending = pending; +} + +function setDragWaypointPending(pending) +{ + dragWaypointPending = pending; +} function isInitialized() { @@ -76,6 +133,83 @@ function setFollowEnabled(enable) followEnabled = enable; } +function enableEventListener() +{ +// listen for mousedown on the window (look specifically for point placemarks) +google.earth.addEventListener(ge.getWindow(), 'mousedown', function(event) +{ + if (event.getTarget().getType() == 'KmlPlacemark' && + event.getTarget().getGeometry().getType() == 'KmlPoint') { + var placemark = event.getTarget(); + event.preventDefault(); + if (draggingAllowed) + { + dragInfo = { + placemark: event.getTarget(), + dragged: false + }; + } + } +}); + +// listen for mousemove on the globe +google.earth.addEventListener(ge.getGlobe(), 'mousemove', function(event) +{ + if (draggingAllowed) + { + if (dragInfo) { + event.preventDefault(); + var point = dragInfo.placemark.getGeometry(); + point.setLatitude(event.getLatitude()); + point.setLongitude(event.getLongitude()); + dragInfo.dragged = true; + dragWaypointIndex = dragInfo.placemark.getDescription(); + dragWaypointLatitude = event.getLatitude(); + dragWaypointLongitude = event.getLongitude(); + dragWaypointAltitude = point.getAltitude(); + dragWaypointPending = true; + } + } +}); + +// listen for mouseup on the window +google.earth.addEventListener(ge.getWindow(), 'mouseup', function(event) +{ + if (draggingAllowed) + { + if (dragInfo) { + if (dragInfo.dragged) + { + // if the placemark was dragged, prevent balloons from popping up + event.preventDefault(); + // Get drag end location + dragWaypointIndex = dragInfo.placemark.getDescription(); + var point = dragInfo.placemark.getGeometry(); + dragWaypointLatitude = event.getLatitude(); + dragWaypointLongitude = event.getLongitude(); + dragWaypointAltitude = point.getAltitude(); + dragWaypointPending = true; + } + + dragInfo = null; + } + } +}); + +// Listen for wp creation request on the globe + +google.earth.addEventListener(ge.getGlobe(), 'dblclick', function(event){ + if (draggingAllowed) + { + event.preventDefault(); + newWaypointLatitude = event.getLatitude(); + newWaypointLongitude = event.getLongitude(); + newWaypointAltitude = ge.getGlobe().getGroundAltitude(newWaypointLatitude, newWaypointLongitude); + newWaypointPending = true; + } +}); +} + function setCurrAircraft(id) @@ -90,14 +224,16 @@ function setGCSHome(lat, lon, alt) homeAlt = alt; - + if (homePlacemark == null) + { var placemark = ge.createPlacemark(''); var icon = ge.createIcon(''); icon.setHref('http://google-maps-icons.googlecode.com/files/blackH.png'); var style = ge.createStyle(''); style.getIconStyle().setIcon(icon); //style.getIconStyle().setScale(0.5); - placemark.setStyleSelector(style); + placemark.setStyleSelector(style); + placemark.setDescription('HOME'); // Set the placemark's location. homeLocation = ge.createPoint(''); @@ -107,7 +243,19 @@ function setGCSHome(lat, lon, alt) placemark.setGeometry(homeLocation); // Add the placemark to Earth. - ge.getFeatures().appendChild(placemark); + ge.getFeatures().appendChild(placemark); + + homePlacemark = placemark; + } + else + { + var location = ge.createPoint(''); + location.setLatitude(lat); + location.setLongitude(lon); + location.setAltitude(alt); + homePlacemark.setGeometry(location); + homePlacemark.setDescription('HOME'); + } homeGroundLevel = ge.getGlobe().getGroundAltitude(lat,lon); if (homeGroundLevel == 0) @@ -116,6 +264,64 @@ function setGCSHome(lat, lon, alt) } } +function updateWaypointListLength(id, len) +{ + // Delete any non-needed waypoints + if (waypoints.length > len) + { + for (var i=len; i index) + { + // Waypoint exists + // Set the placemark's location. + var location = ge.createPoint(''); + location.setLatitude(lat); + location.setLongitude(lon); + location.setAltitude(alt); + waypoints[index].setGeometry(location); + waypoints[index].setDescription(index+""); + console.log('WP LOC:' + lat + lon + alt); + } + else + { + // Waypoint does not exist yet + var placemark = ge.createPlacemark(''); + var icon = ge.createIcon(''); + var numberstring = index; + if (index < 10) numberstring = '0' + numberstring + icon.setHref('http://google-maps-icons.googlecode.com/files/red' + numberstring +'.png'); + var style = ge.createStyle(''); + console.log('WP ICON created:' + 'http://google-maps-icons.googlecode.com/files/red' + numberstring +'.png'); + style.getIconStyle().setIcon(icon); + //style.getIconStyle().setScale(0.5); + placemark.setStyleSelector(style); + placemark.setDescription(index+""); + + // Set the placemark's location. + var location = ge.createPoint(''); + location.setLatitude(lat); + location.setLongitude(lon); + location.setAltitude(alt); + placemark.setGeometry(location); + + // Add the placemark to Earth. + ge.getFeatures().appendChild(placemark); + waypoints[index] = placemark; + } + // Add connecting line + +} + function createAircraft(id, type, color) { planePlacemark = ge.createPlacemark(''); @@ -177,6 +383,18 @@ trailsVisible[id] = false; } +function clearTrail(id) +{ + ge.getFeatures().removeChild(trailPlacemarks[id]); + trailPlacemarks[id] = null; + var isVisible = trailsVisible[id]; + createTrail(id, trailColors[id]); + if (isVisible) + { + showTrail(id); + } +} + function hideTrail(id) { trailsVisible[id] = false; @@ -227,6 +445,8 @@ function initCallback(object) ge.getLayerRoot().enableLayerById(ge.LAYER_BUILDINGS, true); ge.getLayerRoot().enableLayerById(ge.LAYER_BORDERS, true); ge.getLayerRoot().enableLayerById(ge.LAYER_TREES, true); + + enableEventListener(); initialized = true; } @@ -242,7 +462,15 @@ function setAircraftPositionAttitude(id, lat, lon, alt, roll, pitch, yaw) } currLat = lat; currLon = lon; - currAlt = alt; + var trueGroundAlt = ge.getGlobe().getGroundAltitude(lat, lon); + if (trueGroundAlt < alt) + { + currAlt = alt; + } + else + { + currAlt = trueGroundAlt+0.1; + } // Interpolate between t-1 and t and set new states lastLat = lastLat*0.8+currLat*0.2; lastLon = lastLon*0.8+currLon*0.2; @@ -277,6 +505,11 @@ function enableDaylight(enabled) ge.getSun().setVisibility(false); } } + +function enableAtmosphere(enabled) +{ + ge.getOptions().setAtmosphereVisibility(enabled); +} function goHome() { @@ -294,16 +527,59 @@ function setCurrentAircraft(id) currAircraft = id; } +/** @brief Set the current view mode + * + * @param mode 0: side map view, 1: top/north map view, 2: fixed chase cam, 3: free chase cam + */ +function setViewMode(mode) +{ + var currView = ge.getView().copyAsLookAt(ge.ALTITUDE_RELATIVE_TO_GROUND); + + if (mode == 0) + { + currView.setTilt(lastTilt); + currView.setHeading(lastHeading); + } + if (mode == 1 && viewMode != mode) + { + lastTilt = currView.getTilt(); + lastHeading = currView.getHeading(); + //var lastLat2 = currView.getLatitude(); + //var lastLon2 = currView.getLongitude(); + //var lastAlt2 = currView.getAltitude(); + currView.setTilt(0); + currView.setHeading(0); + //currView.setLatitude(lastLat2); + //currView.setLongitude(lastLon2); + //currView.setAltitude(lastAlt2); + } + + viewMode = mode; + + ge.getView().setAbstractView(currView); +} + function updateFollowAircraft() { currView = ge.getView().copyAsLookAt(ge.ALTITUDE_ABSOLUTE); currView.setLatitude(lastLat); currView.setLongitude(lastLon); - currView.setAltitude(lastAlt); + + if (distanceMode == 1) + { + var groundAltitude = ge.getGlobe().getGroundAltitude(lastLat, lastLon); + currView.setAltitude(groundAltitude); + } + + if (distanceMode == 0) currView.setAltitude(lastAlt); currView.setRange(currViewRange); - currView.setTilt(currFollowTilt); - currView.setHeading(currFollowHeading); + + if (viewMode != 3) // VIEW_MODE_CHASE_FREE + { + currView.setTilt(currFollowTilt); + currView.setHeading(currFollowHeading); + } ge.getView().setAbstractView(currView); } diff --git a/images/style-mission.css b/images/style-mission.css index b4b601717240a0a9c10c93192b53d7f7d2a86d6e..4cdf69696b9b68dda40df973a2b060ef8e743824 100644 --- a/images/style-mission.css +++ b/images/style-mission.css @@ -356,3 +356,38 @@ QProgressBar::chunk#speedBar { QProgressBar::chunk#thrustBar { background-color: orange; } + +QDialog { + border: 1px solid #62676B; + border-radius: 2px; +} + + QTabWidget::pane { /* The tab widget frame */ + border: 1px solid #62676B; + border-radius: 2px; + position: absolute; + top: -0.5em; + } + + QTabWidget::tab-bar { + alignment: center; + } + + /* Style the tab using the tab sub-control. Note that + it reads QTabBar _not_ QTabWidget */ + QTabBar::tab { + background-color: qlineargradient(x1: 0, y1: 0, x2: 0, y2: 1, stop: 0 #090909, stop: 1 #353535); + border: 2px solid #62676B; + border-radius: 4px; + min-width: 8ex; + padding: 2px; + } + + QTabBar::tab:selected, QTabBar::tab:hover { + background-color: qlineargradient(x1: 0, y1: 0, x2: 0, y2: 1, stop: 0 #090909, stop: 1 #353535); + border: 2px solid #379AC3; + } + + QTabBar::tab:selected { + border: 2px solid #379AC3; + } diff --git a/images/style-outdoor.css b/images/style-outdoor.css index d765aa3845c55194f210e51708257e9a3a5dc5ee..7b4da9c970af02eba2277fdbaf301000876b06e0 100644 --- a/images/style-outdoor.css +++ b/images/style-outdoor.css @@ -49,7 +49,7 @@ border: 1px solid #111111; } QCheckBox::indicator:checked { - background-color: #222222; + background-color: #333333; } QCheckBox::indicator:checked:hover { @@ -92,12 +92,14 @@ border: 1px solid #111111; /* titlebar-close-icon: url(close.png); titlebar-normal-icon: url(undock.png);*/ } - QDockWidget::title { - text-align: left; /* align the text to the left */ - background: lightgray; - padding-left: 5px; - } + text-align: left; + background: #EEEEEE; + color: #111111; + padding-left: 5px; + height: 10px; + border-bottom: 1px solid #222222; +} QDockWidget::close-button, QDockWidget::float-button { border: 1px solid transparent; @@ -120,15 +122,6 @@ QDockWidget::close-button, QDockWidget::float-button { color: #EEEEEE; } -QDockWidget::title { - text-align: left; - background: #121214; - color: #4A4A4F; - padding-left: 5px; - height: 10px; - border-bottom: 1px solid #222222; -} - QSeparator { color: #EEEEEE; } @@ -184,7 +177,7 @@ QPushButton { border-radius: 5px; padding-left: 10px; padding-right: 10px; - background-color: qlineargradient(x1: 0, y1: 0, x2: 0, y2: 1, stop: 0 #232228, stop: 1 #020208); + background-color: qlineargradient(x1: 0, y1: 0, x2: 0, y2: 1, stop: 0 #C3C2C8, stop: 1 #828288); } QPushButton:checked { @@ -202,7 +195,7 @@ QToolButton { max-height: 18px; border: 2px solid #4A4A4F; border-radius: 5px; - background-color: qlineargradient(x1: 0, y1: 0, x2: 0, y2: 1, stop: 0 #232228, stop: 1 #020208); + background-color: qlineargradient(x1: 0, y1: 0, x2: 0, y2: 1, stop: 0 #C3C2C8, stop: 1 #828288); } QToolButton:checked { diff --git a/lib/QMapControl/src/mapnetwork.cpp b/lib/QMapControl/src/mapnetwork.cpp index 041ba1c4b8b2466660ac7c80295d21d647d1e857..63b6a28b96f83c019be5f4cbc6f7ab1904e7be36 100644 --- a/lib/QMapControl/src/mapnetwork.cpp +++ b/lib/QMapControl/src/mapnetwork.cpp @@ -63,10 +63,10 @@ namespace qmapcontrol void MapNetwork::requestFinished(int id, bool error) { // sleep(1); - // qDebug() << "MapNetwork::requestFinished" << http->state() << ", id: " << id; + qDebug() << "QMapControl: MapNetwork::requestFinished" << http->state() << ", id: " << id; if (error) { - qDebug() << "network error: " << http->errorString(); + qDebug() << "QMapControl: network error: " << http->errorString(); //restart query } @@ -79,7 +79,7 @@ namespace qmapcontrol QString url = loadingMap[id]; loadingMap.remove(id); vectorMutex.unlock(); - // qDebug() << "request finished for id: " << id << ", belongs to: " << notifier.url << endl; + //qDebug() << "QMapControl: request finished for id: " << id << ", belongs to: " << notifier.url << endl; QByteArray ax; if (http->bytesAvailable()>0) @@ -87,17 +87,20 @@ namespace qmapcontrol QPixmap pm; ax = http->readAll(); + qDebug() << "QMapControl: Request consisted of " << ax.size() << "bytes"; + if (pm.loadFromData(ax)) { loaded += pm.size().width()*pm.size().height()*pm.depth()/8/1024; - // qDebug() << "Network loaded: " << (loaded); + qDebug() << "QMapControl: Network loaded: " << (loaded); parent->receivedImage(pm, url); } else if (pm.width() == 0 || pm.height() == 0) { // Silently ignore map request for a // 0xn pixel map - + qDebug() << "QMapControl: IGNORED 0x0 pixel map request, widthxheight:" << pm.width() << "x" << pm.height(); + qDebug() << "QMapControl: HTML ERROR MESSAGE:" << ax << "at " << __FILE__ << __LINE__; } else { @@ -105,7 +108,7 @@ namespace qmapcontrol // TODO Error is currently undetected //qDebug() << "NETWORK_PIXMAP_ERROR: " << ax; qDebug() << "QMapControl external library: ERROR loading map:" << "width:" << pm.width() << "heigh:" << pm.height() << "at " << __FILE__ << __LINE__; - //qDebug() << "HTML ERROR MESSAGE:" << ax << "at " << __FILE__ << __LINE__; + qDebug() << "QMapControl: HTML ERROR MESSAGE:" << ax << "at " << __FILE__ << __LINE__; } } diff --git a/mavground.qrc b/mavground.qrc index 3e7be65296d89971de3b5b32f06734814e32f1a5..4a1d72174e85e7a0e297300c9a5dc224163e41b0 100644 --- a/mavground.qrc +++ b/mavground.qrc @@ -85,6 +85,7 @@ images/earth.html images/mapproviders/googleearth.svg images/contrib/slugs.png + images/style-outdoor.css images/Vera.ttf diff --git a/qgcunittest/UASUnitTest.cc b/qgcunittest/UASUnitTest.cc index 7aa85377bda8d35197933167ad631be5d1ece3f3..8b4c6c1b795a756737bd261b98af3d6ba1ac7660 100644 --- a/qgcunittest/UASUnitTest.cc +++ b/qgcunittest/UASUnitTest.cc @@ -6,14 +6,15 @@ UASUnitTest::UASUnitTest() void UASUnitTest::initTestCase() { - mav= new MAVLinkProtocol(); - uas=new UAS(mav,UASID); + mav= new MAVLinkProtocol(); + link = new SerialLink(); + uas=new UAS(mav,UASID); } void UASUnitTest::cleanupTestCase() { - delete uas; - delete mav; + delete uas; + delete mav; } @@ -34,123 +35,244 @@ void UASUnitTest::getUASID_test() void UASUnitTest::getUASName_test() { - // Test that the name is build as MAV + ID - QCOMPARE(uas->getUASName(), "MAV 0" + QString::number(UASID)); + // Test that the name is build as MAV + ID + QCOMPARE(uas->getUASName(), "MAV 0" + QString::number(UASID)); } void UASUnitTest::getUpTime_test() { - UAS* uas2 = new UAS(mav); - // Test that the uptime starts at zero to a - // precision of seconds - QCOMPARE(floor(uas2->getUptime()/1000.0), 0.0); + UAS* uas2 = new UAS(mav); + // Test that the uptime starts at zero to a + // precision of seconds + QCOMPARE(floor(uas2->getUptime()/1000.0), 0.0); - // Sleep for three seconds - QTest::qSleep(3000); + // Sleep for three seconds + QTest::qSleep(3000); - // Test that the up time is computed correctly to a - // precision of seconds - QCOMPARE(floor(uas2->getUptime()/1000.0), 3.0); + // Test that the up time is computed correctly to a + // precision of seconds + QCOMPARE(floor(uas2->getUptime()/1000.0), 3.0); - delete uas2; + delete uas2; } void UASUnitTest::getCommunicationStatus_test() { - // Verify that upon construction the Comm status is disconnected - QCOMPARE(uas->getCommunicationStatus(), static_cast(UASInterface::COMM_DISCONNECTED)); + // Verify that upon construction the Comm status is disconnected + QCOMPARE(uas->getCommunicationStatus(), static_cast(UASInterface::COMM_DISCONNECTED)); } void UASUnitTest::filterVoltage_test() { float verificar=uas->filterVoltage(0.4f); - // Verify that upon construction the Comm status is disconnected - QCOMPARE(verificar, 8.52f); + // Verify that upon construction the Comm status is disconnected + QCOMPARE(verificar, 8.52f); } void UASUnitTest:: getAutopilotType_test() { - int verificar=uas->getAutopilotType(); - // Verify that upon construction the autopilot is set to -1 - QCOMPARE(verificar, -1); + int type = uas->getAutopilotType(); + // Verify that upon construction the autopilot is set to -1 + QCOMPARE(type, -1); } void UASUnitTest::setAutopilotType_test() { - uas->setAutopilotType(2); - // Verify that the autopilot is set - QCOMPARE(uas->getAutopilotType(), 2); + uas->setAutopilotType(2); + // Verify that the autopilot is set + QCOMPARE(uas->getAutopilotType(), 2); } void UASUnitTest::getStatusForCode_test() { - QString state, desc; - state = ""; - desc = ""; + QString state, desc; + state = ""; + desc = ""; - uas->getStatusForCode(MAV_STATE_UNINIT, state, desc); - QVERIFY(state == "UNINIT"); + uas->getStatusForCode(MAV_STATE_UNINIT, state, desc); + QVERIFY(state == "UNINIT"); - uas->getStatusForCode(MAV_STATE_UNINIT, state, desc); - QVERIFY(state == "UNINIT"); + uas->getStatusForCode(MAV_STATE_UNINIT, state, desc); + QVERIFY(state == "UNINIT"); - uas->getStatusForCode(MAV_STATE_BOOT, state, desc); - QVERIFY(state == "BOOT"); + uas->getStatusForCode(MAV_STATE_BOOT, state, desc); + QVERIFY(state == "BOOT"); - uas->getStatusForCode(MAV_STATE_CALIBRATING, state, desc); - QVERIFY(state == "CALIBRATING"); + uas->getStatusForCode(MAV_STATE_CALIBRATING, state, desc); + QVERIFY(state == "CALIBRATING"); - uas->getStatusForCode(MAV_STATE_ACTIVE, state, desc); - QVERIFY(state == "ACTIVE"); + uas->getStatusForCode(MAV_STATE_ACTIVE, state, desc); + QVERIFY(state == "ACTIVE"); - uas->getStatusForCode(MAV_STATE_STANDBY, state, desc); - QVERIFY(state == "STANDBY"); + uas->getStatusForCode(MAV_STATE_STANDBY, state, desc); + QVERIFY(state == "STANDBY"); - uas->getStatusForCode(MAV_STATE_CRITICAL, state, desc); - QVERIFY(state == "CRITICAL"); + uas->getStatusForCode(MAV_STATE_CRITICAL, state, desc); + QVERIFY(state == "CRITICAL"); - uas->getStatusForCode(MAV_STATE_EMERGENCY, state, desc); - QVERIFY(state == "EMERGENCY"); + uas->getStatusForCode(MAV_STATE_EMERGENCY, state, desc); + QVERIFY(state == "EMERGENCY"); - uas->getStatusForCode(MAV_STATE_POWEROFF, state, desc); - QVERIFY(state == "SHUTDOWN"); + uas->getStatusForCode(MAV_STATE_POWEROFF, state, desc); + QVERIFY(state == "SHUTDOWN"); - uas->getStatusForCode(5325, state, desc); - QVERIFY(state == "UNKNOWN"); + uas->getStatusForCode(5325, state, desc); + QVERIFY(state == "UNKNOWN"); } void UASUnitTest::getLocalX_test() { - QCOMPARE(uas->getLocalX(), 0.0); + QCOMPARE(uas->getLocalX(), 0.0); } void UASUnitTest::getLocalY_test() { - QCOMPARE(uas->getLocalY(), 0.0); + QCOMPARE(uas->getLocalY(), 0.0); } void UASUnitTest::getLocalZ_test() { - QCOMPARE(uas->getLocalZ(), 0.0); + QCOMPARE(uas->getLocalZ(), 0.0); } void UASUnitTest::getLatitude_test() { QCOMPARE(uas->getLatitude(), 0.0); } void UASUnitTest::getLongitude_test() { - QCOMPARE(uas->getLongitude(), 0.0); + QCOMPARE(uas->getLongitude(), 0.0); } void UASUnitTest::getAltitude_test() { - QCOMPARE(uas->getAltitude(), 0.0); + QCOMPARE(uas->getAltitude(), 0.0); } void UASUnitTest::getRoll_test() { - QCOMPARE(uas->getRoll(), 0.0); + QCOMPARE(uas->getRoll(), 0.0); } void UASUnitTest::getPitch_test() { - QCOMPARE(uas->getPitch(), 0.0); + QCOMPARE(uas->getPitch(), 0.0); } void UASUnitTest::getYaw_test() { - QCOMPARE(uas->getYaw(), 0.0); + QCOMPARE(uas->getYaw(), 0.0); +} +void UASUnitTest::attitudeLimitsZero_test() +{ + mavlink_message_t msg; + mavlink_attitude_t att; + + // Set zero values and encode them + att.roll = 0.0f; + att.pitch = 0.0f; + att.yaw = 0.0f; + mavlink_msg_attitude_encode(uas->getUASID(), MAV_COMP_ID_IMU, &msg, &att); + // Let UAS decode message + uas->receiveMessage(link, msg); + // Check result + QCOMPARE(uas->getRoll(), 0.0); + QCOMPARE(uas->getPitch(), 0.0); + QCOMPARE(uas->getYaw(), 0.0); +} + +void UASUnitTest::attitudeLimitsPi_test() +{ + mavlink_message_t msg; + mavlink_attitude_t att; + // Set PI values and encode them + att.roll = M_PI; + att.pitch = M_PI; + att.yaw = M_PI; + mavlink_msg_attitude_encode(uas->getUASID(), MAV_COMP_ID_IMU, &msg, &att); + // Let UAS decode message + uas->receiveMessage(link, msg); + // Check result + QVERIFY((uas->getRoll() < M_PI+0.000001) && (uas->getRoll() > M_PI+-0.000001)); + QVERIFY((uas->getPitch() < M_PI+0.000001) && (uas->getPitch() > M_PI+-0.000001)); + QVERIFY((uas->getYaw() < M_PI+0.000001) && (uas->getYaw() > M_PI+-0.000001)); +} + +void UASUnitTest::attitudeLimitsMinusPi_test() +{ + mavlink_message_t msg; + mavlink_attitude_t att; + // Set -PI values and encode them + att.roll = -M_PI; + att.pitch = -M_PI; + att.yaw = -M_PI; + mavlink_msg_attitude_encode(uas->getUASID(), MAV_COMP_ID_IMU, &msg, &att); + // Let UAS decode message + uas->receiveMessage(link, msg); + // Check result + QVERIFY((uas->getRoll() > -M_PI-0.000001) && (uas->getRoll() < -M_PI+0.000001)); + QVERIFY((uas->getPitch() > -M_PI-0.000001) && (uas->getPitch() < -M_PI+0.000001)); + QVERIFY((uas->getYaw() > -M_PI-0.000001) && (uas->getYaw() < -M_PI+0.000001)); +} + +void UASUnitTest::attitudeLimitsTwoPi_test() +{ + mavlink_message_t msg; + mavlink_attitude_t att; + // Set off-limit values and check + // correct wrapping + // Set 2PI values and encode them + att.roll = 2*M_PI; + att.pitch = 2*M_PI; + att.yaw = 2*M_PI; + mavlink_msg_attitude_encode(uas->getUASID(), MAV_COMP_ID_IMU, &msg, &att); + // Let UAS decode message + uas->receiveMessage(link, msg); + // Check result + QVERIFY((uas->getRoll() < 0.000001) && (uas->getRoll() > -0.000001)); + QVERIFY((uas->getPitch() < 0.000001) && (uas->getPitch() > -0.000001)); + QVERIFY((uas->getYaw() < 0.000001) && (uas->getYaw() > -0.000001)); +} + +void UASUnitTest::attitudeLimitsMinusTwoPi_test() +{ + mavlink_message_t msg; + mavlink_attitude_t att; + // Set -2PI values and encode them + att.roll = -2*M_PI; + att.pitch = -2*M_PI; + att.yaw = -2*M_PI; + mavlink_msg_attitude_encode(uas->getUASID(), MAV_COMP_ID_IMU, &msg, &att); + // Let UAS decode message + uas->receiveMessage(link, msg); + // Check result + QVERIFY((uas->getRoll() < 0.000001) && (uas->getRoll() > -0.000001)); + QVERIFY((uas->getPitch() < 0.000001) && (uas->getPitch() > -0.000001)); + QVERIFY((uas->getYaw() < 0.000001) && (uas->getYaw() > -0.000001)); +} + +void UASUnitTest::attitudeLimitsTwoPiOne_test() +{ + mavlink_message_t msg; + mavlink_attitude_t att; + // Set over 2 PI values and encode them + att.roll = 2*M_PI+1.0f; + att.pitch = 2*M_PI+1.0f; + att.yaw = 2*M_PI+1.0f; + mavlink_msg_attitude_encode(uas->getUASID(), MAV_COMP_ID_IMU, &msg, &att); + // Let UAS decode message + uas->receiveMessage(link, msg); + // Check result + QVERIFY((uas->getRoll() < 1.000001) && (uas->getRoll() > 0.999999)); + QVERIFY((uas->getPitch() < 1.000001) && (uas->getPitch() > 0.999999)); + QVERIFY((uas->getYaw() < 1.000001) && (uas->getYaw() > 0.999999)); +} + +void UASUnitTest::attitudeLimitsMinusTwoPiOne_test() +{ + mavlink_message_t msg; + mavlink_attitude_t att; + // Set 3PI values and encode them + att.roll = -2*M_PI-1.0f; + att.pitch = -2*M_PI-1.0f; + att.yaw = -2*M_PI-1.0f; + mavlink_msg_attitude_encode(uas->getUASID(), MAV_COMP_ID_IMU, &msg, &att); + // Let UAS decode message + uas->receiveMessage(link, msg); + // Check result + QVERIFY((uas->getRoll() > -1.000001) && (uas->getRoll() < -0.999999)); + QVERIFY((uas->getPitch() > -1.000001) && (uas->getPitch() < -0.999999)); + QVERIFY((uas->getYaw() > -1.000001) && (uas->getYaw() < -0.999999)); } diff --git a/qgcunittest/UASUnitTest.h b/qgcunittest/UASUnitTest.h index 67f1eb6d826276dee0858a9705e20f5a52332aa0..7bd98bd23ed0db1be3e38460e01970aad85d68b2 100644 --- a/qgcunittest/UASUnitTest.h +++ b/qgcunittest/UASUnitTest.h @@ -6,6 +6,7 @@ #include #include "UAS.h" #include "MAVLinkProtocol.h" +#include "SerialLink.h" #include "UASInterface.h" #include "AutoTest.h" @@ -13,36 +14,44 @@ class UASUnitTest : public QObject { Q_OBJECT public: - #define UASID 50 - MAVLinkProtocol* mav; - UAS* uas; - UASUnitTest(); +#define UASID 50 + MAVLinkProtocol* mav; + UAS* uas; + SerialLink* link; + UASUnitTest(); signals: private slots: - void initTestCase(); - void cleanupTestCase(); - void getUASID_test(); - void getUASName_test(); - void getUpTime_test(); - void getCommunicationStatus_test(); - void filterVoltage_test(); - void getAutopilotType_test(); - void setAutopilotType_test(); - void getStatusForCode_test(); - void getLocalX_test(); - void getLocalY_test(); - void getLocalZ_test(); - void getLatitude_test(); - void getLongitude_test(); - void getAltitude_test(); - void getRoll_test(); - void getPitch_test(); - void getYaw_test(); + void initTestCase(); + void cleanupTestCase(); + void getUASID_test(); + void getUASName_test(); + void getUpTime_test(); + void getCommunicationStatus_test(); + void filterVoltage_test(); + void getAutopilotType_test(); + void setAutopilotType_test(); + void getStatusForCode_test(); + void getLocalX_test(); + void getLocalY_test(); + void getLocalZ_test(); + void getLatitude_test(); + void getLongitude_test(); + void getAltitude_test(); + void getRoll_test(); + void getPitch_test(); + void getYaw_test(); + void attitudeLimitsZero_test(); + void attitudeLimitsPi_test(); + void attitudeLimitsMinusPi_test(); + void attitudeLimitsTwoPi_test(); + void attitudeLimitsMinusTwoPi_test(); + void attitudeLimitsTwoPiOne_test(); + void attitudeLimitsMinusTwoPiOne_test(); protected: - UAS *prueba; + UAS *prueba; }; diff --git a/qgroundcontrol-plugins.pro b/qgroundcontrol-plugins.pro deleted file mode 100644 index 5f00ac29488907920180b16eff1210166faa360b..0000000000000000000000000000000000000000 --- a/qgroundcontrol-plugins.pro +++ /dev/null @@ -1,45 +0,0 @@ -# ------------------------------------------------- -# QGroundControl - Micro Air Vehicle Groundstation -# Please see our website at -# Author: -# Lorenz Meier -# (c) 2009-2010 PIXHAWK Team -# This file is part of the mav groundstation project -# QGroundControl is free software: you can redistribute it and/or modify -# it under the terms of the GNU General Public License as published by -# the Free Software Foundation, either version 3 of the License, or -# (at your option) any later version. -# QGroundControl is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU General Public License for more details. -# You should have received a copy of the GNU General Public License -# along with QGroundControl. If not, see . -# ------------------------------------------------- -# Include QMapControl map library -# prefer version from external directory / -# from http://github.com/pixhawk/qmapcontrol/ -# over bundled version in lib directory -# Version from GIT repository is preferred -# include ( "../qmapcontrol/QMapControl/QMapControl.pri" ) #{ -# Include bundled version if necessary - - CONFIG += designer plugin - TARGET = $$qtLibraryTarget($$TARGET) - TEMPLATE = lib - QTDIR_build:DESTDIR = $$QT_BUILD_TREE/plugins/designer - - FORMS = src/ui/designer/QGCParamSlider.ui - - HEADERS = src/ui/designer/QGCParamSlider.h \ - src/ui/designer/QGCParamSliderPlugin.h - SOURCES = src/ui/designer/QGCParamSlider.cc \ - src/ui/designer/QGCParamSliderPlugin.cc - - # install - target.path = $$[QT_INSTALL_PLUGINS]/designer - sources.files = $$SOURCES $$HEADERS *.pro - sources.path = $$[QT_INSTALL_EXAMPLES]/designer/worldtimeclockplugin - INSTALLS += target - - symbian: include($$QT_SOURCE_TREE/examples/symbianpkgrules.pri) diff --git a/qgroundcontrol.pri b/qgroundcontrol.pri index f846c60db2671460785bc69f416384ec14913d81..008cf159938adb1d3a68f0850e9664b28469c4e8 100644 --- a/qgroundcontrol.pri +++ b/qgroundcontrol.pri @@ -38,39 +38,44 @@ release { QMAKE_POST_LINK += echo "Copying files" +# Turn off serial port warnings +DEFINES += _TTY_NOWARN_ + #QMAKE_POST_LINK += && cp -rf $$BASEDIR/models $$TARGETDIR/debug/. #QMAKE_POST_LINK += && cp -rf $$BASEDIR/models $$TARGETDIR/release/. # MAC OS X macx { - COMPILER_VERSION = $$system(gcc -v) - message(Using compiler $$COMPILER_VERSION) + # COMPILER_VERSION = $$system(gcc -v) + #message(Using compiler $$COMPILER_VERSION) - HARDWARE_PLATFORM = $$system(uname -a) - contains( HARDWARE_PLATFORM, 9.6.0 ) || contains( HARDWARE_PLATFORM, 9.7.0 ) || contains( HARDWARE_PLATFORM, 9.8.0 ) || contains( HARDWARE_PLATFORM, 9.9.0 ) { - # x86 Mac OS X Leopard 10.5 and earlier CONFIG += x86 cocoa phonon CONFIG -= x86_64 - message(Building for Mac OS X 32bit/Leopard 10.5 and earlier) + + #HARDWARE_PLATFORM = $$system(uname -a) + #contains( $$HARDWARE_PLATFORM, "9.6.0" ) || contains( $$HARDWARE_PLATFORM, "9.7.0" ) || contains( $$HARDWARE_PLATFORM, "9.8.0" ) || contains( $$HARDWARE_PLATFORM, "9.9.0" ) { + # x86 Mac OS X Leopard 10.5 and earlier + + #message(Building for Mac OS X 32bit/Leopard 10.5 and earlier) # Enable function-profiling with the OS X saturn tool - debug { + #debug { #QMAKE_CXXFLAGS += -finstrument-functions #LIBS += -lSaturn - CONFIG += console - } - } else { + # CONFIG += console + #} + #} else { # x64 Mac OS X Snow Leopard 10.6 and later - CONFIG += x86_64 cocoa - CONFIG -= x86 phonon - message(Building for Mac OS X 64bit/Snow Leopard 10.6 and later) - debug { + # CONFIG += x86_64 x86 cocoa phonon + #CONFIG -= x86 # phonon + #message(Building for Mac OS X 64bit/Snow Leopard 10.6 and later) + # debug { #QMAKE_CXXFLAGS += -finstrument-functions #LIBS += -lSaturn CONFIG += console - } - } + # } + #} QMAKE_MACOSX_DEPLOYMENT_TARGET = 10.5 diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index a260a4390a813dcc8ae527f70440cde20e317221..40b68c247fca9be0caf0683fe7b88d8019402dbe 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -144,7 +144,6 @@ FORMS += src/ui/MainWindow.ui \ src/ui/QMap3D.ui \ src/ui/QGCWebView.ui \ src/ui/map3D/QGCGoogleEarthView.ui \ - src/ui/map3D/QGCGoogleEarthViewWin.ui \ src/ui/SlugsDataSensorView.ui \ src/ui/SlugsHilSim.ui \ src/ui/SlugsPIDControl.ui \ @@ -155,7 +154,10 @@ FORMS += src/ui/MainWindow.ui \ src/ui/designer/QGCParamSlider.ui \ src/ui/designer/QGCActionButton.ui \ src/ui/QGCMAVLinkLogPlayer.ui \ - src/ui/QGCWaypointListMulti.ui + src/ui/QGCWaypointListMulti.ui \ + src/ui/mission/QGCCustomWaypointAction.ui \ + src/ui/QGCUDPLinkConfiguration.ui \ + src/ui/QGCSettingsWidget.ui INCLUDEPATH += src \ src/ui \ @@ -265,7 +267,9 @@ HEADERS += src/MG.h \ src/comm/MAVLinkSimulationWaypointPlanner.h \ src/comm/MAVLinkSimulationMAV.h \ src/uas/QGCMAVLinkUASFactory.h \ - src/ui/QGCWaypointListMulti.h + src/ui/QGCWaypointListMulti.h \ + src/ui/QGCUDPLinkConfiguration.h \ + src/ui/QGCSettingsWidget.h # Google Earth is only supported on Mac OS and Windows with Visual Studio Compiler macx|win32-msvc2008: { @@ -390,7 +394,9 @@ SOURCES += src/main.cc \ src/comm/MAVLinkSimulationWaypointPlanner.cc \ src/comm/MAVLinkSimulationMAV.cc \ src/uas/QGCMAVLinkUASFactory.cc \ - src/ui/QGCWaypointListMulti.cc + src/ui/QGCWaypointListMulti.cc \ + src/ui/QGCUDPLinkConfiguration.cc \ + src/ui/QGCSettingsWidget.cc macx|win32-msvc2008: { SOURCES += src/ui/map3D/QGCGoogleEarthView.cc diff --git a/settings/ParameterList.xml b/settings/ParameterList.xml index ac386300a96cc53944a7f956db8eed65a5897338..445f4d5a738d0b7e3c202ad7ae67c7dfbbaa96f9 100644 --- a/settings/ParameterList.xml +++ b/settings/ParameterList.xml @@ -1,227 +1,227 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/GAudioOutput.cc b/src/GAudioOutput.cc index 641f2117036019e7eeeffdad5826c1e62c4d6629..50ef1ecbc630cd59e59f644ff39337d188aefa95 100644 --- a/src/GAudioOutput.cc +++ b/src/GAudioOutput.cc @@ -151,10 +151,14 @@ GAudioOutput::~GAudioOutput() void GAudioOutput::mute(bool mute) { - this->muted = mute; - QSettings settings; - settings.setValue(QGC_GAUDIOOUTPUT_KEY+"muted", this->muted); - settings.sync(); + if (mute != muted) + { + this->muted = mute; + QSettings settings; + settings.setValue(QGC_GAUDIOOUTPUT_KEY+"muted", this->muted); + settings.sync(); + emit mutedChanged(muted); + } } bool GAudioOutput::isMuted() @@ -295,11 +299,12 @@ void GAudioOutput::beep() { if (!muted) { - // Use QFile to transform path for all OS - QFile f(QCoreApplication::applicationDirPath()+QString("/audio/alert.wav")); - m_media->setCurrentSource(Phonon::MediaSource(f.fileName().toStdString().c_str())); - m_media->play(); -} + // Use QFile to transform path for all OS + QFile f(QCoreApplication::applicationDirPath()+QString("/audio/alert.wav")); + qDebug() << "FILE:" << f.fileName(); + m_media->setCurrentSource(Phonon::MediaSource(f.fileName().toStdString().c_str())); + m_media->play(); + } } void GAudioOutput::selectFemaleVoice() diff --git a/src/GAudioOutput.h b/src/GAudioOutput.h index b30a26c7b34eab9987bac77f7c094a9e6016a0dd..316e34d05850b2549e2a6b9608cebdea9eba14b6 100644 --- a/src/GAudioOutput.h +++ b/src/GAudioOutput.h @@ -40,8 +40,8 @@ This file is part of the PIXHAWK project #endif #ifdef Q_OS_LINUX //#include -#include -#include +#include +#include #endif #ifdef Q_OS_WIN #include @@ -80,6 +80,9 @@ public: VOICE_FEMALE } QGVoice; + /** @brief Get the mute state */ + bool isMuted(); + public slots: /** @brief Say this text if current output priority matches */ bool say(QString text, int severity=1); @@ -101,8 +104,9 @@ public slots: void notifyNegative(); /** @brief Mute/unmute sound */ void mute(bool mute); - /** @brief Get the mute state */ - bool isMuted(); + +signals: + void mutedChanged(bool); protected: #ifdef Q_OS_MAC diff --git a/src/QGC.cc b/src/QGC.cc index 087c4885742a48ca3efdc1ad61705f1ea64e4701..d55b7208ba137767114e86f887094b3aba260372 100644 --- a/src/QGC.cc +++ b/src/QGC.cc @@ -23,6 +23,9 @@ This file is part of the QGROUNDCONTROL project #include "QGC.h" +#include +#include + namespace QGC { quint64 groundTimeUsecs() @@ -34,6 +37,49 @@ quint64 groundTimeUsecs() return static_cast(microseconds + (time.time().msec()*1000)); } +quint64 groundTimeMilliseconds() +{ + QDateTime time = QDateTime::currentDateTime(); + time = time.toUTC(); + /* Return seconds and milliseconds, in milliseconds unit */ + quint64 seconds = time.toTime_t() * static_cast(1000); + return static_cast(seconds + (time.time().msec())); +} + +float limitAngleToPMPIf(float angle) +{ + while (angle > ((float)M_PI+FLT_EPSILON)) + { + angle -= 2.0f * (float)M_PI; + } + + while (angle <= -((float)M_PI+FLT_EPSILON)) + { + angle += 2.0f * (float)M_PI; + } + + return angle; +} + +double limitAngleToPMPId(double angle) +{ + if (angle < -M_PI) + { + while (angle < -M_PI) + { + angle += M_PI; + } + } + else if (angle > M_PI) + { + while (angle > M_PI) + { + angle -= M_PI; + } + } + return angle; +} + int applicationVersion() { return APPLICATIONVERSION; diff --git a/src/QGC.h b/src/QGC.h index 7ece2f230415217e17e70a65c24c0907801315ed..e5b87e288e7c575b51c604721f7989b7ca46a894 100644 --- a/src/QGC.h +++ b/src/QGC.h @@ -9,15 +9,26 @@ namespace QGC { + const static int defaultSystemId = 255; + const static int defaultComponentId = 0; + const QColor colorCyan(55, 154, 195); const QColor colorRed(154, 20, 20); const QColor colorGreen(20, 200, 20); const QColor colorYellow(255, 255, 0); + const QColor colorOrange(255, 140, 0); const QColor colorDarkYellow(180, 180, 0); const QColor colorBackground("#050508"); + const QColor colorBlack(0, 0, 0); /** @brief Get the current ground time in microseconds */ quint64 groundTimeUsecs(); + /** @brief Get the current ground time in milliseconds */ + quint64 groundTimeMilliseconds(); + /** @brief Returns the angle limited to -pi - pi */ + float limitAngleToPMPIf(float angle); + /** @brief Returns the angle limited to -pi - pi */ + double limitAngleToPMPId(double angle); int applicationVersion(); const static int MAX_FLIGHT_TIME = 60 * 60 * 24 * 21; diff --git a/src/Waypoint.cc b/src/Waypoint.cc index b629f3355a0205f99fc065d25cbd68f64d27b431..66737be81eded59883ba8c505e5615fa11dfefd0 100644 --- a/src/Waypoint.cc +++ b/src/Waypoint.cc @@ -32,18 +32,20 @@ This file is part of the QGROUNDCONTROL project #include "Waypoint.h" #include -Waypoint::Waypoint(quint16 _id, double _x, double _y, double _z, double _yaw, bool _autocontinue, bool _current, double _orbit, int _holdTime, MAV_FRAME _frame, MAV_ACTION _action) +Waypoint::Waypoint(quint16 _id, double _x, double _y, double _z, double _param1, double _param2, double _param3, double _param4, + bool _autocontinue, bool _current, MAV_FRAME _frame, MAV_CMD _action) : id(_id), x(_x), y(_y), z(_z), - yaw(_yaw), + yaw(_param4), frame(_frame), action(_action), autocontinue(_autocontinue), current(_current), - orbit(_orbit), - holdTime(_holdTime), + orbit(_param3), + param1(_param1), + param2(_param2), name(QString("WP%1").arg(id, 2, 10, QChar('0'))) { } @@ -55,32 +57,34 @@ Waypoint::~Waypoint() void Waypoint::save(QTextStream &saveStream) { - QString position("%1\t%2\t%3\t%4"); + QString position("%1\t%2\t%3"); position = position.arg(x, 0, 'g', 18); position = position.arg(y, 0, 'g', 18); position = position.arg(z, 0, 'g', 18); - position = position.arg(yaw, 0, 'g', 8); - saveStream << this->getId() << "\t" << this->getFrame() << "\t" << this->getAction() << "\t" << this->getOrbit() << "\t" << /*Orbit Direction*/ 0 << "\t" << this->getOrbit() << "\t" << this->getHoldTime() << "\t" << this->getCurrent() << "\t" << position << "\t" << this->getAutoContinue() << "\r\n"; + QString parameters("%1\t%2\t%3\t%4"); + parameters = parameters.arg(param1, 0, 'g', 18).arg(param2, 0, 'g', 18).arg(orbit, 0, 'g', 18).arg(yaw, 0, 'g', 18); + // FORMAT: + // as documented here: http://qgroundcontrol.org/waypoint_protocol + saveStream << this->getId() << "\t" << this->getCurrent() << "\t" << this->getFrame() << "\t" << this->getAction() << "\t" << parameters << "\t" << position << "\t" << this->getAutoContinue() << "\r\n"; } bool Waypoint::load(QTextStream &loadStream) { const QStringList &wpParams = loadStream.readLine().split("\t"); - if (wpParams.size() == 13) + if (wpParams.size() == 12) { this->id = wpParams[0].toInt(); - this->frame = (MAV_FRAME) wpParams[1].toInt(); - this->action = (MAV_ACTION) wpParams[2].toInt(); - this->orbit = wpParams[3].toDouble(); - //TODO: orbit direction - //TODO: param1 - this->holdTime = wpParams[6].toInt(); - this->current = (wpParams[7].toInt() == 1 ? true : false); + this->current = (wpParams[1].toInt() == 1 ? true : false); + this->frame = (MAV_FRAME) wpParams[2].toInt(); + this->action = (MAV_CMD) wpParams[3].toInt(); + this->param1 = wpParams[4].toDouble(); + this->param2 = wpParams[5].toDouble(); + this->orbit = wpParams[6].toDouble(); + this->yaw = wpParams[7].toDouble(); this->x = wpParams[8].toDouble(); this->y = wpParams[9].toDouble(); this->z = wpParams[10].toDouble(); - this->yaw = wpParams[11].toDouble(); - this->autocontinue = (wpParams[12].toInt() == 1 ? true : false); + this->autocontinue = (wpParams[11].toInt() == 1 ? true : false); return true; } return false; @@ -121,6 +125,45 @@ void Waypoint::setZ(double z) } } +void Waypoint::setLatitude(double lat) +{ + if (this->x != lat) + { + this->x = lat; + this->frame = MAV_FRAME_GLOBAL; + emit changed(this); + } +} + +void Waypoint::setLongitude(double lon) +{ + if (this->y != lon) + { + this->y = lon; + this->frame = MAV_FRAME_GLOBAL; + emit changed(this); + } +} + +void Waypoint::setAltitude(double altitude) +{ + if (this->z != altitude) + { + this->z = altitude; + this->frame = MAV_FRAME_GLOBAL; + emit changed(this); + } +} + +void Waypoint::setYaw(int yaw) +{ + if (this->yaw != yaw) + { + this->yaw = yaw; + emit changed(this); + } +} + void Waypoint::setYaw(double yaw) { if (this->yaw != yaw) @@ -130,7 +173,16 @@ void Waypoint::setYaw(double yaw) } } -void Waypoint::setAction(MAV_ACTION action) +void Waypoint::setAction(int action) +{ + if (this->action != (MAV_CMD)action) + { + this->action = (MAV_CMD)action; + emit changed(this); + } +} + +void Waypoint::setAction(MAV_CMD action) { if (this->action != action) { @@ -166,7 +218,79 @@ void Waypoint::setCurrent(bool current) } } -void Waypoint::setOrbit(double orbit) +void Waypoint::setAcceptanceRadius(double radius) +{ + if (this->param2 != radius) + { + this->param2 = radius; + emit changed(this); + } +} + +void Waypoint::setParam1(double param1) +{ + if (this->param1 != param1) + { + this->param1 = param1; + emit changed(this); + } +} + +void Waypoint::setParam2(double param2) +{ + if (this->param2 != param2) + { + this->param2 = param2; + emit changed(this); + } +} + +void Waypoint::setParam3(double param3) +{ + if (this->orbit != param3) + { + this->orbit = param3; + emit changed(this); + } +} + +void Waypoint::setParam4(double param4) +{ + if (this->yaw != param4) + { + this->yaw = param4; + emit changed(this); + } +} + +void Waypoint::setParam5(double param5) +{ + if (this->x != param5) + { + this->x = param5; + emit changed(this); + } +} + +void Waypoint::setParam6(double param6) +{ + if (this->z != param6) + { + this->z = param6; + emit changed(this); + } +} + +void Waypoint::setParam7(double param7) +{ + if (this->z != param7) + { + this->z = param7; + emit changed(this); + } +} + +void Waypoint::setLoiterOrbit(double orbit) { if (this->orbit != orbit) { @@ -177,54 +301,27 @@ void Waypoint::setOrbit(double orbit) void Waypoint::setHoldTime(int holdTime) { - if (this->holdTime != holdTime) - { - this->holdTime = holdTime; - emit changed(this); - } -} - -//void Waypoint::setX(double x) -//{ -// if (this->x != static_cast(x)) -// { -// this->x = x; -// emit changed(this); -// } -//} - -//void Waypoint::setY(double y) -//{ -// if (this->y != static_cast(y)) -// { -// this->y = y; -// emit changed(this); -// } -//} - -//void Waypoint::setZ(double z) -//{ -// if (this->z != static_cast(z)) -// { -// this->z = z; -// emit changed(this); -// } -//} - -//void Waypoint::setYaw(double yaw) -//{ -// if (this->yaw != static_cast(yaw)) -// { -// this->yaw = yaw; -// emit changed(this); -// } -//} - -//void Waypoint::setOrbit(double orbit) -//{ -// if (this->orbit != static_cast(orbit)) -// { -// this->orbit = orbit; -// emit changed(this); -// } -//} + if (this->param1 != holdTime) + { + this->param1 = holdTime; + emit changed(this); + } +} + +void Waypoint::setHoldTime(double holdTime) +{ + if (this->param1 != holdTime) + { + this->param1 = holdTime; + emit changed(this); + } +} + +void Waypoint::setTurns(int turns) +{ + if (this->param1 != turns) + { + this->param1 = turns; + emit changed(this); + } +} diff --git a/src/Waypoint.h b/src/Waypoint.h index afe529668108a4b7f260a49ced39659384fe20cf..e9607ab74dfebcfeae79ac7623c440f7de3af7a5 100644 --- a/src/Waypoint.h +++ b/src/Waypoint.h @@ -36,29 +36,40 @@ This file is part of the PIXHAWK project #include #include #include -#include "mavlink_types.h" +#include "QGCMAVLink.h" class Waypoint : public QObject { Q_OBJECT public: - Waypoint(quint16 id = 0, double x = 0.0f, double y = 0.0f, double z = 0.0f, double yaw = 0.0f, bool autocontinue = false, - bool current = false, double orbit = 0.15f, int holdTime = 0, - MAV_FRAME frame=MAV_FRAME_GLOBAL, MAV_ACTION action=MAV_ACTION_NAVIGATE); + Waypoint(quint16 id = 0, double x = 0.0, double y = 0.0, double z = 0.0, double param1 = 0.0, double param2 = 0.0, double param3 = 0.0, double param4 = 0.0, + bool autocontinue = true, bool current = false, MAV_FRAME frame=MAV_FRAME_GLOBAL, MAV_CMD action=MAV_CMD_NAV_WAYPOINT); ~Waypoint(); quint16 getId() const { return id; } double getX() const { return x; } double getY() const { return y; } double getZ() const { return z; } + double getLatitude() const { return x; } + double getLongitude() const { return y; } + double getAltitude() const { return z; } double getYaw() const { return yaw; } bool getAutoContinue() const { return autocontinue; } bool getCurrent() const { return current; } - double getOrbit() const { return orbit; } - double getHoldTime() const { return holdTime; } + double getLoiterOrbit() const { return orbit; } + double getAcceptanceRadius() const { return param2; } + double getHoldTime() const { return param1; } + double getParam1() const { return param1; } + double getParam2() const { return param2; } + double getParam3() const { return orbit; } + double getParam4() const { return yaw; } + double getParam5() const { return x; } + double getParam6() const { return y; } + double getParam7() const { return z; } + int getTurns() const { return param1; } MAV_FRAME getFrame() const { return frame; } - MAV_ACTION getAction() const { return action; } + MAV_CMD getAction() const { return action; } const QString& getName() const { return name; } void save(QTextStream &saveStream); @@ -72,11 +83,13 @@ protected: double z; double yaw; MAV_FRAME frame; - MAV_ACTION action; + MAV_CMD action; bool autocontinue; bool current; double orbit; - int holdTime; + double param1; + double param2; + int turns; QString name; public slots: @@ -84,21 +97,32 @@ public slots: void setX(double x); void setY(double y); void setZ(double z); + void setLatitude(double lat); + void setLongitude(double lon); + void setAltitude(double alt); + /** @brief Yaw angle in COMPASS DEGREES: 0-360 */ + void setYaw(int yaw); + /** @brief Yaw angle in COMPASS DEGREES: 0-360 */ void setYaw(double yaw); - void setAction(MAV_ACTION action); + /** @brief Set the waypoint action */ + void setAction(int action); + void setAction(MAV_CMD action); void setFrame(MAV_FRAME frame); void setAutocontinue(bool autoContinue); void setCurrent(bool current); - void setOrbit(double orbit); + void setLoiterOrbit(double orbit); + void setParam1(double param1); + void setParam2(double param2); + void setParam3(double param3); + void setParam4(double param4); + void setParam5(double param5); + void setParam6(double param6); + void setParam7(double param7); + void setAcceptanceRadius(double radius); void setHoldTime(int holdTime); - - -// //for QDoubleSpin -// void setX(double x); -// void setY(double y); -// void setZ(double z); -// void setYaw(double yaw); -// void setOrbit(double orbit); + void setHoldTime(double holdTime); + /** @brief Number of turns for loiter waypoints */ + void setTurns(int turns); signals: /** @brief Announces a change to the waypoint data */ diff --git a/src/comm/LinkManager.h b/src/comm/LinkManager.h index 6f2f919b2cf8ffed7ca4c37231499c28aedfaf3b..9d82b359d69c9793e290c6dbe2d185ed13f685a8 100644 --- a/src/comm/LinkManager.h +++ b/src/comm/LinkManager.h @@ -62,6 +62,9 @@ public: /** @brief Get a list of all links */ const QList getLinks(); + /** @brief Get a list of all protocols */ + const QList getProtocols() { return protocolLinks.uniqueKeys(); } + public slots: void add(LinkInterface* link); diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index 57ac970bb25f97bb66849b72fdc0fca32008dafa..5d088cc7aa4074ea32f5cd6b2f0e15f907c5b0fe 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -14,8 +14,10 @@ #include #include #include +#include +#include -#include "MG.h" +//#include "MG.h" #include "MAVLinkProtocol.h" #include "UASInterface.h" #include "UASManager.h" @@ -40,11 +42,18 @@ MAVLinkProtocol::MAVLinkProtocol() : heartbeatRate(MAVLINK_HEARTBEAT_DEFAULT_RATE), m_heartbeatsEnabled(false), m_loggingEnabled(false), - m_logfile(new QFile(QCoreApplication::applicationDirPath()+"/mavlink_packetlog.mavlink")), + m_logfile(NULL), m_enable_version_check(true), - versionMismatchIgnore(false) + m_paramRetransmissionTimeout(350), + m_paramRewriteTimeout(500), + m_paramGuardEnabled(true), + m_actionGuardEnabled(false), + m_actionRetransmissionTimeout(100), + versionMismatchIgnore(false), + systemId(QGC::defaultSystemId) { - start(QThread::LowPriority); + loadSettings(); + //start(QThread::LowPriority); // Start heartbeat timer, emitting a heartbeat at the configured rate connect(heartbeatTimer, SIGNAL(timeout()), this, SLOT(sendHeartbeat())); heartbeatTimer->start(1000/heartbeatRate); @@ -63,12 +72,79 @@ MAVLinkProtocol::MAVLinkProtocol() : emit versionCheckChanged(m_enable_version_check); } +void MAVLinkProtocol::loadSettings() +{ + // Load defaults from settings + QSettings settings; + settings.sync(); + settings.beginGroup("QGC_MAVLINK_PROTOCOL"); + enableHeartbeats(settings.value("HEARTBEATS_ENABLED", m_heartbeatsEnabled).toBool()); + enableVersionCheck(settings.value("VERSION_CHECK_ENABLED", m_enable_version_check).toBool()); + enableMultiplexing(settings.value("MULTIPLEXING_ENABLED", m_multiplexingEnabled).toBool()); + + // Only set logfile if there is a name present in settings + if (settings.contains("LOGFILE_NAME") && m_logfile == NULL) + { + m_logfile = new QFile(settings.value("LOGFILE_NAME").toString()); + } + else if (m_logfile == NULL) + { + m_logfile = new QFile(QDesktopServices::storageLocation(QDesktopServices::HomeLocation) + "/qgroundcontrol_packetlog.mavlink"); + } + // Enable logging + enableLogging(settings.value("LOGGING_ENABLED", m_loggingEnabled).toBool()); + + // Only set system id if it was valid + int temp = settings.value("GCS_SYSTEM_ID", systemId).toInt(); + if (temp > 0 && temp < 256) + { + systemId = temp; + } + + // Parameter interface settings + bool ok; + temp = settings.value("PARAMETER_RETRANSMISSION_TIMEOUT", m_paramRetransmissionTimeout).toInt(&ok); + if (ok) m_paramRetransmissionTimeout = temp; + temp = settings.value("PARAMETER_REWRITE_TIMEOUT", m_paramRewriteTimeout).toInt(&ok); + if (ok) m_paramRewriteTimeout = temp; + m_paramGuardEnabled = settings.value("PARAMETER_TRANSMISSION_GUARD_ENABLED", m_paramGuardEnabled).toBool(); + settings.endGroup(); +} + +void MAVLinkProtocol::storeSettings() +{ + // Store settings + QSettings settings; + settings.beginGroup("QGC_MAVLINK_PROTOCOL"); + settings.setValue("HEARTBEATS_ENABLED", m_heartbeatsEnabled); + settings.setValue("LOGGING_ENABLED", m_loggingEnabled); + settings.setValue("VERSION_CHECK_ENABLED", m_enable_version_check); + settings.setValue("MULTIPLEXING_ENABLED", m_multiplexingEnabled); + settings.setValue("GCS_SYSTEM_ID", systemId); + if (m_logfile) + { + // Logfile exists, store the name + settings.setValue("LOGFILE_NAME", m_logfile->fileName()); + } + // Parameter interface settings + settings.setValue("PARAMETER_RETRANSMISSION_TIMEOUT", m_paramRetransmissionTimeout); + settings.setValue("PARAMETER_REWRITE_TIMEOUT", m_paramRewriteTimeout); + settings.setValue("PARAMETER_TRANSMISSION_GUARD_ENABLED", m_paramGuardEnabled); + settings.endGroup(); + settings.sync(); + //qDebug() << "Storing settings!"; +} + MAVLinkProtocol::~MAVLinkProtocol() { + storeSettings(); if (m_logfile) { - m_logfile->flush(); - m_logfile->close(); + if (m_logfile->isOpen()) + { + m_logfile->flush(); + m_logfile->close(); + } delete m_logfile; } } @@ -77,15 +153,19 @@ MAVLinkProtocol::~MAVLinkProtocol() void MAVLinkProtocol::run() { - forever - { - QGC::SLEEP::msleep(5000); - } + exec(); } QString MAVLinkProtocol::getLogfileName() { - return m_logfile->fileName(); + if (m_logfile) + { + return m_logfile->fileName(); + } + else + { + return QDesktopServices::storageLocation(QDesktopServices::HomeLocation) + "/qgroundcontrol_packetlog.mavlink"; + } } /** @@ -108,20 +188,21 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) if (decodeState == 1) { // Log data - if (m_loggingEnabled) + if (m_loggingEnabled && m_logfile) { const int len = MAVLINK_MAX_PACKET_LEN+sizeof(quint64); uint8_t buf[len]; quint64 time = QGC::groundTimeUsecs(); memcpy(buf, (void*)&time, sizeof(quint64)); - // int packetlen = -// quint64 checktime = *((quint64*)buf); -// qDebug() << "TIME" << time << "CHECKTIME:" << checktime; + // Write message to buffer mavlink_msg_to_send_buffer(buf+sizeof(quint64), &message); QByteArray b((const char*)buf, len); - //int packetlen = - if(m_logfile->write(b) < MAVLINK_MAX_PACKET_LEN+sizeof(quint64)) qDebug() << "WRITING TO LOG FAILED!"; - //qDebug() << "WROTE LOGFILE"; + if(m_logfile->write(b) < static_cast(MAVLINK_MAX_PACKET_LEN+sizeof(quint64))) + { + emit protocolStatusMessage(tr("MAVLink Logging failed"), tr("Could not write to file %1, disabling logging.").arg(m_logfile->fileName())); + // Stop logging + enableLogging(false); + } } // ORDER MATTERS HERE! @@ -240,6 +321,21 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) // kind of inefficient, but no issue for a groundstation pc. // It buys as reentrancy for the whole code over all threads emit messageReceived(link, message); + + // Multiplex message if enabled + if (m_multiplexingEnabled) + { + // Get all links connected to this unit + QList links = LinkManager::instance()->getLinksForProtocol(this); + + // Emit message on all links that are currently connected + foreach (LinkInterface* currLink, links) + { + // Only forward this message to the other links, + // not the link the message was received on + if (currLink != link) sendMessage(currLink, message); + } + } } } } @@ -257,13 +353,18 @@ QString MAVLinkProtocol::getName() /** @return System id of this application */ int MAVLinkProtocol::getSystemId() { - return MG::SYSTEM::ID; + return systemId; +} + +void MAVLinkProtocol::setSystemId(int id) +{ + systemId = id; } /** @return Component id of this application */ int MAVLinkProtocol::getComponentId() { - return MG::SYSTEM::COMPID; + return QGC::defaultComponentId; } /** @@ -325,6 +426,60 @@ void MAVLinkProtocol::enableHeartbeats(bool enabled) emit heartbeatChanged(enabled); } +void MAVLinkProtocol::enableMultiplexing(bool enabled) +{ + bool changed = false; + if (enabled != m_multiplexingEnabled) changed = true; + + m_multiplexingEnabled = enabled; + if (changed) emit multiplexingChanged(m_multiplexingEnabled); +} + +void MAVLinkProtocol::enableParamGuard(bool enabled) +{ + if (enabled != m_paramGuardEnabled) + { + m_paramGuardEnabled = enabled; + emit paramGuardChanged(m_paramGuardEnabled); + } +} + +void MAVLinkProtocol::enableActionGuard(bool enabled) +{ + if (enabled != m_actionGuardEnabled) + { + m_actionGuardEnabled = enabled; + emit actionGuardChanged(m_actionGuardEnabled); + } +} + +void MAVLinkProtocol::setParamRetransmissionTimeout(int ms) +{ + if (ms != m_paramRetransmissionTimeout) + { + m_paramRetransmissionTimeout = ms; + emit paramRetransmissionTimeoutChanged(m_paramRetransmissionTimeout); + } +} + +void MAVLinkProtocol::setParamRewriteTimeout(int ms) +{ + if (ms != m_paramRewriteTimeout) + { + m_paramRewriteTimeout = ms; + emit paramRewriteTimeoutChanged(m_paramRewriteTimeout); + } +} + +void MAVLinkProtocol::setActionRetransmissionTimeout(int ms) +{ + if (ms != m_actionRetransmissionTimeout) + { + m_actionRetransmissionTimeout = ms; + emit actionRetransmissionTimeoutChanged(m_actionRetransmissionTimeout); + } +} + void MAVLinkProtocol::enableLogging(bool enabled) { bool changed = false; @@ -332,21 +487,32 @@ void MAVLinkProtocol::enableLogging(bool enabled) if (enabled) { - if (m_logfile->isOpen()) + if (m_logfile && m_logfile->isOpen()) { m_logfile->flush(); m_logfile->close(); } - if (!m_logfile->open(QIODevice::WriteOnly | QIODevice::Append)) + if (m_logfile) { - emit protocolStatusMessage(tr("Opening MAVLink logfile for writing failed"), tr("MAVLink cannot log to the file %1, please choose a different file.").arg(m_logfile->fileName())); - qDebug() << "OPENING LOGFILE FAILED!"; + if (!m_logfile->open(QIODevice::WriteOnly | QIODevice::Append)) + { + emit protocolStatusMessage(tr("Opening MAVLink logfile for writing failed"), tr("MAVLink cannot log to the file %1, please choose a different file. Stopping logging.").arg(m_logfile->fileName())); + m_loggingEnabled = false; + } } } else if (!enabled) { - m_logfile->flush(); - m_logfile->close(); + if (m_logfile) + { + if (m_logfile->isOpen()) + { + m_logfile->flush(); + m_logfile->close(); + } + delete m_logfile; + m_logfile = NULL; + } } m_loggingEnabled = enabled; if (changed) emit loggingChanged(enabled); @@ -354,8 +520,15 @@ void MAVLinkProtocol::enableLogging(bool enabled) void MAVLinkProtocol::setLogfileName(const QString& filename) { - m_logfile->flush(); - m_logfile->close(); + if (!m_logfile) + { + m_logfile = new QFile(filename); + } + else + { + m_logfile->flush(); + m_logfile->close(); + } m_logfile->setFileName(filename); enableLogging(m_loggingEnabled); } @@ -366,21 +539,6 @@ void MAVLinkProtocol::enableVersionCheck(bool enabled) emit versionCheckChanged(enabled); } -bool MAVLinkProtocol::heartbeatsEnabled(void) -{ - return m_heartbeatsEnabled; -} - -bool MAVLinkProtocol::loggingEnabled(void) -{ - return m_loggingEnabled; -} - -bool MAVLinkProtocol::versionCheckEnabled(void) -{ - return m_enable_version_check; -} - /** * The default rate is 1 Hertz. * diff --git a/src/comm/MAVLinkProtocol.h b/src/comm/MAVLinkProtocol.h index af1ba184078a927a3b468958c88281ec3539ed9b..f23cbf35bbcdc06f9a710b57f25bbf95a6d7b810 100644 --- a/src/comm/MAVLinkProtocol.h +++ b/src/comm/MAVLinkProtocol.h @@ -40,6 +40,7 @@ This file is part of the QGROUNDCONTROL project #include "ProtocolInterface.h" #include "LinkInterface.h" #include "QGCMAVLink.h" +#include "QGC.h" /** * @brief MAVLink micro air vehicle protocol reference implementation. @@ -65,15 +66,27 @@ public: /** @brief The auto heartbeat emission rate in Hertz */ int getHeartbeatRate(); /** @brief Get heartbeat state */ - bool heartbeatsEnabled(void); + bool heartbeatsEnabled() const { return m_heartbeatsEnabled; } /** @brief Get logging state */ - bool loggingEnabled(void); + bool loggingEnabled() const { return m_loggingEnabled; } /** @brief Get protocol version check state */ - bool versionCheckEnabled(void); + bool versionCheckEnabled() const { return m_enable_version_check; } + /** @brief Get the multiplexing state */ + bool multiplexingEnabled() const { return m_multiplexingEnabled; } /** @brief Get the protocol version */ int getVersion() { return MAVLINK_VERSION; } /** @brief Get the name of the packet log file */ QString getLogfileName(); + /** @brief Get state of parameter retransmission */ + bool paramGuardEnabled() { return m_paramGuardEnabled; } + /** @brief Get parameter read timeout */ + int getParamRetransmissionTimeout() { return m_paramRetransmissionTimeout; } + /** @brief Get parameter write timeout */ + int getParamRewriteTimeout() { return m_paramRewriteTimeout; } + /** @brief Get state of action retransmission */ + bool actionGuardEnabled() { return m_actionGuardEnabled; } + /** @brief Get parameter read timeout */ + int getActionRetransmissionTimeout() { return m_actionRetransmissionTimeout; } public slots: /** @brief Receive bytes from a communication interface */ @@ -84,6 +97,8 @@ public slots: void sendMessage(LinkInterface* link, mavlink_message_t message); /** @brief Set the rate at which heartbeats are emitted */ void setHeartbeatRate(int rate); + /** @brief Set the system id of this application */ + void setSystemId(int id); /** @brief Enable / disable the heartbeat emission */ void enableHeartbeats(bool enabled); @@ -91,6 +106,24 @@ public slots: /** @brief Enable/disable binary packet logging */ void enableLogging(bool enabled); + /** @brief Enabled/disable packet multiplexing */ + void enableMultiplexing(bool enabled); + + /** @brief Enable / disable parameter retransmission */ + void enableParamGuard(bool enabled); + + /** @brief Enable / disable action retransmission */ + void enableActionGuard(bool enabled); + + /** @brief Set parameter read timeout */ + void setParamRetransmissionTimeout(int ms); + + /** @brief Set parameter write timeout */ + void setParamRewriteTimeout(int ms); + + /** @brief Set parameter read timeout */ + void setActionRetransmissionTimeout(int ms); + /** @brief Set log file name */ void setLogfileName(const QString& filename); @@ -100,13 +133,24 @@ public slots: /** @brief Send an extra heartbeat to all connected units */ void sendHeartbeat(); + /** @brief Load protocol settings */ + void loadSettings(); + /** @brief Store protocol settings */ + void storeSettings(); + protected: QTimer* heartbeatTimer; ///< Timer to emit heartbeats int heartbeatRate; ///< Heartbeat rate, controls the timer interval bool m_heartbeatsEnabled; ///< Enabled/disable heartbeat emission bool m_loggingEnabled; ///< Enable/disable packet logging + bool m_multiplexingEnabled; ///< Enable/disable packet multiplexing QFile* m_logfile; ///< Logfile bool m_enable_version_check; ///< Enable checking of version match of MAV and QGC + int m_paramRetransmissionTimeout; ///< Timeout for parameter retransmission + int m_paramRewriteTimeout; ///< Timeout for sending re-write request + bool m_paramGuardEnabled; ///< Parameter retransmission/rewrite enabled + bool m_actionGuardEnabled; ///< Action request retransmission enabled + int m_actionRetransmissionTimeout; ///< Timeout for parameter retransmission QMutex receiveMutex; ///< Mutex to protect receiveBytes function int lastIndex[256][256]; int totalReceiveCounter; @@ -114,6 +158,7 @@ protected: int currReceiveCounter; int currLossCounter; bool versionMismatchIgnore; + int systemId; signals: /** @brief Message received and directly copied via signal */ @@ -122,10 +167,24 @@ signals: void heartbeatChanged(bool heartbeats); /** @brief Emitted if logging is started / stopped */ void loggingChanged(bool enabled); + /** @brief Emitted if multiplexing is started / stopped */ + void multiplexingChanged(bool enabled); /** @brief Emitted if version check is enabled / disabled */ void versionCheckChanged(bool enabled); /** @brief Emitted if a message from the protocol should reach the user */ void protocolStatusMessage(const QString& title, const QString& message); + /** @brief Emitted if a new system ID was set */ + void systemIdChanged(int systemId); + /** @brief Emitted if param guard status changed */ + void paramGuardChanged(bool enabled); + /** @brief Emitted if param read timeout changed */ + void paramRetransmissionTimeoutChanged(int ms); + /** @brief Emitted if param write timeout changed */ + void paramRewriteTimeoutChanged(int ms); + /** @brief Emitted if action guard status changed */ + void actionGuardChanged(bool enabled); + /** @brief Emitted if actiion request timeout changed */ + void actionRetransmissionTimeoutChanged(int ms); }; #endif // MAVLINKPROTOCOL_H_ diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc index 6524f46d17508c5b446e197a05b61ac7dae953df..9cac0f6f0b11cd5405209a7e703df4ed01bf1e07 100644 --- a/src/comm/MAVLinkSimulationLink.cc +++ b/src/comm/MAVLinkSimulationLink.cc @@ -114,7 +114,6 @@ void MAVLinkSimulationLink::run() status.mode = MAV_MODE_UNINIT; status.status = MAV_STATE_UNINIT; status.vbat = 0; - status.motor_block = 1; status.packet_drop = 0; @@ -182,10 +181,10 @@ void MAVLinkSimulationLink::mainloop() // Fake system values - static float fullVoltage = 4.2 * 3; - static float emptyVoltage = 3.35 * 3; + static float fullVoltage = 4.2f * 3.0f; + static float emptyVoltage = 3.35f * 3.0f; static float voltage = fullVoltage; - static float drainRate = 0.025; // x.xx% of the capacity is linearly drained per second + static float drainRate = 0.025f; // x.xx% of the capacity is linearly drained per second mavlink_attitude_t attitude; memset(&attitude, 0, sizeof(mavlink_attitude_t)); @@ -212,7 +211,7 @@ void MAVLinkSimulationLink::mainloop() // VOLTAGE // The battery is drained constantly voltage = voltage - ((fullVoltage - emptyVoltage) * drainRate / rate); - if (voltage < 3.550 * 3) voltage = 3.550 * 3; + if (voltage < 3.550f * 3.0f) voltage = 3.550f * 3.0f; static int state = 0; @@ -611,17 +610,18 @@ void MAVLinkSimulationLink::mainloop() // Send controller states - #ifdef MAVLINK_ENABLED_PIXHAWK - uint8_t attControl = 1; - uint8_t posXYControl = 1; - uint8_t posZControl = 0; - uint8_t posYawControl = 1; - - uint8_t gpsLock = 2; - uint8_t visLock = 3; - uint8_t posLock = qMax(gpsLock, visLock); - messageSize = mavlink_msg_control_status_pack(systemId, componentId, &msg, posLock, visLock, gpsLock, attControl, posXYControl, posZControl, posYawControl); - #endif +#ifdef MAVLINK_ENABLED_PIXHAWK + uint8_t attControl = 1; + uint8_t posXYControl = 1; + uint8_t posZControl = 0; + uint8_t posYawControl = 1; + + uint8_t gpsLock = 2; + uint8_t visLock = 3; + uint8_t ahrsHealth = 200; + uint8_t posLock = qMax(gpsLock, visLock); + messageSize = mavlink_msg_control_status_pack(systemId, componentId, &msg, posLock, visLock, gpsLock, ahrsHealth, attControl, posXYControl, posZControl, posYawControl); +#endif bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); memcpy(stream+streampointer, buffer, bufferlength); @@ -817,8 +817,8 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size) qDebug() << "GCS REQUESTED PARAM LIST FROM SIMULATION"; mavlink_param_request_list_t read; mavlink_msg_param_request_list_decode(&msg, &read); - if (read.target_system == systemId) - { +// if (read.target_system == systemId) +// { // Output all params // Iterate through all components, through all parameters and emit them QMap::iterator i; @@ -826,27 +826,33 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size) int j = 0; for (i = onboardParams.begin(); i != onboardParams.end(); ++i) { - // Pack message and get size of encoded byte string - mavlink_msg_param_value_pack(systemId, componentId, &msg, (int8_t*)i.key().toStdString().c_str(), i.value(), onboardParams.size(), j); - // Allocate buffer with packet data - bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); - //add data into datastream - memcpy(stream+streampointer,buffer, bufferlength); - streampointer+=bufferlength; + if (j != 5) + { + // Pack message and get size of encoded byte string + mavlink_msg_param_value_pack(read.target_system, componentId, &msg, (int8_t*)i.key().toStdString().c_str(), i.value(), onboardParams.size(), j); + // Allocate buffer with packet data + bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); + //add data into datastream + memcpy(stream+streampointer,buffer, bufferlength); + streampointer+=bufferlength; + } j++; } qDebug() << "SIMULATION SENT PARAMETERS TO GCS"; - } +// } } break; case MAVLINK_MSG_ID_PARAM_SET: { - qDebug() << "SIMULATION RECEIVED COMMAND TO SET PARAMETER"; - mavlink_param_set_t set; - mavlink_msg_param_set_decode(&msg, &set); - if (set.target_system == systemId) + // Drop on even milliseconds + if (QGC::groundTimeMilliseconds() % 2 == 0) { + qDebug() << "SIMULATION RECEIVED COMMAND TO SET PARAMETER"; + mavlink_param_set_t set; + mavlink_msg_param_set_decode(&msg, &set); + // if (set.target_system == systemId) + // { QString key = QString((char*)set.param_id); if (onboardParams.contains(key)) { @@ -854,13 +860,50 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size) onboardParams.insert(key, set.param_value); // Pack message and get size of encoded byte string - mavlink_msg_param_value_pack(systemId, componentId, &msg, (int8_t*)key.toStdString().c_str(), set.param_value, onboardParams.size(), 0); + mavlink_msg_param_value_pack(set.target_system, componentId, &msg, (int8_t*)key.toStdString().c_str(), set.param_value, onboardParams.size(), onboardParams.keys().indexOf(key)); // Allocate buffer with packet data bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); //add data into datastream memcpy(stream+streampointer,buffer, bufferlength); streampointer+=bufferlength; } + // } + } + } + break; + case MAVLINK_MSG_ID_PARAM_REQUEST_READ: + { + qDebug() << "SIMULATION RECEIVED COMMAND TO SEND PARAMETER"; + mavlink_param_request_read_t read; + mavlink_msg_param_request_read_decode(&msg, &read); + QByteArray bytes((char*)read.param_id, MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN); + QString key = QString(bytes); + if (onboardParams.contains(key)) + { + float paramValue = onboardParams.value(key); + + // Pack message and get size of encoded byte string + mavlink_msg_param_value_pack(read.target_system, componentId, &msg, (int8_t*)key.toStdString().c_str(), paramValue, onboardParams.size(), onboardParams.keys().indexOf(key)); + // Allocate buffer with packet data + bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); + //add data into datastream + memcpy(stream+streampointer,buffer, bufferlength); + streampointer+=bufferlength; + //qDebug() << "Sending PARAM" << key; + } + else if (read.param_index < onboardParams.size()) + { + key = onboardParams.keys().at(read.param_index); + float paramValue = onboardParams.value(key); + + // Pack message and get size of encoded byte string + mavlink_msg_param_value_pack(read.target_system, componentId, &msg, (int8_t*)key.toStdString().c_str(), paramValue, onboardParams.size(), onboardParams.keys().indexOf(key)); + // Allocate buffer with packet data + bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); + //add data into datastream + memcpy(stream+streampointer,buffer, bufferlength); + streampointer+=bufferlength; + //qDebug() << "Sending PARAM #ID" << (read.param_index) << "KEY:" << key; } } break; @@ -868,10 +911,10 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size) } - //unsigned char v=data[i]; - //fprintf(stderr,"%02x ", v); + unsigned char v=data[i]; + fprintf(stderr,"%02x ", v); } - //fprintf(stderr,"\n"); + fprintf(stderr,"\n"); readyBufferMutex.lock(); for (int i = 0; i < streampointer; i++) @@ -903,22 +946,22 @@ void MAVLinkSimulationLink::readBytes() { readyBufferMutex.unlock(); - // if (len > 0) - // { - // qDebug() << "Simulation sent " << len << " bytes to groundstation: "; - // - // /* Increase write counter */ - // //bitsSentTotal += size * 8; - // - // //Output all bytes as hex digits - // int i; - // for (i=0; i 0) +// { +// qDebug() << "Simulation sent " << len << " bytes to groundstation: "; + +// /* Increase write counter */ +// //bitsSentTotal += size * 8; + +// //Output all bytes as hex digits +// int i; +// for (i=0; istart(rate); return true; } diff --git a/src/comm/MAVLinkSimulationMAV.cc b/src/comm/MAVLinkSimulationMAV.cc index 8f8e8eebbe224ac952ad8bd2dbf4eca70e2495a5..92f8877f7cd3d2f34938c7f514434d33f0a34e56 100644 --- a/src/comm/MAVLinkSimulationMAV.cc +++ b/src/comm/MAVLinkSimulationMAV.cc @@ -1,9 +1,10 @@ #include #include +#include #include "MAVLinkSimulationMAV.h" -MAVLinkSimulationMAV::MAVLinkSimulationMAV(MAVLinkSimulationLink *parent, int systemid, double lat, double lon) : +MAVLinkSimulationMAV::MAVLinkSimulationMAV(MAVLinkSimulationLink *parent, int systemid, double lat, double lon, int version) : QObject(parent), link(parent), planner(parent, systemid), @@ -14,8 +15,8 @@ MAVLinkSimulationMAV::MAVLinkSimulationMAV(MAVLinkSimulationLink *parent, int sy latitude(lat), longitude(lon), altitude(0.0), - x(lon), - y(lat), + x(lat), + y(lon), z(550), roll(0.0), pitch(0.0), @@ -33,7 +34,8 @@ MAVLinkSimulationMAV::MAVLinkSimulationMAV(MAVLinkSimulationLink *parent, int sy sys_mode(MAV_MODE_READY), sys_state(MAV_STATE_STANDBY), nav_mode(MAV_NAV_GROUNDED), - flying(false) + flying(false), + mavlink_version(version) { // Please note: The waypoint planner is running connect(&mainloopTimer, SIGNAL(timeout()), this, SLOT(mainloop())); @@ -61,28 +63,41 @@ void MAVLinkSimulationMAV::mainloop() if (timer1Hz <= 0) { mavlink_message_t msg; - mavlink_msg_heartbeat_pack(systemid, MAV_COMP_ID_IMU, &msg, MAV_FIXED_WING, MAV_AUTOPILOT_PIXHAWK); + mavlink_msg_heartbeat_pack_version_free(systemid, MAV_COMP_ID_IMU, &msg, MAV_FIXED_WING, MAV_AUTOPILOT_PIXHAWK, mavlink_version); link->sendMAVLinkMessage(&msg); planner.handleMessage(msg); + + mavlink_servo_output_raw_t servos; + servos.servo1_raw = 1000; + servos.servo2_raw = 1250; + servos.servo3_raw = 1400; + servos.servo4_raw = 1500; + servos.servo5_raw = 1800; + servos.servo6_raw = 1500; + servos.servo7_raw = 1500; + servos.servo8_raw = 2000; + + mavlink_msg_servo_output_raw_encode(systemid, MAV_COMP_ID_IMU, &msg, &servos); + link->sendMAVLinkMessage(&msg); timer1Hz = 50; } // 10 Hz execution if (timer10Hz <= 0) { - if (!firstWP) - { - double radPer100ms = 0.00006; - double altPer100ms = 1.0; - double xm = (nextSPX - x); - double ym = (nextSPY - y); - double zm = (nextSPZ - z); + double radPer100ms = 0.00006; + double altPer100ms = 0.4; + double xm = (nextSPX - x); + double ym = (nextSPY - y); + double zm = (nextSPZ - z); - float zsign = (zm < 0) ? -1.0f : 1.0f; + float zsign = (zm < 0) ? -1.0f : 1.0f; + if (!firstWP) + { //float trueyaw = atan2f(xm, ym); - float newYaw = atan2f(xm, ym); + float newYaw = atan2f(ym, xm); if (fabs(yaw - newYaw) < 90) { @@ -98,8 +113,8 @@ void MAVLinkSimulationMAV::mainloop() //if (sqrt(xm*xm+ym*ym) > 0.0000001) if (flying) { - x += sin(yaw)*radPer100ms; - y += cos(yaw)*radPer100ms; + x += cos(yaw)*radPer100ms; + y += sin(yaw)*radPer100ms; z += altPer100ms*zsign; } @@ -119,20 +134,24 @@ void MAVLinkSimulationMAV::mainloop() mavlink_message_t msg; mavlink_global_position_int_t pos; pos.alt = z*1000.0; - pos.lat = y*1E7; - pos.lon = x*1E7; - pos.vx = 0; + pos.lat = x*1E7; + pos.lon = y*1E7; + pos.vx = sin(yaw)*10.0f*100.0f; pos.vy = 0; - pos.vz = 0; + pos.vz = altPer100ms*10.0f*100.0f*zsign*-1.0f; mavlink_msg_global_position_int_encode(systemid, MAV_COMP_ID_IMU, &msg, &pos); link->sendMAVLinkMessage(&msg); planner.handleMessage(msg); // ATTITUDE mavlink_attitude_t attitude; + attitude.usec = 0; attitude.roll = 0.0f; attitude.pitch = 0.0f; attitude.yaw = yaw; + attitude.rollspeed = 0.0f; + attitude.pitchspeed = 0.0f; + attitude.yawspeed = 0.0f; mavlink_msg_attitude_encode(systemid, MAV_COMP_ID_IMU, &msg, &attitude); link->sendMAVLinkMessage(&msg); @@ -140,26 +159,86 @@ void MAVLinkSimulationMAV::mainloop() // SYSTEM STATUS mavlink_sys_status_t status; status.load = 300; - status.motor_block = 1; status.mode = sys_mode; status.nav_mode = nav_mode; status.packet_drop = 0; status.vbat = 10500; status.status = sys_state; - + status.battery_remaining = 912; mavlink_msg_sys_status_encode(systemid, MAV_COMP_ID_IMU, &msg, &status); link->sendMAVLinkMessage(&msg); timer10Hz = 5; + + // VFR HUD + mavlink_vfr_hud_t hud; + hud.airspeed = pos.vx/100.0f; + hud.groundspeed = pos.vx/100.0f; + hud.alt = z; + hud.heading = static_cast((yaw/M_PI)*180.0f+180.0f) % 360; + hud.climb = pos.vz/100.0f; + hud.throttle = 90; + mavlink_msg_vfr_hud_encode(systemid, MAV_COMP_ID_IMU, &msg, &hud); + link->sendMAVLinkMessage(&msg); + + // NAV CONTROLLER + mavlink_nav_controller_output_t nav; + nav.nav_roll = roll; + nav.nav_pitch = pitch; + nav.nav_bearing = yaw; + nav.target_bearing = yaw; + nav.wp_dist = 2.0f; + nav.alt_error = 0.5f; + nav.xtrack_error = 0.2f; + nav.aspd_error = 0.0f; + mavlink_msg_nav_controller_output_encode(systemid, MAV_COMP_ID_IMU, &msg, &nav); + link->sendMAVLinkMessage(&msg); + + // RAW PRESSURE + mavlink_raw_pressure_t pressure; + pressure.press_abs = 1000; + pressure.press_diff1 = 2000; + pressure.press_diff2 = 5000; + pressure.temperature = 18150; // 18.15 deg Celsius + pressure.usec = 0; // Works also with zero timestamp + mavlink_msg_raw_pressure_encode(systemid, MAV_COMP_ID_IMU, &msg, &pressure); + link->sendMAVLinkMessage(&msg); } // 25 Hz execution if (timer25Hz <= 0) { + // The message container to be used for sending + mavlink_message_t ret; + + #ifdef MAVLINK_ENABLED_PIXHAWK + // Send which controllers are active + mavlink_control_status_t control_status; + control_status.control_att = 1; + control_status.control_pos_xy = 1; + control_status.control_pos_yaw = 1; + control_status.control_pos_z = 1; + control_status.gps_fix = 2; // 2D GPS fix + control_status.position_fix = 3; // 3D fix from GPS + barometric pressure + control_status.vision_fix = 0; // no fix from vision system + control_status.ahrs_health = 230; + mavlink_msg_control_status_encode(systemid, MAV_COMP_ID_IMU, &ret, &control_status); + link->sendMAVLinkMessage(&ret); + #endif //MAVLINK_ENABLED_PIXHAWK + + // Send actual controller outputs + // This message just shows the direction + // and magnitude of the control output +// mavlink_position_controller_output_t pos; +// pos.x = sin(yaw)*127.0f; +// pos.y = cos(yaw)*127.0f; +// pos.z = 0; +// mavlink_msg_position_controller_output_encode(systemid, MAV_COMP_ID_IMU, &ret, &pos); +// link->sendMAVLinkMessage(&ret); // Send a named value with name "FLOAT" and 0.5f as value // The message container to be used for sending - mavlink_message_t ret; + //mavlink_message_t ret; // The C-struct holding the data to be sent mavlink_named_value_float_t val; @@ -167,7 +246,7 @@ void MAVLinkSimulationMAV::mainloop() // Name of the value, maximum 10 characters // see full message specs at: // http://pixhawk.ethz.ch/wiki/mavlink/ - strcpy(val.name, "FLOAT"); + strcpy((char *)val.name, "FLOAT"); // Value, in this case 0.5 val.value = 0.5f; @@ -195,7 +274,7 @@ void MAVLinkSimulationMAV::mainloop() // Name of the value, maximum 10 characters // see full message specs at: // http://pixhawk.ethz.ch/wiki/mavlink/ - strcpy(valint.name, "INTEGER"); + strcpy((char *)valint.name, "INTEGER"); // Value, in this case 18000 valint.value = 18000; @@ -239,21 +318,26 @@ void MAVLinkSimulationMAV::handleMessage(const mavlink_message_t& msg) mavlink_msg_action_decode(&msg, &action); if (systemid == action.target && (action.target_component == 0 || action.target_component == MAV_COMP_ID_IMU)) { + mavlink_action_ack_t ack; + ack.action = action.action; switch (action.action) { - case MAV_ACTION_LAUNCH: + case MAV_ACTION_TAKEOFF: flying = true; + nav_mode = MAV_NAV_LIFTOFF; + ack.result = 1; break; default: { - mavlink_statustext_t text; - mavlink_message_t r_msg; - sprintf((char*)text.text, "MAV%d ignored unknown action %d", systemid, action.action); - mavlink_msg_statustext_encode(systemid, MAV_COMP_ID_IMU, &r_msg, &text); - link->sendMAVLinkMessage(&r_msg); + ack.result = 0; } break; } + + // Give feedback about action + mavlink_message_t r_msg; + mavlink_msg_action_ack_encode(systemid, MAV_COMP_ID_IMU, &r_msg, &ack); + link->sendMAVLinkMessage(&r_msg); } } break; @@ -263,6 +347,7 @@ void MAVLinkSimulationMAV::handleMessage(const mavlink_message_t& msg) mavlink_msg_local_position_setpoint_set_decode(&msg, &sp); if (sp.target_system == this->systemid) { + nav_mode = MAV_NAV_WAYPOINT; previousSPX = nextSPX; previousSPY = nextSPY; previousSPZ = nextSPZ; diff --git a/src/comm/MAVLinkSimulationMAV.h b/src/comm/MAVLinkSimulationMAV.h index 3a9d37adaec3db61513cf332de520b04f76fb2cd..32f04d8445550133f59adbcd9cfc5fdae319848d 100644 --- a/src/comm/MAVLinkSimulationMAV.h +++ b/src/comm/MAVLinkSimulationMAV.h @@ -11,7 +11,7 @@ class MAVLinkSimulationMAV : public QObject { Q_OBJECT public: - explicit MAVLinkSimulationMAV(MAVLinkSimulationLink *parent, int systemid, double lat=47.376389, double lon=8.548056); + explicit MAVLinkSimulationMAV(MAVLinkSimulationLink *parent, int systemid, double lat=47.376389, double lon=8.548056, int version=MAVLINK_VERSION); signals: @@ -53,6 +53,19 @@ protected: uint8_t sys_state; uint8_t nav_mode; bool flying; + int mavlink_version; + + static inline uint16_t mavlink_msg_heartbeat_pack_version_free(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t type, uint8_t autopilot, uint8_t version) + { + uint16_t i = 0; + msg->msgid = MAVLINK_MSG_ID_HEARTBEAT; + + i += put_uint8_t_by_index(type, i, msg->payload); // Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + i += put_uint8_t_by_index(autopilot, i, msg->payload); // Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM + i += put_uint8_t_by_index(version, i, msg->payload); // MAVLink version + + return mavlink_finalize_message(msg, system_id, component_id, i); + } }; diff --git a/src/comm/MAVLinkSimulationWaypointPlanner.cc b/src/comm/MAVLinkSimulationWaypointPlanner.cc index 645096b60abf00934f473ea4e619d5fcf402603b..d86eaa8a84ea77adf1bcb6d9d22ab0e2490c6e8b 100644 --- a/src/comm/MAVLinkSimulationWaypointPlanner.cc +++ b/src/comm/MAVLinkSimulationWaypointPlanner.cc @@ -205,6 +205,7 @@ MAVLinkSimulationWaypointPlanner::MAVLinkSimulationWaypointPlanner(MAVLinkSimula silent(false) { connect(parent, SIGNAL(messageReceived(mavlink_message_t)), this, SLOT(handleMessage(mavlink_message_t))); + qDebug() << "PLANNER FOR SYSTEM" << systemid << "INITIALIZED"; } @@ -283,7 +284,7 @@ void MAVLinkSimulationWaypointPlanner::send_setpoint(uint16_t seq) PControlSetPoint.x = cur->x; PControlSetPoint.y = cur->y; PControlSetPoint.z = cur->z; - PControlSetPoint.yaw = cur->yaw; + PControlSetPoint.yaw = cur->param4; mavlink_msg_local_position_setpoint_set_encode(systemid, compid, &msg, &PControlSetPoint); link->sendMAVLinkMessage(&msg); @@ -298,7 +299,7 @@ void MAVLinkSimulationWaypointPlanner::send_setpoint(uint16_t seq) PControlSetPoint.x = cur->x; PControlSetPoint.y = cur->y; PControlSetPoint.z = cur->z; - PControlSetPoint.yaw = cur->yaw; + PControlSetPoint.yaw = cur->param4; mavlink_msg_local_position_setpoint_set_encode(systemid, compid, &msg, &PControlSetPoint); link->sendMAVLinkMessage(&msg); @@ -336,7 +337,7 @@ void MAVLinkSimulationWaypointPlanner::send_waypoint(uint8_t target_systemid, ui wp->target_system = target_systemid; wp->target_component = target_compid; - if (verbose) qDebug("Sent waypoint %u (%u / %u / %u / %u / %u / %f / %u / %f / %f / %u / %f / %f / %f / %f / %u)\n", wp->seq, wp->target_system, wp->target_component, wp->seq, wp->frame, wp->action, wp->orbit, wp->orbit_direction, wp->param1, wp->param2, wp->current, wp->x, wp->y, wp->z, wp->yaw, wp->autocontinue); + if (verbose) qDebug("Sent waypoint %u (%u / %u / %u / %u / %u / %f / %f / %f / %u / %f / %f / %f / %f / %u)\n", wp->seq, wp->target_system, wp->target_component, wp->seq, wp->frame, wp->command, wp->param3, wp->param1, wp->param2, wp->current, wp->x, wp->y, wp->z, wp->param4, wp->autocontinue); mavlink_msg_waypoint_encode(systemid, compid, &msg, wp); link->sendMAVLinkMessage(&msg); @@ -501,19 +502,19 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* //compare current yaw if (att.yaw - yaw_tolerance >= 0.0f && att.yaw + yaw_tolerance < 2.f*M_PI) { - if (att.yaw - yaw_tolerance <= wp->yaw && att.yaw + yaw_tolerance >= wp->yaw) + if (att.yaw - yaw_tolerance <= wp->param4 && att.yaw + yaw_tolerance >= wp->param4) yawReached = true; } else if(att.yaw - yaw_tolerance < 0.0f) { float lowerBound = 360.0f + att.yaw - yaw_tolerance; - if (lowerBound < wp->yaw || wp->yaw < att.yaw + yaw_tolerance) + if (lowerBound < wp->param4 || wp->param4 < att.yaw + yaw_tolerance) yawReached = true; } else { float upperBound = att.yaw + yaw_tolerance - 2.f*M_PI; - if (att.yaw - yaw_tolerance < wp->yaw || wp->yaw < upperBound) + if (att.yaw - yaw_tolerance < wp->param4 || wp->param4 < upperBound) yawReached = true; } @@ -535,7 +536,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* { mavlink_local_position_t pos; mavlink_msg_local_position_decode(msg, &pos); - qDebug() << "Received new position: x:" << pos.x << "| y:" << pos.y << "| z:" << pos.z; + //qDebug() << "Received new position: x:" << pos.x << "| y:" << pos.y << "| z:" << pos.z; posReached = false; @@ -572,18 +573,18 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* mavlink_global_position_int_t pos; mavlink_msg_global_position_int_decode(msg, &pos); - float x = static_cast(pos.lon)/1E7; - float y = static_cast(pos.lat)/1E7; - float z = static_cast(pos.alt)/1000; + float x = static_cast(pos.lat)/1E7; + float y = static_cast(pos.lon)/1E7; + //float z = static_cast(pos.alt)/1000; - qDebug() << "Received new position: x:" << x << "| y:" << y << "| z:" << z; + //qDebug() << "Received new position: x:" << x << "| y:" << y << "| z:" << z; posReached = false; yawReached = true; // FIXME big hack for simulation! //float oneDegreeOfLatMeters = 111131.745f; - float orbit = 0.00008; + float orbit = 0.00008f; // compare current position (given in message) with current waypoint //float orbit = wp->param1; @@ -706,6 +707,10 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* } } } + else + { + qDebug() << "SYSTEM / COMPONENT ID MISMATCH: target sys:" << wpc.target_system << "this system:" << systemid << "target comp:" << wpc.target_component << "this comp:" << compid; + } break; } @@ -1004,7 +1009,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* if (cur_wp->autocontinue) { cur_wp->current = 0; - if (current_active_wp_id == waypoints->size() - 1 && waypoints->size() > 1) + if (current_active_wp_id == waypoints->size() - 1 && waypoints->size() > 0) { //the last waypoint was reached, if auto continue is //activated restart the waypoint list from the beginning diff --git a/src/comm/MAVLinkXMLParser.cc b/src/comm/MAVLinkXMLParser.cc index 34b830fe66579da12eb88d5dad3eda3255162f54..792883e123b410a9d351a5784fa2f79d24531571 100644 --- a/src/comm/MAVLinkXMLParser.cc +++ b/src/comm/MAVLinkXMLParser.cc @@ -247,11 +247,11 @@ bool MAVLinkXMLParser::generate() // Everything sane, starting with enum content currEnum = "enum " + enumName.toUpper() + "\n{\n"; - currEnumEnd = "};\n\n"; + currEnumEnd = QString("\t%1_ENUM_END\n};\n\n").arg(enumName.toUpper()); int nextEnumValue = 0; - // Get the message fields + // Get the enum fields QDomNode f = e.firstChild(); while (!f.isNull()) { @@ -285,20 +285,22 @@ bool MAVLinkXMLParser::generate() if (e2.text().length() > 0) { fieldComment = " /* " + e2.text() + "*/"; + fieldComment = fieldComment.replace("\n", " "); } currEnum += "\t" + fieldName.toUpper() + "=" + fieldValue + "," + fieldComment + "\n"; } else if(!e2.isNull() && e2.tagName() == "description") { - comment = e2.text() + comment; + comment = e2.text().replace("\n", " ") + comment; } f = f.nextSibling(); } } // Add the last parsed enum // Remove the last comma, as the last value has none - int commaPosition = currEnum.lastIndexOf(","); - currEnum.remove(commaPosition, 1); + // ENUM END MARKER IS LAST ENTRY, COMMA REMOVAL NOT NEEDED + //int commaPosition = currEnum.lastIndexOf(","); + //currEnum.remove(commaPosition, 1); enums += "/** @brief " + comment + " */\n" + currEnum + currEnumEnd; } // Element is non-zero and element name is @@ -407,7 +409,7 @@ bool MAVLinkXMLParser::generate() { // Send arguments are the same for integral types and arrays sendArguments += ", " + fieldName; - commentLines += commentEntry.arg(fieldName, fieldText); + commentLines += commentEntry.arg(fieldName, fieldText.replace("\n", " ")); } // MAVLink version field diff --git a/src/comm/SerialLink.cc b/src/comm/SerialLink.cc index 80600efa690c63fe6187cf9289ebf169c7215159..d6a3cc5389ea28083b447de589a3d44d356b8073 100644 --- a/src/comm/SerialLink.cc +++ b/src/comm/SerialLink.cc @@ -39,22 +39,12 @@ SerialLink::SerialLink(QString portname, BaudRateType baudrate, FlowType flow, P // Set unique ID and add link to the list of links this->id = getNextLinkId(); - // *nix (Linux, MacOS tested) serial port support -// port = new QextSerialPort(porthandle, QextSerialPort::Polling); - //port = new QextSerialPort(porthandle, QextSerialPort::EventDriven); - this->baudrate = baudrate; this->flow = flow; this->parity = parity; this->dataBits = dataBits; this->stopBits = stopBits; this->timeout = 1; ///< The timeout controls how long the program flow should wait for new serial bytes. As we're polling, we don't want to wait at all. -// port->setTimeout(timeout); // Timeout of 0 ms, we don't want to wait for data, we just poll again next time -// port->setBaudRate(baudrate); -// port->setFlowControl(flow); -// port->setParity(parity); -// port->setDataBits(dataBits); -// port->setStopBits(stopBits); // Set the port name if (porthandle == "") @@ -103,7 +93,7 @@ void SerialLink::loadSettings() settings.sync(); if (settings.contains("SERIALLINK_COMM_PORT")) { - setPortName(settings.value("SERIALLINK_COMM_PORT").toString()); + if (porthandle == "") setPortName(settings.value("SERIALLINK_COMM_PORT").toString()); setBaudRateType(settings.value("SERIALLINK_COMM_BAUD").toInt()); setParityType(settings.value("SERIALLINK_COMM_PARITY").toInt()); setStopBits(settings.value("SERIALLINK_COMM_STOPBITS").toInt()); @@ -179,18 +169,18 @@ void SerialLink::writeBytes(const char* data, qint64 size) if (b > 0) { - qDebug() << "Serial link " << this->getName() << "transmitted" << b << "bytes:"; +// qDebug() << "Serial link " << this->getName() << "transmitted" << b << "bytes:"; // Increase write counter bitsSentTotal += size * 8; - int i; - for (i=0; isetLocalAddress(QHostAddress(address)); } -void UDPLink::setPort(quint16 port) +void UDPLink::setPort(int port) { this->port = port; + disconnect(); + connect(); +} + +/** + * @param host Hostname in standard formatting, e.g. localhost:14551 or 192.168.1.1:14551 + */ +void UDPLink::addHost(const QString& host) +{ + if (host.contains(":")) + { + // Add host + hosts->append(QHostAddress(host.split(":").first())); + // Set port according to user input + ports->append(host.split(":").last().toInt()); + } + else + { + // Add host + hosts->append(QHostAddress(host)); + // Set port according to default (this port) + ports->append(port); + } } diff --git a/src/comm/UDPLink.h b/src/comm/UDPLink.h index fb1eb079c09cfbc57c209ebfe89c5f873cf4117e..cf5bcd573e7be98dfb83560e084613b81d7127e9 100644 --- a/src/comm/UDPLink.h +++ b/src/comm/UDPLink.h @@ -51,6 +51,7 @@ public: bool isConnected(); qint64 bytesAvailable(); + int getPort() const { return port; } /** * @brief The human readable port name @@ -82,7 +83,9 @@ public: public slots: void setAddress(QString address); - void setPort(quint16 port); + void setPort(int port); + /** @brief Add a new host to broadcast messages to */ + void addHost(const QString& host); // void readPendingDatagrams(); void readBytes(); diff --git a/src/configuration.h b/src/configuration.h index 51dec3d2b83e8f2472a2b40dd4a05e792fa46465..1285ebf954f8cb5bc3ac412ff0f77270eca40b51 100644 --- a/src/configuration.h +++ b/src/configuration.h @@ -17,7 +17,7 @@ #define WITH_TEXT_TO_SPEECH 1 #define QGC_APPLICATION_NAME "QGroundControl" -#define QGC_APPLICATION_VERSION "v. 0.8.3 (Alpha RC6)" +#define QGC_APPLICATION_VERSION "v. 0.8.3 (Alpha RC7)" namespace QGC diff --git a/src/uas/PxQuadMAV.cc b/src/uas/PxQuadMAV.cc index 841d5655a3f0602ce85bf120e7a2065fdb3c3272..12b6f75826d9d9bb58495f29acd94635285b6c5e 100644 --- a/src/uas/PxQuadMAV.cc +++ b/src/uas/PxQuadMAV.cc @@ -1,5 +1,4 @@ /*===================================================================== - QGroundControl Open Source Ground Control Station (c) 2009, 2010 QGROUNDCONTROL PROJECT @@ -10,15 +9,15 @@ This file is part of the QGROUNDCONTROL project it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. - + QGROUNDCONTROL is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. - + You should have received a copy of the GNU General Public License along with QGROUNDCONTROL. If not, see . - + ======================================================================*/ #include "PxQuadMAV.h" @@ -39,20 +38,12 @@ PxQuadMAV::PxQuadMAV(MAVLinkProtocol* mavlink, int id) : */ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) { - // Let UAS handle the default message set - UAS::receiveMessage(link, message); - - //qDebug() << "PX RECEIVED" << msg->sysid << msg->compid << msg->msgid; - -// Only compile this portion if matching MAVLink packets have been compiled + // Only compile this portion if matching MAVLink packets have been compiled #ifdef MAVLINK_ENABLED_PIXHAWK mavlink_message_t* msg = &message; if (message.sysid == uasId) { - QString uasState; - QString stateDescription; - QString patternPath; switch (message.msgid) { case MAVLINK_MSG_ID_RAW_AUX: @@ -88,7 +79,7 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) emit letterDetected(uasId, name, detected.confidence, detected.detected); } break; - case MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT: + case MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT: { mavlink_watchdog_heartbeat_t payload; mavlink_msg_watchdog_heartbeat_decode(msg, &payload); @@ -97,7 +88,7 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) } break; - case MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO: + case MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO: { mavlink_watchdog_process_info_t payload; mavlink_msg_watchdog_process_info_decode(msg, &payload); @@ -106,14 +97,14 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) } break; - case MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS: + case MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS: { mavlink_watchdog_process_status_t payload; mavlink_msg_watchdog_process_status_decode(msg, &payload); emit processChanged(this->uasId, payload.watchdog_id, payload.process_id, payload.state, (payload.muted == 1) ? true : false, payload.crashes, payload.pid); } break; - case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE: + case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE: { mavlink_vision_position_estimate_t pos; mavlink_msg_vision_position_estimate_decode(&message, &pos); @@ -128,21 +119,21 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) } break; case MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE: - { - mavlink_vicon_position_estimate_t pos; - mavlink_msg_vicon_position_estimate_decode(&message, &pos); - quint64 time = getUnixTime(pos.usec); - //emit valueChanged(uasId, "vis. time", pos.usec, time); - emit valueChanged(uasId, "vicon roll", "rad", pos.roll, time); - emit valueChanged(uasId, "vicon pitch", "rad", pos.pitch, time); - emit valueChanged(uasId, "vicon yaw", "rad", pos.yaw, time); - emit valueChanged(uasId, "vicon x", "m", pos.x, time); - emit valueChanged(uasId, "vicon y", "m", pos.y, time); - emit valueChanged(uasId, "vicon z", "m", pos.z, time); - emit localPositionChanged(this, pos.x, pos.y, pos.z, time); - } - break; - case MAVLINK_MSG_ID_AUX_STATUS: + { + mavlink_vicon_position_estimate_t pos; + mavlink_msg_vicon_position_estimate_decode(&message, &pos); + quint64 time = getUnixTime(pos.usec); + //emit valueChanged(uasId, "vis. time", pos.usec, time); + emit valueChanged(uasId, "vicon roll", "rad", pos.roll, time); + emit valueChanged(uasId, "vicon pitch", "rad", pos.pitch, time); + emit valueChanged(uasId, "vicon yaw", "rad", pos.yaw, time); + emit valueChanged(uasId, "vicon x", "m", pos.x, time); + emit valueChanged(uasId, "vicon y", "m", pos.y, time); + emit valueChanged(uasId, "vicon z", "m", pos.z, time); + emit localPositionChanged(this, pos.x, pos.y, pos.z, time); + } + break; + case MAVLINK_MSG_ID_AUX_STATUS: { mavlink_aux_status_t status; mavlink_msg_aux_status_decode(&message, &status); @@ -152,31 +143,21 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) emit errCountChanged(uasId, "IMU", "SPI0", status.spi0_err_count); emit errCountChanged(uasId, "IMU", "SPI1", status.spi1_err_count); emit errCountChanged(uasId, "IMU", "UART", status.uart_total_err_count); - emit valueChanged(uasId, "Load", "%", ((float)status.load)/1000.0f, MG::TIME::getGroundTimeNow()); - } - break; - case MAVLINK_MSG_ID_CONTROL_STATUS: - { - mavlink_control_status_t status; - mavlink_msg_control_status_decode(&message, &status); - // Emit control status vector - emit attitudeControlEnabled(static_cast(status.control_att)); - emit positionXYControlEnabled(static_cast(status.control_pos_xy)); - emit positionZControlEnabled(static_cast(status.control_pos_z)); - emit positionYawControlEnabled(static_cast(status.control_pos_yaw)); - - // Emit localization status vector - emit localizationChanged(this, status.position_fix); - emit visionLocalizationChanged(this, status.vision_fix); - emit gpsLocalizationChanged(this, status.gps_fix); + emit valueChanged(uasId, "Load", "%", ((float)status.load)/10.0f, getUnixTime()); } break; - default: - // Do nothing + default: + // Let UAS handle the default message set + UAS::receiveMessage(link, message); break; } } +#else + // Let UAS handle the default message set + UAS::receiveMessage(link, message); + Q_UNUSED(link); + Q_UNUSED(message); #endif } diff --git a/src/uas/SlugsMAV.cc b/src/uas/SlugsMAV.cc index 6126ab0a88df410225c934f01e7f1dd8bb7e9d1e..775819d188721ba96ac6855a403692f139cf10c0 100644 --- a/src/uas/SlugsMAV.cc +++ b/src/uas/SlugsMAV.cc @@ -64,11 +64,12 @@ void SlugsMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) if (message.sysid == uasId) { - // Handle your special messages mavlink_message_t* msg = &message; +#ifdef MAVLINK_ENABLED_SLUGS + // Handle your special messages mavlink_message_t* msg = &message; switch (message.msgid) { -#ifdef MAVLINK_ENABLED_SLUGS + case MAVLINK_MSG_ID_RAW_IMU: mavlink_msg_raw_imu_decode(&message, &mlRawImuData); @@ -151,12 +152,11 @@ void SlugsMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) break; -#endif - default: // qDebug() << "\nSLUGS RECEIVED MESSAGE WITH ID" << message.msgid; break; } + #endif } } diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index dde06f5391bd68b447821e1a0971113839e9ff70..be1dba7d2a72845fd633e2e0bd50d904afa9ff6b 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -16,10 +16,10 @@ #include #include #include +#include #include "UAS.h" #include "LinkInterface.h" #include "UASManager.h" -//#include "MG.h" #include "QGC.h" #include "GAudioOutput.h" #include "MAVLinkProtocol.h" @@ -27,20 +27,6 @@ #include "LinkManager.h" #include "SerialLink.h" -#ifndef M_PI -#define M_PI 3.14159265358979323846 /* pi */ -#endif - -#ifndef M_PI_2 -#define M_PI_2 1.57079632679489661923 /* pi/2 */ -#endif - -#ifndef M_PI_4 -#define M_PI_4 0.78539816339744830962 /* pi/4 */ -#endif - - - UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(), uasId(id), startTime(MG::TIME::getGroundTimeNow()), @@ -54,10 +40,14 @@ waypointManager(*this), thrustSum(0), thrustMax(10), startVoltage(0), +warnVoltage(9.5f), +warnLevelPercent(20.0f), currentVoltage(12.0f), lpVoltage(12.0f), -mode(MAV_MODE_UNINIT), -status(MAV_STATE_UNINIT), +batteryRemainingEstimateEnabled(false), +mode(-1), +status(-1), +navMode(-1), onboardTimeOffset(0), controlRollManual(true), controlPitchManual(true), @@ -82,7 +72,8 @@ pitch(0.0), yaw(0.0), statusTimeout(new QTimer(this)), paramsOnceRequested(false), -airframe(0) +airframe(0), +attitudeKnown(false) { color = UASInterface::getNextColor(); setBattery(LIPOLY, 3); @@ -106,6 +97,7 @@ void UAS::writeSettings() settings.setValue("NAME", this->name); settings.setValue("AIRFRAME", this->airframe); settings.setValue("AP_TYPE", this->autopilot); + settings.setValue("BATTERY_SPECS", getBatterySpecs()); settings.endGroup(); settings.sync(); } @@ -117,6 +109,10 @@ void UAS::readSettings() this->name = settings.value("NAME", this->name).toString(); this->airframe = settings.value("AIRFRAME", this->airframe).toInt(); this->autopilot = settings.value("AP_TYPE", this->autopilot).toInt(); + if (settings.contains("BATTERY_SPECS")) + { + setBatterySpecs(settings.value("BATTERY_SPECS").toString()); + } settings.endGroup(); } @@ -170,13 +166,15 @@ void UAS::receiveMessageNamedValue(const mavlink_message_t& message) { mavlink_named_value_float_t val; mavlink_msg_named_value_float_decode(&message, &val); - emit valueChanged(this->getUASID(), QString(val.name), tr("raw"), val.value, getUnixTime(0)); + QByteArray bytes(val.name, MAVLINK_MSG_NAMED_VALUE_FLOAT_FIELD_NAME_LEN); + emit valueChanged(this->getUASID(), QString(bytes), tr("raw"), val.value, getUnixTime()); } else if (message.msgid == MAVLINK_MSG_ID_NAMED_VALUE_INT) { mavlink_named_value_int_t val; mavlink_msg_named_value_int_decode(&message, &val); - emit valueChanged(this->getUASID(), QString(val.name), tr("raw"), (float)val.value, getUnixTime(0)); + QByteArray bytes(val.name, MAVLINK_MSG_NAMED_VALUE_INT_FIELD_NAME_LEN); + emit valueChanged(this->getUASID(), QString(bytes), tr("raw"), val.value, getUnixTime()); } } @@ -199,10 +197,6 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) { QString uasState; QString stateDescription; - QString patternPath; - - // Receive named value message - receiveMessageNamedValue(message); switch (message.msgid) { @@ -232,6 +226,11 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) emit systemTypeSet(this, type); } + break; + case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT: + case MAVLINK_MSG_ID_NAMED_VALUE_INT: + // Receive named value message + receiveMessageNamedValue(message); break; case MAVLINK_MSG_ID_BOOT: getStatusForCode((int)MAV_STATE_BOOT, uasState, stateDescription); @@ -246,7 +245,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) // FIXME //qDebug() << "SYSTEM NAV MODE:" << state.nav_mode; - QString audiostring = "System " + QString::number(this->getUASID()); + QString audiostring = "System " + getUASName(); QString stateAudio = ""; QString modeAudio = ""; bool statechanged = false; @@ -255,19 +254,26 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) if (state.status != this->status) { statechanged = true; - this->status = (int)state.status; + this->status = state.status; getStatusForCode((int)state.status, uasState, stateDescription); emit statusChanged(this, uasState, stateDescription); emit statusChanged(this->status); - emit loadChanged(this,state.load/10.0f); - emit valueChanged(uasId, "Load", "%", ((float)state.load)/1000.0f, MG::TIME::getGroundTimeNow()); stateAudio = " changed status to " + uasState; } - if (this->mode != static_cast(state.mode)) + if (navMode != state.nav_mode) + { + emit navModeChanged(uasId, state.nav_mode, getNavModeText(state.nav_mode)); + navMode = state.nav_mode; + } + + emit loadChanged(this,state.load/10.0f); + emit valueChanged(uasId, "Load", "%", ((float)state.load)/10.0f, getUnixTime()); + + if (this->mode != static_cast(state.mode)) { modechanged = true; - this->mode = static_cast(state.mode); + this->mode = static_cast(state.mode); QString mode; switch (state.mode) @@ -285,7 +291,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) mode = "GUIDED MODE"; break; case (uint8_t)MAV_MODE_READY: - mode = "READY"; + mode = "READY MODE"; break; case (uint8_t)MAV_MODE_TEST1: mode = "TEST1 MODE"; @@ -311,13 +317,16 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) lpVoltage = filterVoltage(currentVoltage); if (startVoltage == 0) startVoltage = currentVoltage; timeRemaining = calculateTimeRemaining(); + if (!batteryRemainingEstimateEnabled) + { + chargeLevel = state.battery_remaining/10.0f; + } //qDebug() << "Voltage: " << currentVoltage << " Chargelevel: " << getChargeLevel() << " Time remaining " << timeRemaining; emit batteryChanged(this, lpVoltage, getChargeLevel(), timeRemaining); emit voltageChanged(message.sysid, state.vbat/1000.0f); // LOW BATTERY ALARM - float chargeLevel = getChargeLevel(); - if (chargeLevel <= 20.0f) + if (lpVoltage < warnVoltage) { startLowBattAlarm(); } @@ -358,6 +367,25 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) } } break; + + #ifdef MAVLINK_ENABLED_PIXHAWK + case MAVLINK_MSG_ID_CONTROL_STATUS: + { + mavlink_control_status_t status; + mavlink_msg_control_status_decode(&message, &status); + // Emit control status vector + emit attitudeControlEnabled(static_cast(status.control_att)); + emit positionXYControlEnabled(static_cast(status.control_pos_xy)); + emit positionZControlEnabled(static_cast(status.control_pos_z)); + emit positionYawControlEnabled(static_cast(status.control_pos_yaw)); + + // Emit localization status vector + emit localizationChanged(this, status.position_fix); + emit visionLocalizationChanged(this, status.vision_fix); + emit gpsLocalizationChanged(this, status.gps_fix); + } + break; +#endif // PIXHAWK case MAVLINK_MSG_ID_RAW_IMU: { mavlink_raw_imu_t raw; @@ -375,6 +403,23 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) emit valueChanged(uasId, "mag z", "raw", static_cast(raw.zmag), time); } break; + case MAVLINK_MSG_ID_SCALED_IMU: + { + mavlink_scaled_imu_t scaled; + mavlink_msg_scaled_imu_decode(&message, &scaled); + quint64 time = getUnixTime(scaled.usec); + + emit valueChanged(uasId, "accel x", "g", scaled.xacc/1000.0f, time); + emit valueChanged(uasId, "accel y", "g", scaled.yacc/1000.0f, time); + emit valueChanged(uasId, "accel z", "g", scaled.zacc/1000.0f, time); + emit valueChanged(uasId, "gyro roll", "rad/s", scaled.xgyro/1000.0f, time); + emit valueChanged(uasId, "gyro pitch", "rad/s", scaled.ygyro/1000.0f, time); + emit valueChanged(uasId, "gyro yaw", "rad/s", scaled.zgyro/1000.0f, time); + emit valueChanged(uasId, "mag x", "tesla", scaled.xmag/1000.0f, time); + emit valueChanged(uasId, "mag y", "tesla", scaled.ymag/1000.0f, time); + emit valueChanged(uasId, "mag z", "tesla", scaled.zmag/1000.0f, time); + } + break; case MAVLINK_MSG_ID_ATTITUDE: //std::cerr << std::endl; //std::cerr << "Decoded attitude message:" << " roll: " << std::dec << mavlink_msg_attitude_get_roll(message.payload) << " pitch: " << mavlink_msg_attitude_get_pitch(message.payload) << " yaw: " << mavlink_msg_attitude_get_yaw(message.payload) << std::endl; @@ -382,45 +427,81 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) mavlink_attitude_t attitude; mavlink_msg_attitude_decode(&message, &attitude); quint64 time = getUnixTime(attitude.usec); - roll = attitude.roll; - pitch = attitude.pitch; - yaw = attitude.yaw; -// emit valueChanged(uasId, "roll IMU", mavlink_msg_attitude_get_roll(&message), time); -// emit valueChanged(uasId, "pitch IMU", mavlink_msg_attitude_get_pitch(&message), time); -// emit valueChanged(uasId, "yaw IMU", mavlink_msg_attitude_get_yaw(&message), time); - emit valueChanged(uasId, "roll", "rad", mavlink_msg_attitude_get_roll(&message), time); - emit valueChanged(uasId, "pitch", "rad", mavlink_msg_attitude_get_pitch(&message), time); - emit valueChanged(uasId, "yaw", "rad", mavlink_msg_attitude_get_yaw(&message), time); + + roll = QGC::limitAngleToPMPIf(attitude.roll); + pitch = QGC::limitAngleToPMPIf(attitude.pitch); + yaw = QGC::limitAngleToPMPIf(attitude.yaw); + + emit valueChanged(uasId, "roll", "rad", roll, time); + emit valueChanged(uasId, "pitch", "rad", pitch, time); + emit valueChanged(uasId, "yaw", "rad", yaw, time); emit valueChanged(uasId, "rollspeed", "rad/s", attitude.rollspeed, time); emit valueChanged(uasId, "pitchspeed", "rad/s", attitude.pitchspeed, time); emit valueChanged(uasId, "yawspeed", "rad/s", attitude.yawspeed, time); // Emit in angles - emit valueChanged(uasId, "roll", "deg", (attitude.roll/M_PI)*180.0, time); - emit valueChanged(uasId, "pitch", "deg", (attitude.pitch/M_PI)*180.0, time); - emit valueChanged(uasId, "rollspeed", "deg/s", (attitude.rollspeed/M_PI)*180.0, time); - emit valueChanged(uasId, "pitchspeed", "deg/s", (attitude.pitchspeed/M_PI)*180.0, time); - - // Force yaw to 180 deg range - double yaw = ((attitude.yaw/M_PI)*180.0); - double sign = 1.0; - if (yaw < 0) - { - sign = -1.0; - yaw = -yaw; - } - while (yaw > 180.0) + // Convert yaw angle to compass value + // in 0 - 360 deg range + float compass = (yaw/M_PI)*180.0+360.0f; + while (compass > 360.0f) { - yaw -= 180.0; + compass -= 360.0f; } - yaw *= sign; + attitudeKnown = true; - emit valueChanged(uasId, "yaw", "deg", yaw, time); - emit valueChanged(uasId, "yawspeed", "deg/s", (attitude.yawspeed/M_PI)*180.0, time); + emit valueChanged(uasId, "roll deg", "deg", (roll/M_PI)*180.0, time); + emit valueChanged(uasId, "pitch deg", "deg", (pitch/M_PI)*180.0, time); + emit valueChanged(uasId, "heading deg", "deg", compass, time); + emit valueChanged(uasId, "rollspeed d/s", "deg/s", (attitude.rollspeed/M_PI)*180.0, time); + emit valueChanged(uasId, "pitchspeed d/s", "deg/s", (attitude.pitchspeed/M_PI)*180.0, time); + emit valueChanged(uasId, "yawspeed d/s", "deg/s", (attitude.yawspeed/M_PI)*180.0, time); - emit attitudeChanged(this, attitude.roll, attitude.pitch, attitude.yaw, time); + emit attitudeChanged(this, roll, pitch, yaw, time); + emit attitudeSpeedChanged(uasId, attitude.rollspeed, attitude.pitchspeed, attitude.yawspeed, time); + } + break; + case MAVLINK_MSG_ID_VFR_HUD: + { + mavlink_vfr_hud_t hud; + mavlink_msg_vfr_hud_decode(&message, &hud); + quint64 time = getUnixTime(); + // Display updated values + emit valueChanged(uasId, "airspeed", "m/s", hud.airspeed, time); + emit valueChanged(uasId, "groundspeed", "m/s", hud.groundspeed, time); + emit valueChanged(uasId, "altitude", "m", hud.alt, time); + emit valueChanged(uasId, "heading", "deg", hud.heading, time); + emit valueChanged(uasId, "climbrate", "m/s", hud.climb, time); + emit valueChanged(uasId, "throttle", "%", hud.throttle, time); + emit thrustChanged(this, hud.throttle/100.0); + + if (!attitudeKnown) + { + yaw = QGC::limitAngleToPMPId((((double)hud.heading-180.0)/360.0)*M_PI); + emit attitudeChanged(this, roll, pitch, yaw, time); + } + + emit altitudeChanged(uasId, hud.alt); + //yaw = (hud.heading-180.0f/360.0f)*M_PI; + //emit attitudeChanged(this, roll, pitch, yaw, getUnixTime()); + emit speedChanged(this, hud.airspeed, 0.0f, hud.climb, getUnixTime()); + } + break; + case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT: + { + mavlink_nav_controller_output_t nav; + mavlink_msg_nav_controller_output_decode(&message, &nav); + quint64 time = getUnixTime(); + // Update UI + emit valueChanged(uasId, "nav roll", "deg", nav.nav_roll, time); + emit valueChanged(uasId, "nav pitch", "deg", nav.nav_pitch, time); + emit valueChanged(uasId, "nav bearing", "deg", nav.nav_bearing, time); + emit valueChanged(uasId, "target bearing", "deg", nav.target_bearing, time); + emit valueChanged(uasId, "wp dist", "m", nav.wp_dist, time); + emit valueChanged(uasId, "alt err", "m", nav.alt_error, time); + emit valueChanged(uasId, "airspeed err", "m/s", nav.alt_error, time); + emit valueChanged(uasId, "xtrack err", "m", nav.xtrack_error, time); } break; case MAVLINK_MSG_ID_LOCAL_POSITION: @@ -471,10 +552,38 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) emit valueChanged(uasId, "latitude", "deg", latitude, time); emit valueChanged(uasId, "longitude", "deg", longitude, time); emit valueChanged(uasId, "altitude", "m", altitude, time); - emit valueChanged(uasId, "gps x speed", "m/s", speedX, time); - emit valueChanged(uasId, "gps y speed", "m/s", speedY, time); - emit valueChanged(uasId, "gps z speed", "m/s", speedZ, time); - emit globalPositionChanged(this, longitude, latitude, altitude, time); + double totalSpeed = sqrt(speedX*speedX + speedY*speedY + speedZ*speedZ); + emit valueChanged(uasId, "gps speed", "m/s", totalSpeed, time); + emit globalPositionChanged(this, latitude, longitude, altitude, time); + emit speedChanged(this, speedX, speedY, speedZ, time); + // Set internal state + if (!positionLock) + { + // If position was not locked before, notify positive + GAudioOutput::instance()->notifyPositive(); + } + positionLock = true; + //TODO fix this hack for forwarding of global position for patch antenna tracking + forwardMessage(message); + } + break; + case MAVLINK_MSG_ID_GLOBAL_POSITION: + { + mavlink_global_position_t pos; + mavlink_msg_global_position_decode(&message, &pos); + quint64 time = QGC::groundTimeUsecs()/1000; + latitude = pos.lat; + longitude = pos.lon; + altitude = pos.alt; + speedX = pos.vx; + speedY = pos.vy; + speedZ = pos.vz; + emit valueChanged(uasId, "latitude", "deg", latitude, time); + emit valueChanged(uasId, "longitude", "deg", longitude, time); + emit valueChanged(uasId, "altitude", "m", altitude, time); + double totalSpeed = sqrt(speedX*speedX + speedY*speedY + speedZ*speedZ); + emit valueChanged(uasId, "gps speed", "m/s", totalSpeed, time); + emit globalPositionChanged(this, latitude, longitude, altitude, time); emit speedChanged(this, speedX, speedY, speedZ, time); // Set internal state if (!positionLock) @@ -497,20 +606,19 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) // SANITY CHECK // only accept values in a realistic range // quint64 time = getUnixTime(pos.usec); - quint64 time = MG::TIME::getGroundTimeNow(); + quint64 time = getUnixTime(); emit valueChanged(uasId, "latitude", "deg", pos.lat, time); emit valueChanged(uasId, "longitude", "deg", pos.lon, time); - // FIXME REMOVE - longitude = pos.lon; - latitude = pos.lat; - altitude = pos.alt; - emit globalPositionChanged(this, longitude, latitude, altitude, time); - if (pos.fix_type > 0) { - emit globalPositionChanged(this, pos.lon, pos.lat, pos.alt, time); + emit globalPositionChanged(this, pos.lat, pos.lon, pos.alt, time); + emit valueChanged(uasId, "gps speed", "m/s", pos.v, time); + latitude = pos.lat; + longitude = pos.lon; + altitude = pos.alt; + positionLock = true; // Check for NaN int alt = pos.alt; @@ -534,6 +642,50 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) } } break; + case MAVLINK_MSG_ID_GPS_RAW_INT: + { + mavlink_gps_raw_int_t pos; + mavlink_msg_gps_raw_int_decode(&message, &pos); + + // SANITY CHECK + // only accept values in a realistic range + // quint64 time = getUnixTime(pos.usec); + quint64 time = getUnixTime(); + + emit valueChanged(uasId, "latitude", "deg", pos.lat/(double)1E7, time); + emit valueChanged(uasId, "longitude", "deg", pos.lon/(double)1E7, time); + + if (pos.fix_type > 0) + { + emit globalPositionChanged(this, pos.lat/(double)1E7, pos.lon/(double)1E7, pos.alt/1000.0, time); + emit valueChanged(uasId, "gps speed", "m/s", pos.v, time); + latitude = pos.lat/(double)1E7; + longitude = pos.lon/(double)1E7; + altitude = pos.alt/1000.0; + positionLock = true; + + // Check for NaN + int alt = pos.alt; + if (alt != alt) + { + alt = 0; + emit textMessageReceived(uasId, message.compid, 255, "GCS ERROR: RECEIVED NaN FOR ALTITUDE"); + } + emit valueChanged(uasId, "altitude", "m", pos.alt/(double)1E3, time); + // Smaller than threshold and not NaN + if (pos.v < 1000000 && pos.v == pos.v) + { + emit valueChanged(uasId, "speed", "m/s", pos.v, time); + //qDebug() << "GOT GPS RAW"; + // emit speedChanged(this, (double)pos.v, 0.0, 0.0, time); + } + else + { + emit textMessageReceived(uasId, message.compid, 255, QString("GCS ERROR: RECEIVED INVALID SPEED OF %1 m/s").arg(pos.v)); + } + } + } + break; case MAVLINK_MSG_ID_GPS_STATUS: { mavlink_gps_status_t pos; @@ -555,10 +707,11 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) { mavlink_raw_pressure_t pressure; mavlink_msg_raw_pressure_decode(&message, &pressure); - quint64 time = this->getUnixTime(0); - emit valueChanged(uasId, "abs pressure", "hP", pressure.press_abs, time); - emit valueChanged(uasId, "diff pressure 1", "hP", pressure.press_diff1, time); - emit valueChanged(uasId, "diff pressure 2", "hP", pressure.press_diff2, time); + quint64 time = this->getUnixTime(pressure.usec); + emit valueChanged(uasId, "abs pressure", "hPa", pressure.press_abs, time); + emit valueChanged(uasId, "diff pressure 1", "hPa", pressure.press_diff1, time); + emit valueChanged(uasId, "diff pressure 2", "hPa", pressure.press_diff2, time); + emit valueChanged(uasId, "temperature", "deg C", pressure.temperature/100.0f, time); } break; case MAVLINK_MSG_ID_RC_CHANNELS_RAW: @@ -595,8 +748,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) { mavlink_param_value_t value; mavlink_msg_param_value_decode(&message, &value); - - QString parameterName = QString((char*)value.param_id); + QByteArray bytes((char*)value.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + QString parameterName = QString(bytes); int component = message.compid; float val = value.param_value; @@ -612,6 +765,19 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) // Emit change emit parameterChanged(uasId, message.compid, parameterName, val); + emit parameterChanged(uasId, message.compid, value.param_count, value.param_index, parameterName, val); + } + break; + case MAVLINK_MSG_ID_ACTION_ACK: + mavlink_action_ack_t ack; + mavlink_msg_action_ack_decode(&message, &ack); + if (ack.result == 1) + { + emit textMessageReceived(uasId, message.compid, 0, tr("SUCCESS: Executed action: %1").arg(ack.action)); + } + else + { + emit textMessageReceived(uasId, message.compid, 0, tr("FAILURE: Rejected action: %1").arg(ack.action)); } break; case MAVLINK_MSG_ID_DEBUG: @@ -689,6 +855,9 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) mavlink_waypoint_reached_t wpr; mavlink_msg_waypoint_reached_decode(&message, &wpr); waypointManager.handleWaypointReached(message.sysid, message.compid, &wpr); + QString text = QString("System %1 reached waypoint %2").arg(getUASName()).arg(wpr.seq); + GAudioOutput::instance()->say(text); + emit textMessageReceived(message.sysid, message.compid, 0, text); } break; @@ -707,11 +876,25 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) emit positionSetPointsChanged(uasId, p.x, p.y, p.z, p.yaw, QGC::groundTimeUsecs()); } break; - + case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW: + { + mavlink_servo_output_raw_t servos; + mavlink_msg_servo_output_raw_decode(&message, &servos); + quint64 time = getUnixTime(0); + emit valueChanged(uasId, "servo #1", "us", servos.servo1_raw, time); + emit valueChanged(uasId, "servo #2", "us", servos.servo2_raw, time); + emit valueChanged(uasId, "servo #3", "us", servos.servo3_raw, time); + emit valueChanged(uasId, "servo #4", "us", servos.servo4_raw, time); + emit valueChanged(uasId, "servo #5", "us", servos.servo5_raw, time); + emit valueChanged(uasId, "servo #6", "us", servos.servo6_raw, time); + emit valueChanged(uasId, "servo #7", "us", servos.servo7_raw, time); + emit valueChanged(uasId, "servo #8", "us", servos.servo8_raw, time); + } + break; case MAVLINK_MSG_ID_STATUSTEXT: { QByteArray b; - b.resize(256); + b.resize(MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN); mavlink_msg_statustext_get_text(&message, (int8_t*)b.data()); //b.append('\0'); QString text = QString(b); @@ -721,7 +904,53 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) emit textMessageReceived(uasId, message.compid, severity, text); } break; - case MAVLINK_MSG_ID_DEBUG_VECT: +#ifdef MAVLINK_ENABLED_PIXHAWK + case MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE: + { + qDebug() << "RECIEVED ACK TO GET IMAGE"; + mavlink_data_transmission_handshake_t p; + mavlink_msg_data_transmission_handshake_decode(&message, &p); + imageSize = p.size; + imagePackets = p.packets; + imagePayload = p.payload; + imageQuality = p.jpg_quality; + imageStart = QGC::groundTimeMilliseconds(); + } + break; + + case MAVLINK_MSG_ID_ENCAPSULATED_DATA: + { + mavlink_encapsulated_data_t img; + mavlink_msg_encapsulated_data_decode(&message, &img); + int seq = img.seqnr; + int pos = seq * imagePayload; + + for (int i = 0; i < imagePayload; ++i) + { + if (pos <= imageSize) + { + imageRecBuffer[pos] = img.data[i]; + } + ++pos; + } + + ++imagePacketsArrived; + + // emit signal if all packets arrived + if ((imagePacketsArrived == imagePackets)) + { + image.loadFromData(imageRecBuffer); + emit imageReady(this); + // Restart statemachine + imagePacketsArrived = 0; + + //this->requestImage(); + //qDebug() << "SENDING REQUEST TO GET NEW IMAGE FROM SYSTEM" << uasId; + } + } + break; +#endif + case MAVLINK_MSG_ID_DEBUG_VECT: { mavlink_debug_vect_t vect; mavlink_msg_debug_vect_decode(&message, &vect); @@ -793,12 +1022,17 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) break; #endif + // Messages to ignore + case MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET: + break; default: { if (!unknownPackets.contains(message.msgid)) { unknownPackets.append(message.msgid); - //GAudioOutput::instance()->say("UNABLE TO DECODE MESSAGE WITH ID " + QString::number(message.msgid) + " FROM SYSTEM " + QString::number(message.sysid)); + QString errString = tr("UNABLE TO DECODE MESSAGE NUMBER %1").arg(message.msgid); + GAudioOutput::instance()->say(errString+tr(", please check the communication console for details.")); + emit textMessageReceived(uasId, message.compid, 255, errString); std::cout << "Unable to decode message from system " << std::dec << static_cast(message.sysid) << " with message id:" << static_cast(message.msgid) << std::endl; //qDebug() << std::cerr << "Unable to decode message from system " << std::dec << static_cast(message.acid) << " with message id:" << static_cast(message.msgid) << std::endl; } @@ -989,10 +1223,17 @@ void UAS::setMode(int mode) void UAS::sendMessage(mavlink_message_t message) { // Emit message on all links that are currently connected - QList::iterator i; - for (i = links->begin(); i != links->end(); ++i) + foreach (LinkInterface* link, *links) { - sendMessage(*i, message); + if (link) + { + sendMessage(link, message); + } + else + { + // Remove from list + links->removeAt(links->indexOf(link)); + } } } @@ -1013,7 +1254,7 @@ void UAS::forwardMessage(mavlink_message_t message) { if(serial != links->at(i)) { - qDebug()<<"Forwarding Over link: "<getName()<<" "<getName()<<" "<getSystemId(), mavlink->getComponentId(), &msg, DATA_TYPE_JPEG_IMAGE, 0, 0, 0, 50); + sendMessage(msg); + } + else if (QGC::groundTimeMilliseconds() - imageStart >= 1000) + { + // handshake happened more than 1 second ago, packets should have arrived by now + // maybe we missed some packets (dropped along the way) + image.loadFromData(imageRecBuffer); + emit imageReady(this); + // Restart statemachine + imagePacketsArrived = 0; + } +#endif + // default else, wait? +} /* MANAGEMENT */ @@ -1256,27 +1561,27 @@ void UAS::enableRawControllerDataTransmission(int rate) sendMessage(msg); } -void UAS::enableRawSensorFusionTransmission(int rate) -{ - // Buffers to write data to - mavlink_message_t msg; - mavlink_request_data_stream_t stream; - // Select the message to request from now on - stream.req_stream_id = MAV_DATA_STREAM_RAW_SENSOR_FUSION; - // Select the update rate in Hz the message should be send - stream.req_message_rate = rate; - // Start / stop the message - stream.start_stop = (rate) ? 1 : 0; - // The system which should take this command - stream.target_system = uasId; - // The component / subsystem which should take this command - stream.target_component = 0; - // Encode and send the message - mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream); - // Send message twice to increase chance of reception - sendMessage(msg); - sendMessage(msg); -} +//void UAS::enableRawSensorFusionTransmission(int rate) +//{ +// // Buffers to write data to +// mavlink_message_t msg; +// mavlink_request_data_stream_t stream; +// // Select the message to request from now on +// stream.req_stream_id = MAV_DATA_STREAM_RAW_SENSOR_FUSION; +// // Select the update rate in Hz the message should be send +// stream.req_message_rate = rate; +// // Start / stop the message +// stream.start_stop = (rate) ? 1 : 0; +// // The system which should take this command +// stream.target_system = uasId; +// // The component / subsystem which should take this command +// stream.target_component = 0; +// // Encode and send the message +// mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream); +// // Send message twice to increase chance of reception +// sendMessage(msg); +// sendMessage(msg); +//} void UAS::enablePositionTransmission(int rate) { @@ -1391,11 +1696,11 @@ void UAS::setParameter(const int component, const QString& id, const float value { p.param_id[i] = id.toAscii()[i]; } - // Null termination at end of string or end of buffer - else if ((int)i == id.length() || i == (sizeof(p.param_id) - 1)) - { - p.param_id[i] = '\0'; - } +// // Null termination at end of string or end of buffer +// else if ((int)i == id.length() || i == (sizeof(p.param_id) - 1)) +// { +// p.param_id[i] = '\0'; +// } // Zero fill else { @@ -1407,6 +1712,37 @@ void UAS::setParameter(const int component, const QString& id, const float value } } +void UAS::requestParameter(int component, int parameter) +{ + mavlink_message_t msg; + mavlink_param_request_read_t read; + read.param_index = parameter; + read.target_system = uasId; + read.target_component = component; + mavlink_msg_param_request_read_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &read); + sendMessage(msg); + qDebug() << __FILE__ << __LINE__ << "REQUESTING PARAM RETRANSMISSION FROM COMPONENT" << component << "FOR PARAM ID" << parameter; +} + +void UAS::setSystemType(int systemType) +{ + type = systemType; + // If the airframe is still generic, change it to a close default type + if (airframe == 0) + { + switch (systemType) + { + case MAV_FIXED_WING: + airframe = QGC_AIRFRAME_EASYSTAR; + break; + case MAV_QUADROTOR: + airframe = QGC_AIRFRAME_MIKROKOPTER; + break; + } + } + emit systemSpecsChanged(uasId); +} + void UAS::setUASName(const QString& name) { this->name = name; @@ -1436,7 +1772,7 @@ void UAS::launch() { mavlink_message_t msg; // TODO Replace MG System ID with static function call and allow to change ID in GUI - mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_LAUNCH); + mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_TAKEOFF); // Send message twice to increase chance of reception sendMessage(msg); sendMessage(msg); @@ -1450,7 +1786,7 @@ void UAS::enable_motors() { mavlink_message_t msg; // TODO Replace MG System ID with static function call and allow to change ID in GUI - mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_MOTORS_START); + mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_MOTORS_START); // Send message twice to increase chance of reception sendMessage(msg); sendMessage(msg); @@ -1464,7 +1800,7 @@ void UAS::disable_motors() { mavlink_message_t msg; // TODO Replace MG System ID with static function call and allow to change ID in GUI - mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_MOTORS_STOP); + mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_MOTORS_STOP); // Send message twice to increase chance of reception sendMessage(msg); sendMessage(msg); @@ -1722,8 +2058,6 @@ void UAS::addLink(LinkInterface* link) links->append(link); connect(link, SIGNAL(destroyed(QObject*)), this, SLOT(removeLink(QObject*))); } - //links->append(link); - //qDebug() << link<<" ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK"; } void UAS::removeLink(QObject* object) @@ -1769,6 +2103,64 @@ void UAS::setBattery(BatteryType type, int cells) } } +void UAS::setBatterySpecs(const QString& specs) +{ + if (specs.length() == 0 || specs.contains("%")) + { + batteryRemainingEstimateEnabled = false; + bool ok; + QString percent = specs; + percent = percent.remove("%"); + float temp = percent.toFloat(&ok); + if (ok) + { + warnLevelPercent = temp; + } + else + { + emit textMessageReceived(0, 0, 0, "Could not set battery options, format is wrong"); + } + } + else + { + batteryRemainingEstimateEnabled = true; + QString stringList = specs; + stringList = stringList.remove("V"); + stringList = stringList.remove("v"); + QStringList parts = stringList.split(","); + if (parts.length() == 3) + { + float temp; + bool ok; + // Get the empty voltage + temp = parts.at(0).toFloat(&ok); + if (ok) emptyVoltage = temp; + // Get the warning voltage + temp = parts.at(1).toFloat(&ok); + if (ok) warnVoltage = temp; + // Get the full voltage + temp = parts.at(2).toFloat(&ok); + if (ok) fullVoltage = temp; + } + else + { + emit textMessageReceived(0, 0, 0, "Could not set battery options, format is wrong"); + } + } +} + +QString UAS::getBatterySpecs() +{ + if (batteryRemainingEstimateEnabled) + { + return QString("%1V,%2V,%3V").arg(emptyVoltage).arg(warnVoltage).arg(fullVoltage); + } + else + { + return QString("%1%").arg(warnLevelPercent); + } +} + int UAS::calculateTimeRemaining() { quint64 dt = MG::TIME::getGroundTimeNow() - startTime; @@ -1785,20 +2177,22 @@ int UAS::calculateTimeRemaining() /** * @return charge level in percent - 0 - 100 */ -double UAS::getChargeLevel() +float UAS::getChargeLevel() { - float chargeLevel; - if (lpVoltage < emptyVoltage) - { - chargeLevel = 0.0f; - } - else if (lpVoltage > fullVoltage) - { - chargeLevel = 100.0f; - } - else + if (batteryRemainingEstimateEnabled) { - chargeLevel = 100.0f * ((lpVoltage - emptyVoltage)/(fullVoltage - emptyVoltage)); + if (lpVoltage < emptyVoltage) + { + chargeLevel = 0.0f; + } + else if (lpVoltage > fullVoltage) + { + chargeLevel = 100.0f; + } + else + { + chargeLevel = 100.0f * ((lpVoltage - emptyVoltage)/(fullVoltage - emptyVoltage)); + } } return chargeLevel; } @@ -1807,7 +2201,7 @@ void UAS::startLowBattAlarm() { if (!lowBattAlarm) { - GAudioOutput::instance()->alert("LOW BATTERY"); + GAudioOutput::instance()->alert(tr("SYSTEM %1 HAS LOW BATTERY").arg(getUASName())); QTimer::singleShot(2000, GAudioOutput::instance(), SLOT(startEmergency())); lowBattAlarm = true; } diff --git a/src/uas/UAS.h b/src/uas/UAS.h index d65d319f00aa942e83f870e9d064983c22fab823..008cbd6c5b97e32aec7f14dafc9b01bf0d87b9fd 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -120,14 +120,19 @@ protected: //COMMENTS FOR TEST UNIT double thrustMax; ///< Maximum forward/up thrust of this vehicle, in Newtons // Battery stats - double fullVoltage; ///< Voltage of the fully charged battery (100%) - double emptyVoltage; ///< Voltage of the empty battery (0%) - double startVoltage; ///< Voltage at system start + float fullVoltage; ///< Voltage of the fully charged battery (100%) + float emptyVoltage; ///< Voltage of the empty battery (0%) + float startVoltage; ///< Voltage at system start + float warnVoltage; ///< Voltage where QGC will start to warn about low battery + float warnLevelPercent; ///< Warning level, in percent double currentVoltage; ///< Voltage currently measured float lpVoltage; ///< Low-pass filtered voltage + bool batteryRemainingEstimateEnabled; ///< If the estimate is enabled, QGC will try to estimate the remaining battery life + float chargeLevel; ///< Charge level of battery, in percent int timeRemaining; ///< Remaining time calculated based on previous and current - unsigned int mode; ///< The current mode of the MAV + int mode; ///< The current mode of the MAV int status; ///< The current status of the MAV + int navMode; ///< The current navigation mode of the MAV quint64 onboardTimeOffset; bool controlRollManual; ///< status flag, true if roll is controlled manually @@ -157,37 +162,56 @@ protected: //COMMENTS FOR TEST UNIT double yaw; quint64 lastHeartbeat; ///< Time of the last heartbeat message QTimer* statusTimeout; ///< Timer for various status timeouts + + int imageSize; ///< Image size being transmitted (bytes) + int imagePackets; ///< Number of data packets being sent for this image + int imagePacketsArrived; ///< Number of data packets recieved + int imagePayload; ///< Payload size per transmitted packet (bytes). Standard is 254, and decreases when image resolution increases. + int imageQuality; ///< JPEG-Quality of the transmitted image (percentage) + QByteArray imageRecBuffer; ///< Buffer for the incoming bytestream + QImage image; ///< Image data of last completely transmitted image + quint64 imageStart; + QMap* > parameters; ///< All parameters bool paramsOnceRequested; ///< If the parameter list has been read at least once int airframe; ///< The airframe type + bool attitudeKnown; ///< True if attitude was received, false else + public: /** @brief Set the current battery type */ void setBattery(BatteryType type, int cells); /** @brief Estimate how much flight time is remaining */ int calculateTimeRemaining(); /** @brief Get the current charge level */ - double getChargeLevel(); + float getChargeLevel(); /** @brief Get the human-readable status message for this code */ void getStatusForCode(int statusCode, QString& uasState, QString& stateDescription); + /** @brief Get the human-readable navigation mode translation for this mode */ + QString getNavModeText(int mode); /** @brief Check if vehicle is in autonomous mode */ bool isAuto(); -public: UASWaypointManager* getWaypointManager() { return &waypointManager; } int getSystemType(); + QImage getImage(); + void requestImage(); // ? int getAutopilotType() {return autopilot;} public slots: /** @brief Set the autopilot type */ void setAutopilotType(int apType) { autopilot = apType; emit systemSpecsChanged(uasId); } /** @brief Set the type of airframe */ - void setSystemType(int systemType) { type = systemType; emit systemSpecsChanged(uasId); } + void setSystemType(int systemType); /** @brief Set the specific airframe type */ void setAirframe(int airframe) { this->airframe = airframe; emit systemSpecsChanged(uasId); } /** @brief Set a new name **/ void setUASName(const QString& name); /** @brief Executes an action **/ void setAction(MAV_ACTION action); + /** @brief Set the current battery type and voltages */ + void setBatterySpecs(const QString& specs); + /** @brief Get the current battery type and specs */ + QString getBatterySpecs(); /** @brief Launches the system **/ void launch(); @@ -252,6 +276,9 @@ public slots: /** @brief Request all parameters */ void requestParameters(); + /** @brief Request a single parameter by index */ + void requestParameter(int component, int parameter); + /** @brief Set a system parameter */ void setParameter(const int component, const QString& id, const float value); @@ -271,7 +298,7 @@ public slots: void enableExtendedSystemStatusTransmission(int rate); void enableRCChannelDataTransmission(int rate); void enableRawControllerDataTransmission(int rate); - void enableRawSensorFusionTransmission(int rate); + //void enableRawSensorFusionTransmission(int rate); void enablePositionTransmission(int rate); void enableExtra1Transmission(int rate); void enableExtra2Transmission(int rate); @@ -310,10 +337,12 @@ signals: /** @brief Propagate a heartbeat received from the system */ void heartbeat(UASInterface* uas); void imageStarted(quint64 timestamp); + /** @brief A new camera image has arrived */ + void imageReady(UASInterface* uas); protected: - /** @brief Get the UNIX timestamp in microseconds */ - quint64 getUnixTime(quint64 time); + /** @brief Get the UNIX timestamp in milliseconds */ + quint64 getUnixTime(quint64 time=0); protected slots: /** @brief Write settings to disk */ diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index cf7f65b4891e69ebf0cedb5a359510bdd9129126..643a715a5fe0d7966161a825a0cd6a34c1a92b01 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -226,6 +226,8 @@ public slots: virtual void setLocalOriginAtCurrentGPSPosition() = 0; /** @brief Request all onboard parameters of all components */ virtual void requestParameters() = 0; + /** @brief Request one specific onboard parameter */ + virtual void requestParameter(int component, int parameter) = 0; /** @brief Write parameter to permanent storage */ virtual void writeParametersToStorage() = 0; /** @brief Read parameter from permanent storage */ @@ -259,7 +261,7 @@ public slots: virtual void enableExtendedSystemStatusTransmission(int rate) = 0; virtual void enableRCChannelDataTransmission(int rate) = 0; virtual void enableRawControllerDataTransmission(int rate) = 0; - virtual void enableRawSensorFusionTransmission(int rate) = 0; + //virtual void enableRawSensorFusionTransmission(int rate) = 0; virtual void setLocalPositionSetpoint(float x, float y, float z, float yaw) = 0; virtual void setLocalPositionOffset(float x, float y, float z, float yaw) = 0; @@ -269,7 +271,10 @@ public slots: virtual void startGyroscopeCalibration() = 0; virtual void startPressureCalibration() = 0; - + /** @brief Set the current battery type and voltages */ + virtual void setBatterySpecs(const QString& specs) = 0; + /** @brief Get the current battery type and specs */ + virtual QString getBatterySpecs() = 0; protected: QColor color; @@ -301,7 +306,11 @@ signals: void poiFound(UASInterface* uas, int type, int colorIndex, QString message, float x, float y, float z); void poiConnectionFound(UASInterface* uas, int type, int colorIndex, QString message, float x1, float y1, float z1, float x2, float y2, float z2); + /** @brief A text message from the system has been received */ void textMessageReceived(int uasid, int componentid, int severity, QString text); + + void navModeChanged(int uasid, int mode, const QString& text); + /** * @brief Update the error count of a device * @@ -366,6 +375,7 @@ signals: void waypointReached(UASInterface* uas, int id); void autoModeChanged(bool autoMode); void parameterChanged(int uas, int component, QString parameterName, float value); + void parameterChanged(int uas, int component, int parameterCount, int parameterId, QString parameterName, float value); void patternDetected(int uasId, QString patternPath, float confidence, bool detected); void letterDetected(int uasId, QString letter, float confidence, bool detected); /** @@ -382,10 +392,12 @@ signals: void thrustChanged(UASInterface*, double thrust); void heartbeat(UASInterface* uas); void attitudeChanged(UASInterface*, double roll, double pitch, double yaw, quint64 usec); + void attitudeSpeedChanged(int uas, double rollspeed, double pitchspeed, double yawspeed, quint64 usec); void attitudeThrustSetPointChanged(UASInterface*, double rollDesired, double pitchDesired, double yawDesired, double thrustDesired, quint64 usec); void positionSetPointsChanged(int uasid, float xDesired, float yDesired, float zDesired, float yawDesired, quint64 usec); void localPositionChanged(UASInterface*, double x, double y, double z, quint64 usec); void globalPositionChanged(UASInterface*, double lat, double lon, double alt, quint64 usec); + void altitudeChanged(int uasid, double altitude); /** @brief Update the status of one satellite used for localization */ void gpsSatelliteStatusChanged(int uasid, int satid, float azimuth, float direction, float snr, bool used); void speedChanged(UASInterface*, double x, double y, double z, quint64 usec); diff --git a/src/uas/UASManager.cc b/src/uas/UASManager.cc index c59f50ada8e203b390bccbb0b597efca4d2f726b..7ce41e847127c4aaf5530a78acbef897c0ad63bf 100644 --- a/src/uas/UASManager.cc +++ b/src/uas/UASManager.cc @@ -58,21 +58,26 @@ UASManager* UASManager::instance() UASManager::UASManager() : activeUAS(NULL) { - systems = QList(); start(QThread::LowPriority); } UASManager::~UASManager() { + // Delete all systems + foreach (UASInterface* mav, systems) + { + delete mav; + } } void UASManager::run() { - forever - { - QGC::SLEEP::msleep(5000); - } +// forever +// { +// QGC::SLEEP::msleep(5000); +// } + exec(); } void UASManager::addUAS(UASInterface* uas) diff --git a/src/uas/UASWaypointManager.cc b/src/uas/UASWaypointManager.cc index e339a2e5d4a5428ae72b8f2822fd93c99b9aa8c5..17680bde41269c2c914b790e4811e0c88a01577d 100644 --- a/src/uas/UASWaypointManager.cc +++ b/src/uas/UASWaypointManager.cc @@ -138,8 +138,8 @@ void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_ if(wp->seq == current_wp_id) { - qDebug() << "Got WP: " << wp->seq << wp->x << wp->y << wp->z << wp->yaw << "auto:" << wp->autocontinue << "curr:" << wp->current << wp->param1 << wp->param2 << (MAV_FRAME) wp->frame << (MAV_ACTION) wp->action; - Waypoint *lwp = new Waypoint(wp->seq, wp->x, wp->y, wp->z, wp->yaw, wp->autocontinue, wp->current, wp->param1, wp->param2, (MAV_FRAME) wp->frame, (MAV_ACTION) wp->action); + //qDebug() << "Got WP: " << wp->seq << wp->x << wp->y << wp->z << wp->param4 << "auto:" << wp->autocontinue << "curr:" << wp->current << wp->param1 << wp->param2 << (MAV_FRAME) wp->frame << (MAV_CMD) wp->command; + Waypoint *lwp = new Waypoint(wp->seq, wp->x, wp->y, wp->z, wp->param1, wp->param2, wp->param3, wp->param4, wp->autocontinue, wp->current, (MAV_FRAME) wp->frame, (MAV_CMD) wp->command); addWaypoint(lwp, false); //get next waypoint @@ -236,6 +236,7 @@ void UASWaypointManager::handleWaypointCurrent(quint8 systemId, quint8 compId, m { if (systemId == uas.getUASID() && compId == MAV_COMP_ID_WAYPOINTPLANNER) { + // FIXME Petri if (current_state == WP_SETCURRENT) { protocol_timer.stop(); @@ -257,11 +258,12 @@ void UASWaypointManager::handleWaypointCurrent(quint8 systemId, quint8 compId, m } } - emit updateStatusString(QString("New current waypoint %1").arg(wpc->seq)); - //emit update to UI widgets - emit currentWaypointChanged(wpc->seq); + //qDebug() << "Updated waypoints list"; } - qDebug() << "new current waypoint" << wpc->seq; + emit updateStatusString(QString("New current waypoint %1").arg(wpc->seq)); + //emit update to UI widgets + emit currentWaypointChanged(wpc->seq); + //qDebug() << "new current waypoint" << wpc->seq; } } @@ -394,10 +396,11 @@ void UASWaypointManager::saveWaypoints(const QString &saveFile) QTextStream out(&file); //write the waypoint list version to the first line for compatibility check - out << "QGC WPL 100\r\n"; + out << "QGC WPL 110\r\n"; for (int i = 0; i < waypoints.size(); i++) { + waypoints[i]->setId(i); waypoints[i]->save(out); } file.close(); @@ -420,7 +423,7 @@ void UASWaypointManager::loadWaypoints(const QString &loadFile) const QStringList &version = in.readLine().split(" "); - if (!(version.size() == 3 && version[0] == "QGC" && version[1] == "WPL" && version[2] == "100")) + if (!(version.size() == 3 && version[0] == "QGC" && version[1] == "WPL" && version[2] == "110")) { emit updateStatusString(tr("The waypoint file is not compatible with the current version of QGroundControl.")); //MainWindow::instance()->showCriticalMessage(tr("Error loading waypoint file"),tr("The waypoint file is not compatible with the current version of QGroundControl.")); @@ -487,11 +490,119 @@ void UASWaypointManager::clearWaypointList() } } +const QVector UASWaypointManager::getGlobalFrameWaypointList() +{ + // TODO Keep this global frame list up to date + // with complete waypoint list + // instead of filtering on each request + QVector wps; + foreach (Waypoint* wp, waypoints) + { + if (wp->getFrame() == MAV_FRAME_GLOBAL) + { + wps.append(wp); + } + } + return wps; +} + int UASWaypointManager::getIndexOf(Waypoint* wp) { return waypoints.indexOf(wp); } +int UASWaypointManager::getGlobalFrameIndexOf(Waypoint* wp) +{ + // Search through all waypoints, + // counting only those in global frame + int i = 0; + foreach (Waypoint* p, waypoints) + { + if (p->getFrame() == MAV_FRAME_GLOBAL) + { + if (p == wp) + { + return i; + } + i++; + } + } + + return -1; +} + +int UASWaypointManager::getGlobalFrameCount() +{ + // Search through all waypoints, + // counting only those in global frame + int i = 0; + foreach (Waypoint* p, waypoints) + { + if (p->getFrame() == MAV_FRAME_GLOBAL) + { + i++; + } + } + + return i; +} + +int UASWaypointManager::getLocalFrameCount() +{ + // Search through all waypoints, + // counting only those in global frame + int i = 0; + foreach (Waypoint* p, waypoints) + { + if (p->getFrame() == MAV_FRAME_GLOBAL) + { + i++; + } + } + + return i; +} + +int UASWaypointManager::getLocalFrameIndexOf(Waypoint* wp) +{ + // Search through all waypoints, + // counting only those in local frame + int i = 0; + foreach (Waypoint* p, waypoints) + { + if (p->getFrame() == MAV_FRAME_LOCAL) + { + if (p == wp) + { + return i; + } + i++; + } + } + + return -1; +} + +int UASWaypointManager::getMissionFrameIndexOf(Waypoint* wp) +{ + // Search through all waypoints, + // counting only those in mission frame + int i = 0; + foreach (Waypoint* p, waypoints) + { + if (p->getFrame() == MAV_FRAME_MISSION) + { + if (p == wp) + { + return i; + } + i++; + } + } + + return -1; +} + void UASWaypointManager::readWaypoints() { emit readGlobalWPFromUAS(true); @@ -555,17 +666,16 @@ void UASWaypointManager::writeWaypoints() cur_d->autocontinue = cur_s->getAutoContinue(); cur_d->current = cur_s->getCurrent() & noCurrent; //make sure only one current waypoint is selected, the first selected will be chosen - cur_d->orbit = 0; - cur_d->orbit_direction = 0; - cur_d->param1 = cur_s->getOrbit(); - cur_d->param2 = cur_s->getHoldTime(); + cur_d->param1 = cur_s->getParam1(); + cur_d->param2 = cur_s->getParam2(); + cur_d->param3 = cur_s->getParam3(); + cur_d->param4 = cur_s->getParam4(); cur_d->frame = cur_s->getFrame(); - cur_d->action = cur_s->getAction(); + cur_d->command = cur_s->getAction(); cur_d->seq = i; // don't read out the sequence number of the waypoint class cur_d->x = cur_s->getX(); cur_d->y = cur_s->getY(); cur_d->z = cur_s->getZ(); - cur_d->yaw = cur_s->getYaw(); if (cur_s->getCurrent() && noCurrent) noCurrent = false; diff --git a/src/uas/UASWaypointManager.h b/src/uas/UASWaypointManager.h index 5e62476144ed26d03853b284d59e78caf028da7a..0157d4ae6076177cd9b97d62e8e7cd2c2c622227 100644 --- a/src/uas/UASWaypointManager.h +++ b/src/uas/UASWaypointManager.h @@ -86,7 +86,13 @@ public: /** @name Waypoint list operations */ /*@{*/ const QVector &getWaypointList(void) { return waypoints; } ///< Returns a const reference to the waypoint list. - int getIndexOf(Waypoint* wp); ///< Get the index of a waypoint in the list + const QVector getGlobalFrameWaypointList(); ///< Returns a global waypoint list + int getIndexOf(Waypoint* wp); ///< Get the index of a waypoint in the list + int getGlobalFrameIndexOf(Waypoint* wp); ///< Get the index of a waypoint in the list, counting only global waypoints + int getLocalFrameIndexOf(Waypoint* wp); ///< Get the index of a waypoint in the list, counting only local waypoints + int getMissionFrameIndexOf(Waypoint* wp); ///< Get the index of a waypoint in the list, counting only mission waypoints + int getGlobalFrameCount(); ///< Get the count of global waypoints in the list + int getLocalFrameCount(); ///< Get the count of local waypoints in the list /*@}*/ UAS& getUAS() { return this->uas; } ///< Returns the owning UAS diff --git a/src/ui/CommConfigurationWindow.cc b/src/ui/CommConfigurationWindow.cc index 8b0bfd8f0845bdd975944dac575b86a00ecb7878..2a87d59d4c58a7fd1a854777c6c7efa8e12c9207 100644 --- a/src/ui/CommConfigurationWindow.cc +++ b/src/ui/CommConfigurationWindow.cc @@ -47,6 +47,7 @@ This file is part of the QGROUNDCONTROL project #endif #include "MAVLinkProtocol.h" #include "MAVLinkSettingsWidget.h" +#include "QGCUDPLinkConfiguration.h" #include "LinkManager.h" CommConfigurationWindow::CommConfigurationWindow(LinkInterface* link, ProtocolInterface* protocol, QWidget *parent, Qt::WindowFlags flags) : QWidget(parent, flags) @@ -57,20 +58,24 @@ CommConfigurationWindow::CommConfigurationWindow(LinkInterface* link, ProtocolIn ui.setupUi(this); // add link types - ui.linkType->addItem("Serial",QGC_LINK_SERIAL); - ui.linkType->addItem("UDP",QGC_LINK_UDP); - ui.linkType->addItem("Simulation",QGC_LINK_SIMULATION); - ui.linkType->addItem("Serial Forwarding",QGC_LINK_FORWARDING); + ui.linkType->addItem(tr("Serial"), QGC_LINK_SERIAL); + ui.linkType->addItem(tr("UDP"), QGC_LINK_UDP); + ui.linkType->addItem(tr("Simulation"), QGC_LINK_SIMULATION); + ui.linkType->addItem(tr("Opal-RT Link"), QGC_LINK_OPAL); + ui.linkType->setEditable(false); + //ui.linkType->setEnabled(false); ui.connectionType->addItem("MAVLink", QGC_PROTOCOL_MAVLINK); - ui.connectionType->addItem("GPS NMEA", QGC_PROTOCOL_NMEA); + //ui.connectionType->addItem("GPS NMEA", QGC_PROTOCOL_NMEA); // Create action to open this menu // Create configuration action for this link // Connect the current UAS - action = new QAction(QIcon(":/images/devices/network-wireless.svg"), "", link); + action = new QAction(QIcon(":/images/devices/network-wireless.svg"), "", this); LinkManager::instance()->add(link); action->setData(LinkManager::instance()->getLinks().indexOf(link)); + action->setEnabled(true); + action->setVisible(true); setLinkName(link->getName()); connect(action, SIGNAL(triggered()), this, SLOT(show())); @@ -110,17 +115,24 @@ CommConfigurationWindow::CommConfigurationWindow(LinkInterface* link, ProtocolIn layout->addWidget(conf); ui.linkGroupBox->setLayout(layout); ui.linkGroupBox->setTitle(tr("Serial Link")); + ui.linkType->setCurrentIndex(0); //ui.linkGroupBox->setTitle(link->getName()); //connect(link, SIGNAL(nameChanged(QString)), ui.linkGroupBox, SLOT(setTitle(QString))); } UDPLink* udp = dynamic_cast(link); if (udp != 0) { + QWidget* conf = new QGCUDPLinkConfiguration(udp, this); + QBoxLayout* layout = new QBoxLayout(QBoxLayout::LeftToRight, ui.linkGroupBox); + layout->addWidget(conf); + ui.linkGroupBox->setLayout(layout); ui.linkGroupBox->setTitle(tr("UDP Link")); + ui.linkType->setCurrentIndex(1); } MAVLinkSimulationLink* sim = dynamic_cast(link); if (sim != 0) { + ui.linkType->setCurrentIndex(2); ui.linkGroupBox->setTitle(tr("MAVLink Simulation Link")); } #ifdef OPAL_RT @@ -131,6 +143,7 @@ CommConfigurationWindow::CommConfigurationWindow(LinkInterface* link, ProtocolIn QBoxLayout* layout = new QBoxLayout(QBoxLayout::LeftToRight, ui.linkGroupBox); layout->addWidget(conf); ui.linkGroupBox->setLayout(layout); + ui.linkType->setCurrentIndex(3); ui.linkGroupBox->setTitle(tr("Opal-RT Link")); } #endif @@ -201,10 +214,9 @@ void CommConfigurationWindow::setConnection() void CommConfigurationWindow::setLinkName(QString name) { - Q_UNUSED(name); // FIXME - action->setText(tr("Configure ") + link->getName()); - action->setStatusTip(tr("Configure ") + link->getName()); - this->window()->setWindowTitle(tr("Settings for ") + this->link->getName()); + action->setText(tr("%1 Settings").arg(name)); + action->setStatusTip(tr("Adjust setting for link %1").arg(name)); + this->window()->setWindowTitle(tr("Settings for %1").arg(name)); } void CommConfigurationWindow::remove() diff --git a/src/ui/CommConfigurationWindow.h b/src/ui/CommConfigurationWindow.h index 27b1ad2fb5fdb1b6ed7af153acb679553c4471d3..6b126f808f4619b13d3fa5598ceea661a8db3667 100644 --- a/src/ui/CommConfigurationWindow.h +++ b/src/ui/CommConfigurationWindow.h @@ -44,7 +44,8 @@ enum qgc_link_t QGC_LINK_SERIAL, QGC_LINK_UDP, QGC_LINK_SIMULATION, - QGC_LINK_FORWARDING + QGC_LINK_FORWARDING, + QGC_LINK_OPAL }; enum qgc_protocol_t diff --git a/src/ui/DebugConsole.cc b/src/ui/DebugConsole.cc index 820b7d3400954d68c100cfc7e89f4ccc8722f7a8..58ec696d57ef234e9dfedfa7b64d6ac8bd2d0b20 100644 --- a/src/ui/DebugConsole.cc +++ b/src/ui/DebugConsole.cc @@ -29,6 +29,7 @@ This file is part of the QGROUNDCONTROL project * */ #include +#include #include "DebugConsole.h" #include "ui_DebugConsole.h" @@ -45,6 +46,7 @@ DebugConsole::DebugConsole(QWidget *parent) : holdOn(false), convertToAscii(true), filterMAVLINK(false), + autoHold(true), bytesToIgnore(0), lastByte(-1), sentBytes(), @@ -57,7 +59,7 @@ DebugConsole::DebugConsole(QWidget *parent) : dataRate(0.0f), lowpassDataRate(0.0f), dataRateThreshold(500), - autoHold(true), + commandIndex(0), m_ui(new Ui::DebugConsole) { // Setup basic user interface @@ -65,7 +67,7 @@ DebugConsole::DebugConsole(QWidget *parent) : // Hide sent text field - it is only useful after send has been hit m_ui->sentText->setVisible(false); // Hide auto-send checkbox - m_ui->specialCheckBox->setVisible(false); + //m_ui->specialCheckBox->setVisible(false); // Make text area not editable m_ui->receiveText->setReadOnly(true); // Limit to 500 lines @@ -83,10 +85,10 @@ DebugConsole::DebugConsole(QWidget *parent) : snapShotTimer.setInterval(snapShotInterval); snapShotTimer.start(); - // Set hex checkbox checked - m_ui->hexCheckBox->setChecked(!convertToAscii); - m_ui->mavlinkCheckBox->setChecked(filterMAVLINK); - m_ui->holdCheckBox->setChecked(autoHold); +// // Set hex checkbox checked +// m_ui->hexCheckBox->setChecked(!convertToAscii); +// m_ui->mavlinkCheckBox->setChecked(filterMAVLINK); +// m_ui->holdCheckBox->setChecked(autoHold); // Get a list of all existing links links = QList(); @@ -113,17 +115,68 @@ DebugConsole::DebugConsole(QWidget *parent) : connect(m_ui->addSymbolButton, SIGNAL(clicked()), this, SLOT(appendSpecialSymbol())); // Connect Checkbox connect(m_ui->specialComboBox, SIGNAL(highlighted(QString)), this, SLOT(specialSymbolSelected(QString))); + // Set add button invisible if auto add checkbox is checked + //connect(m_ui->specialCheckBox, SIGNAL(clicked(bool)), m_ui->addSymbolButton, SLOT(setHidden(bool))); + // Allow to send via return + connect(m_ui->sendText, SIGNAL(returnPressed()), this, SLOT(sendBytes())); - hold(false); + loadSettings(); + + // Warn user about not activated hold + if (!m_ui->holdCheckBox->isChecked()) + { + m_ui->receiveText->appendHtml(QString("%2\n").arg(QColor(Qt::red).name(), tr("WARNING: You have NOT enabled auto-hold (stops updating the console is huge amounts of serial data arrive). Updating the console consumes significant CPU load, so if you receive more than about 5 KB/s of serial data, make sure to enable auto-hold if not using the console."))); + } +} - this->setVisible(false); +void DebugConsole::hideEvent(QHideEvent* event) +{ + Q_UNUSED(event); + storeSettings(); } DebugConsole::~DebugConsole() { + storeSettings(); delete m_ui; } +void DebugConsole::loadSettings() +{ + // Load defaults from settings + QSettings settings; + settings.sync(); + settings.beginGroup("QGC_DEBUG_CONSOLE"); + m_ui->specialComboBox->setCurrentIndex(settings.value("SPECIAL_SYMBOL", m_ui->specialComboBox->currentIndex()).toInt()); + m_ui->specialCheckBox->setChecked(settings.value("SPECIAL_SYMBOL_CHECKBOX_STATE", m_ui->specialCheckBox->isChecked()).toBool()); + hexModeEnabled(settings.value("HEX_MODE_ENABLED", m_ui->hexCheckBox->isChecked()).toBool()); + MAVLINKfilterEnabled(settings.value("MAVLINK_FILTER_ENABLED", filterMAVLINK).toBool()); + setAutoHold(settings.value("AUTO_HOLD_ENABLED", autoHold).toBool()); + settings.endGroup(); + +// // Update visibility settings +// if (m_ui->specialCheckBox->isChecked()) +// { +// m_ui->specialCheckBox->setVisible(true); +// m_ui->addSymbolButton->setVisible(false); +// } +} + +void DebugConsole::storeSettings() +{ + // Store settings + QSettings settings; + settings.beginGroup("QGC_DEBUG_CONSOLE"); + settings.setValue("SPECIAL_SYMBOL", m_ui->specialComboBox->currentIndex()); + settings.setValue("SPECIAL_SYMBOL_CHECKBOX_STATE", m_ui->specialCheckBox->isChecked()); + settings.setValue("HEX_MODE_ENABLED", m_ui->hexCheckBox->isChecked()); + settings.setValue("MAVLINK_FILTER_ENABLED", filterMAVLINK); + settings.setValue("AUTO_HOLD_ENABLED", autoHold); + settings.endGroup(); + settings.sync(); + //qDebug() << "Storing settings!"; +} + /** * Add a link to the debug console output */ @@ -193,6 +246,11 @@ void DebugConsole::setAutoHold(bool hold) this->hold(false); m_ui->holdButton->setChecked(false); } + // Set auto hold checkbox + if (m_ui->holdCheckBox->isChecked() != hold) + { + m_ui->holdCheckBox->setChecked(hold); + } // Set new state autoHold = hold; } @@ -203,7 +261,31 @@ void DebugConsole::setAutoHold(bool hold) void DebugConsole::receiveTextMessage(int id, int component, int severity, QString text) { Q_UNUSED(severity); - m_ui->receiveText->appendHtml(QString("(MAV%2:%3) %4").arg(UASManager::instance()->getUASForId(id)->getColor().name(), QString::number(id), QString::number(component), text)); + QString name = UASManager::instance()->getUASForId(id)->getUASName(); + QString comp; + // Get a human readable name if possible + switch (component) + { + // TODO: To be completed + case MAV_COMP_ID_IMU: + comp = tr("IMU"); + break; + case MAV_COMP_ID_MAPPER: + comp = tr("MAPPER"); + break; + case MAV_COMP_ID_WAYPOINTPLANNER: + comp = tr("WP-PLANNER"); + break; + case MAV_COMP_ID_AIRSLAM: + comp = tr("AIRSLAM"); + break; + default: + comp = QString::number(component); + break; + } + + m_ui->receiveText->appendHtml(QString("(%2:%3) %4\n").arg(UASManager::instance()->getUASForId(id)->getColor().name(), name, comp, text)); + //m_ui->receiveText->appendPlainText(""); } void DebugConsole::updateTrafficMeasurements() @@ -291,15 +373,23 @@ void DebugConsole::receiveBytes(LinkInterface* link, QByteArray bytes) { switch (byte) { - // Catch line feed -// case (unsigned char)'\n': -// m_ui->receiveText->appendPlainText(str); -// str = ""; -// break; - // Catch carriage return and line feed - case (unsigned char)0xD: - case (unsigned char)0xA: - // Ignore + // Accept line feed and tab + case (unsigned char)'\n': + { + if (lastByte != '\r') + { + // Do not break line again for CR+LF + // only break line for single LF bytes + str.append(byte); + } + } + break; + case (unsigned char)'\t': + str.append(byte); + break; + // Catch and ignore carriage return + case (unsigned char)'\r': + str.append(byte); break; default: str.append(QChar(QChar::ReplacementCharacter)); @@ -319,6 +409,7 @@ void DebugConsole::receiveBytes(LinkInterface* link, QByteArray bytes) str.append(str2); } lineBuffer.append(str); + lastByte = byte; } else { @@ -328,9 +419,13 @@ void DebugConsole::receiveBytes(LinkInterface* link, QByteArray bytes) } } - if (lineBuffer.length() > 0) m_ui->receiveText->appendPlainText(lineBuffer); - lineBuffer.clear(); - + if (lineBuffer.length() > 0) + { + m_ui->receiveText->insertPlainText(lineBuffer); + // Ensure text area scrolls correctly + m_ui->receiveText->ensureCursorVisible(); + lineBuffer.clear(); + } } else if (link == currLink && holdOn) { @@ -341,50 +436,96 @@ void DebugConsole::receiveBytes(LinkInterface* link, QByteArray bytes) QByteArray DebugConsole::symbolNameToBytes(const QString& text) { QByteArray b; - if (text == "LF") + if (text.contains("CR+LF")) { + b.append(static_cast(0x0D)); b.append(static_cast(0x0A)); } - else if (text == "FF") + else if (text.contains("LF")) { - b.append(static_cast(0x0C)); + b.append(static_cast(0x0A)); } - else if (text == "CR") + else if (text.contains("FF")) { - b.append(static_cast(0x0D)); + b.append(static_cast(0x0C)); } - else if (text == "CR+LF") + else if (text.contains("CR")) { b.append(static_cast(0x0D)); - b.append(static_cast(0x0A)); } - else if (text == "TAB") + else if (text.contains("TAB")) { b.append(static_cast(0x09)); } - else if (text == "NUL") + else if (text.contains("NUL")) { b.append(static_cast(0x00)); } - else if (text == "ESC") + else if (text.contains("ESC")) { b.append(static_cast(0x1B)); } - else if (text == "~") + else if (text.contains("~")) { b.append(static_cast(0x7E)); } - else if (text == "") + else if (text.contains("")) { b.append(static_cast(0x20)); } return b; } +QString DebugConsole::bytesToSymbolNames(const QByteArray& b) +{ + QString text; + if (b.size() > 1 && b.contains(0x0D) && b.contains(0x0A)) + { + text = ""; + } + else if (b.contains(0x0A)) + { + text = ""; + } + else if (b.contains(0x0C)) + { + text = ""; + } + else if (b.contains(0x0D)) + { + text = ""; + } + else if (b.contains(0x09)) + { + text = ""; + } + else if (b.contains((char)0x00)) + { + text == ""; + } + else if (b.contains(0x1B)) + { + text = ""; + } + else if (b.contains(0x7E)) + { + text = "<~>"; + } + else if (b.contains(0x20)) + { + text = ""; + } + else + { + text.append(b); + } + return text; +} + void DebugConsole::specialSymbolSelected(const QString& text) { Q_UNUSED(text); - m_ui->specialCheckBox->setVisible(true); + //m_ui->specialCheckBox->setVisible(true); } void DebugConsole::appendSpecialSymbol(const QString& text) @@ -415,6 +556,18 @@ void DebugConsole::appendSpecialSymbol() void DebugConsole::sendBytes() { + // FIXME This store settings should be removed + // once all threading issues have been resolved + // since its called in the destructor, which + // is absolutely sufficient + storeSettings(); + + // Store command history + commandHistory.append(m_ui->sendText->text()); + // Since text was just sent, we're at position commandHistory.length() + // which is the current text + commandIndex = commandHistory.length(); + if (!m_ui->sentText->isVisible()) { m_ui->sentText->setVisible(true); @@ -426,10 +579,27 @@ void DebugConsole::sendBytes() return; } + QString transmitUnconverted = m_ui->sendText->text(); + QByteArray specialSymbol; + // Append special symbol if checkbox is checked if (m_ui->specialCheckBox->isChecked()) { - appendSpecialSymbol(m_ui->specialComboBox->currentText()); + // Get auto-add special symbols + specialSymbol = symbolNameToBytes(m_ui->specialComboBox->currentText()); + + // Convert them if needed + if (!convertToAscii) + { + QString specialSymbolConverted; + for (int i = 0; i < specialSymbol.length(); i++) + { + QString add(" 0x%1"); + specialSymbolConverted.append(add.arg(static_cast(specialSymbol.at(i)), 2, 16, QChar('0'))); + } + specialSymbol.clear(); + specialSymbol.append(specialSymbolConverted); + } } QByteArray transmit; @@ -438,13 +608,27 @@ void DebugConsole::sendBytes() if (convertToAscii) { // ASCII text is not converted - transmit = m_ui->sendText->text().toLatin1(); - feedback = transmit; + transmit = transmitUnconverted.toLatin1(); + // Auto-add special symbol handling + transmit.append(specialSymbol); + + QString translated; + + // Replace every occurence of a special symbol with its text name + for (int i = 0; i < transmit.size(); ++i) + { + QByteArray specialChar; + specialChar.append(transmit.at(i)); + translated.append(bytesToSymbolNames(specialChar)); + } + + feedback.append(translated); } else { // HEX symbols are converted to bytes - QString str = m_ui->sendText->text().toLatin1(); + QString str = transmitUnconverted.toLatin1(); + str.append(specialSymbol); str.remove(' '); str.remove("0x"); str.simplified(); @@ -485,16 +669,16 @@ void DebugConsole::sendBytes() if (ok && m_ui->sendText->text().toLatin1().size() > 0) { // Transmit only if conversion succeeded -// int transmitted = - currLink->writeBytes(transmit, transmit.size()); -// if (transmit.size() == transmitted) -// { - m_ui->sentText->setText(tr("Sent: ") + feedback); -// } -// else -// { -// m_ui->sentText->setText(tr("Error during sending: Transmitted only %1 bytes instead of %2.").arg(transmitted, transmit.size())); -// } + // int transmitted = + currLink->writeBytes(transmit, transmit.size()); + // if (transmit.size() == transmitted) + // { + m_ui->sentText->setText(tr("Sent: ") + feedback); + // } + // else + // { + // m_ui->sentText->setText(tr("Error during sending: Transmitted only %1 bytes instead of %2.").arg(transmitted, transmit.size())); + // } } else if (m_ui->sendText->text().toLatin1().size() > 0) { @@ -512,10 +696,18 @@ void DebugConsole::sendBytes() */ void DebugConsole::hexModeEnabled(bool mode) { - convertToAscii = !mode; - m_ui->receiveText->clear(); - m_ui->sendText->clear(); - m_ui->sentText->clear(); + if (convertToAscii == mode) + { + convertToAscii = !mode; + if (m_ui->hexCheckBox->isChecked() != mode) + { + m_ui->hexCheckBox->setChecked(mode); + } + m_ui->receiveText->clear(); + m_ui->sendText->clear(); + m_ui->sentText->clear(); + commandHistory.clear(); + } } /** @@ -523,14 +715,23 @@ void DebugConsole::hexModeEnabled(bool mode) */ void DebugConsole::MAVLINKfilterEnabled(bool filter) { - filterMAVLINK = filter; - bytesToIgnore = 0; + if (filterMAVLINK != filter) + { + filterMAVLINK = filter; + bytesToIgnore = 0; + if (m_ui->mavlinkCheckBox->isChecked() != filter) + { + m_ui->mavlinkCheckBox->setChecked(filter); + } + } } /** * @param hold Freeze the input and thus any scrolling */ void DebugConsole::hold(bool hold) { + if (holdOn != hold) + { // Check if we need to append bytes from the hold buffer if (this->holdOn && !hold) { @@ -551,6 +752,11 @@ void DebugConsole::hold(bool hold) { m_ui->receiveText->setTextInteractionFlags(Qt::NoTextInteraction); } + if (m_ui->holdCheckBox->isChecked() != hold) + { + m_ui->holdCheckBox->setChecked(hold); + } +} } /** @@ -561,12 +767,12 @@ void DebugConsole::setConnectionState(bool connected) if(connected) { m_ui->connectButton->setText(tr("Disconn.")); - m_ui->receiveText->appendHtml(QString("%2").arg(QGC::colorGreen.name(), tr("Link %1 is connected.").arg(currLink->getName()))); + m_ui->receiveText->appendHtml(QString("%2\n").arg(QGC::colorGreen.name(), tr("Link %1 is connected.").arg(currLink->getName()))); } else { m_ui->connectButton->setText(tr("Connect")); - m_ui->receiveText->appendHtml(QString("%2").arg(QGC::colorYellow.name(), tr("Link %1 is unconnected.").arg(currLink->getName()))); + m_ui->receiveText->appendHtml(QString("%2\n").arg(QGC::colorOrange.name(), tr("Link %1 is unconnected.").arg(currLink->getName()))); } } @@ -586,6 +792,75 @@ void DebugConsole::handleConnectButton() } } +void DebugConsole::keyPressEvent(QKeyEvent * event) +{ + if (event->key() == Qt::Key_Up) + { + cycleCommandHistory(true); + } + else if (event->key() == Qt::Key_Down) + { + cycleCommandHistory(false); + } + else + { + QWidget::keyPressEvent(event); + } +} + +void DebugConsole::cycleCommandHistory(bool up) +{ + // Only cycle if there is a history + if (commandHistory.length() > 0) + { + // Store current command if we're not in history yet + if (commandIndex == commandHistory.length() && up) + { + currCommand = m_ui->sendText->text(); + } + + if (up) + { + // UP + commandIndex--; + if (commandIndex >= 0) + { + m_ui->sendText->setText(commandHistory.at(commandIndex)); + } + + // If the index + } + else + { + // DOWN + commandIndex++; + if (commandIndex < commandHistory.length()) + { + m_ui->sendText->setText(commandHistory.at(commandIndex)); + } + // If the index is at history length, load the last current command + + } + + // Restore current command if we went out of history + if (commandIndex == commandHistory.length()) + { + m_ui->sendText->setText(currCommand); + } + + // If we are too far down or too far up, wrap around to current command + if (commandIndex < 0 || commandIndex > commandHistory.length()) + { + commandIndex = commandHistory.length(); + m_ui->sendText->setText(currCommand); + } + + // Bound the index + if (commandIndex < 0) commandIndex = 0; + if (commandIndex > commandHistory.length()) commandIndex = commandHistory.length(); + } +} + void DebugConsole::changeEvent(QEvent *e) { QWidget::changeEvent(e); diff --git a/src/ui/DebugConsole.h b/src/ui/DebugConsole.h index a441d84f446466c7fea48481fa98598e4cdff6f3..d3052ded52c963494f53e0efaffe8145af502309 100644 --- a/src/ui/DebugConsole.h +++ b/src/ui/DebugConsole.h @@ -36,6 +36,7 @@ This file is part of the QGROUNDCONTROL project #include #include #include +#include #include "LinkInterface.h" @@ -95,10 +96,20 @@ public slots: void paintEvent(QPaintEvent *event); /** @brief Update traffic measurements */ void updateTrafficMeasurements(); + void loadSettings(); + void storeSettings(); protected: void changeEvent(QEvent *e); + void hideEvent(QHideEvent* event); + /** @brief Convert a symbol name to the byte representation */ QByteArray symbolNameToBytes(const QString& symbol); + /** @brief Convert a symbol byte to the name */ + QString bytesToSymbolNames(const QByteArray& b); + /** @brief Handle keypress events */ + void keyPressEvent(QKeyEvent * event); + /** @brief Cycle through the command history */ + void cycleCommandHistory(bool up); QList links; LinkInterface* currLink; @@ -106,6 +117,7 @@ protected: bool holdOn; ///< Hold current view, ignore new data bool convertToAscii; ///< Convert data to ASCII bool filterMAVLINK; ///< Set true to filter out MAVLink in output + bool autoHold; ///< Auto-hold mode sets view into hold if the data rate is too high int bytesToIgnore; ///< Number of bytes to ignore char lastByte; ///< The last received byte QList sentBytes; ///< Transmitted bytes, per transmission @@ -118,7 +130,9 @@ protected: float dataRate; ///< Current data rate float lowpassDataRate; ///< Lowpass filtered data rate float dataRateThreshold; ///< Threshold where to enable auto-hold - bool autoHold; ///< Auto-hold mode sets view into hold if the data rate is too high + QStringList commandHistory; + QString currCommand; + int commandIndex; private: Ui::DebugConsole *m_ui; diff --git a/src/ui/DebugConsole.ui b/src/ui/DebugConsole.ui index bb32e331b16ab6ed5d4e99bbf7febc7f65513a32..9fcfe00d4989ebb2c89f58ef62511bba2ddf0f92 100644 --- a/src/ui/DebugConsole.ui +++ b/src/ui/DebugConsole.ui @@ -6,7 +6,7 @@ 0 0 - 466 + 566 190 @@ -50,6 +50,9 @@ Ignore MAVLINK protocol messages in display + + Ignore MAVLINK protocol messages in display + Hide MAVLink @@ -58,7 +61,10 @@ - Display and send bytes in HEX representation + Display and enter bytes in HEX representation (e.g. 0xAA) + + + Display and enter bytes in HEX representation (e.g. 0xAA) HEX @@ -171,11 +177,6 @@ TAB - - - NUL - - ESC diff --git a/src/ui/HDDisplay.cc b/src/ui/HDDisplay.cc index d598f458fdafa244affd911148dabac7130a8d34..5f655587eacbfbbaa84587377c9a1a16aa85a254 100644 --- a/src/ui/HDDisplay.cc +++ b/src/ui/HDDisplay.cc @@ -18,28 +18,17 @@ #include #include #include +#include #include "UASManager.h" #include "HDDisplay.h" #include "ui_HDDisplay.h" #include "MG.h" #include "QGC.h" -#ifndef M_PI -#define M_PI 3.14159265358979323846 -#endif #include HDDisplay::HDDisplay(QStringList* plotList, QString title, QWidget *parent) : QGraphicsView(parent), uas(NULL), - values(QMap()), - valuesDot(QMap()), - valuesMean(QMap()), - valuesCount(QMap()), - lastUpdate(QMap()), - minValues(), - maxValues(), - goodRanges(), - critRanges(), xCenterOffset(0.0f), yCenterOffset(0.0f), vwidth(80.0f), @@ -293,12 +282,12 @@ void HDDisplay::addGauge() } else { - items.append(QString("%1,%2,%3,%4").arg("-180").arg(key).arg(unit).arg("+180")); + items.append(QString("%1,%2,%3,%4").arg("0").arg(key).arg(unit).arg("+100")); } } bool ok; QString item = QInputDialog::getItem(this, tr("Add Gauge Instrument"), - tr("Format: min, curve name, max[,s]"), items, 0, true, &ok); + tr("Format: min, curve name, unit, max[,s]"), items, 0, true, &ok); if (ok && !item.isEmpty()) { addGauge(item); @@ -474,18 +463,17 @@ void HDDisplay::renderOverlay() */ void HDDisplay::setActiveUAS(UASInterface* uas) { - //qDebug() << "ATTEMPTING TO SET UAS"; if (this->uas != NULL) { // Disconnect any previously connected active MAV disconnect(this->uas, SIGNAL(valueChanged(int,QString,QString,double,quint64)), this, SLOT(updateValue(int,QString,QString,double,quint64))); + disconnect(this->uas, SIGNAL(valueChanged(int,QString,QString,int,quint64)), this, SLOT(updateValue(int,QString,QString,int,quint64))); } // Now connect the new UAS - - //qDebug() << "UAS SET!" << "ID:" << uas->getUASID(); // Setup communication connect(uas, SIGNAL(valueChanged(int,QString,QString,double,quint64)), this, SLOT(updateValue(int,QString,QString,double,quint64))); + connect(uas, SIGNAL(valueChanged(int,QString,QString,int,quint64)), this, SLOT(updateValue(int,QString,QString,int,quint64))); this->uas = uas; } @@ -629,7 +617,16 @@ void HDDisplay::drawGauge(float xRef, float yRef, float radius, float min, float //drawCircle(xRef, yRef+nameHeight, radius, 0.0f, 170.0f, 1.0f, color, painter); QString label; - label.sprintf("% 06.1f", value); + + // Show integer values without decimal places + if (intValues.contains(name)) + { + label.sprintf("% 05d", (int)value); + } + else + { + label.sprintf("% 06.1f", value); + } // Text @@ -857,6 +854,12 @@ float HDDisplay::refLineWidthToPen(float line) return line * 2.50f; } +void HDDisplay::updateValue(const int uasId, const QString& name, const QString& unit, const int value, const quint64 msec) +{ + if (!intValues.contains(name)) intValues.insert(name, true); + updateValue(uasId, name, unit, (double)value, msec); +} + void HDDisplay::updateValue(const int uasId, const QString& name, const QString& unit, const double value, const quint64 msec) { Q_UNUSED(uasId); diff --git a/src/ui/HDDisplay.h b/src/ui/HDDisplay.h index ee148629bd0a21435c389d5d2cb6fe9d98c543bc..63af2e57cdf801cac83977b57aa876dac6e3bf5d 100644 --- a/src/ui/HDDisplay.h +++ b/src/ui/HDDisplay.h @@ -63,9 +63,11 @@ public: ~HDDisplay(); public slots: - /** @brief Update a HDD value */ + /** @brief Update a HDD double value */ void updateValue(const int uasId, const QString& name, const QString& unit, const double value, const quint64 msec); - void setActiveUAS(UASInterface* uas); + /** @brief Update a HDD integer value */ + void updateValue(const int uasId, const QString& name, const QString& unit, const int value, const quint64 msec); + virtual void setActiveUAS(UASInterface* uas); /** @brief Removes a plot item by the action data */ void removeItemByAction(); @@ -137,14 +139,15 @@ protected: UASInterface* uas; ///< The uas currently monitored QMap values; ///< The variables this HUD displays - QMap units; ///< The units + QMap units; ///< The units QMap valuesDot; ///< First derivative of the variable QMap valuesMean; ///< Mean since system startup for this variable QMap valuesCount; ///< Number of values received so far QMap lastUpdate; ///< The last update time for this variable QMap minValues; ///< The minimum value this variable is assumed to have QMap maxValues; ///< The maximum value this variable is assumed to have - QMap symmetric; ///< Draw the gauge / dial symmetric bool = yes + QMap symmetric; ///< Draw the gauge / dial symmetric bool = yes + QMap intValues; ///< Is the gauge value an integer? QMap > goodRanges; ///< The range of good values QMap > critRanges; ///< The range of critical values double scalingFactor; ///< Factor used to scale all absolute values to screen coordinates diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc index 2b7da31cd97b867ae1e1d3c4aaba84701102f6f6..8c058793491e2981f9ef32a88319ca4d6c5a6040 100644 --- a/src/ui/HSIDisplay.cc +++ b/src/ui/HSIDisplay.cc @@ -37,11 +37,11 @@ This file is part of the QGROUNDCONTROL project #include #include "UASManager.h" #include "HSIDisplay.h" -#include "MG.h" #include "QGC.h" #include "Waypoint.h" #include "UASWaypointManager.h" #include "Waypoint2DIcon.h" +#include "MAV2DIcon.h" #include @@ -49,34 +49,34 @@ HSIDisplay::HSIDisplay(QWidget *parent) : HDDisplay(NULL, "HSI", parent), gpsSatellites(), satellitesUsed(0), - attXSet(0), - attYSet(0), - attYawSet(0), - altitudeSet(1.0), - posXSet(0), - posYSet(0), - posZSet(0), + attXSet(0.0f), + attYSet(0.0f), + attYawSet(0.0f), + altitudeSet(1.0f), + posXSet(0.0f), + posYSet(0.0f), + posZSet(0.0f), attXSaturation(0.5f), attYSaturation(0.5f), attYawSaturation(0.5f), - posXSaturation(0.05), - posYSaturation(0.05), - altitudeSaturation(1.0), - lat(0), - lon(0), - alt(0), + posXSaturation(0.05f), + posYSaturation(0.05f), + altitudeSaturation(1.0f), + lat(0.0f), + lon(0.0f), + alt(0.0f), globalAvailable(0), - x(0), - y(0), - z(0), - vx(0), - vy(0), - vz(0), - speed(0), + x(0.0f), + y(0.0f), + z(0.0f), + vx(0.0f), + vy(0.0f), + vz(0.0f), + speed(0.0f), localAvailable(0), - roll(0), - pitch(0), - yaw(1.0f), // FIXME Should be 0 + roll(0.0f), + pitch(0.0f), + yaw(0.0f), bodyXSetCoordinate(0.0f), bodyYSetCoordinate(0.0f), bodyZSetCoordinate(0.0f), @@ -85,7 +85,7 @@ HSIDisplay::HSIDisplay(QWidget *parent) : uiYSetCoordinate(0.0f), uiZSetCoordinate(0.0f), uiYawSet(0.0f), - metricWidth(4.0f), + metricWidth(4.0), positionLock(false), attControlEnabled(false), xyControlEnabled(false), @@ -96,23 +96,24 @@ HSIDisplay::HSIDisplay(QWidget *parent) : visionFix(0), laserFix(0), mavInitialized(false), - bottomMargin(3.0f), - topMargin(3.0f) + bottomMargin(10.0f), + topMargin(12.0f), + userSetPointSet(false) { - connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*))); refreshTimer->setInterval(updateInterval); columns = 1; + this->setAutoFillBackground(true); + QPalette pal = palette(); + pal.setColor(backgroundRole(), QGC::colorBlack); + setPalette(pal); - - // this->setScene(new QGraphicsScene(-metricWidth/2.0f, -metricWidth/2.0f, metricWidth, metricWidth, this)); - - vwidth = 80; - vheight = 80; + vwidth = 80.0f; + vheight = 80.0f; xCenterPos = vwidth/2.0f; yCenterPos = vheight/2.0f + topMargin - bottomMargin; - qDebug() << "CENTER" << xCenterPos << yCenterPos; + //qDebug() << "CENTER" << xCenterPos << yCenterPos; // Add interaction elements QHBoxLayout* layout = new QHBoxLayout(this); @@ -121,24 +122,62 @@ HSIDisplay::HSIDisplay(QWidget *parent) : QDoubleSpinBox* spinBox = new QDoubleSpinBox(this); spinBox->setMinimum(0.1); spinBox->setMaximum(9999); + spinBox->setMaximumWidth(50); + spinBox->setValue(metricWidth); + spinBox->setToolTip(tr("Ground width in meters shown on instrument")); + spinBox->setStatusTip(tr("Ground width in meters shown on instrument")); connect(spinBox, SIGNAL(valueChanged(double)), this, SLOT(setMetricWidth(double))); connect(this, SIGNAL(metricWidthChanged(double)), spinBox, SLOT(setValue(double))); layout->addWidget(spinBox); - layout->setAlignment(spinBox, Qt::AlignBottom | Qt::AlignLeft); + layout->setAlignment(spinBox, Qt::AlignBottom | Qt::AlignRight); this->setLayout(layout); - this->setVisible(false); + uas = NULL; + resetMAVState(); + // Do first update setMetricWidth(metricWidth); } +void HSIDisplay::resetMAVState() +{ + mavInitialized = false; + attControlKnown = false; + attControlEnabled = false; + xyControlKnown = false; + xyControlEnabled = false; + zControlKnown = false; + zControlEnabled = false; + yawControlKnown = false; + yawControlEnabled = false; + + // Draw position lock indicators + positionFixKnown = false; + positionFix = 0; + visionFixKnown = false; + visionFix = 0; + gpsFixKnown = false; + gpsFix = 0; + iruFixKnown = false; + iruFix = 0; + + // Data + setPointKnown = false; + localAvailable = 0; + globalAvailable = 0; + + // Setpoints + positionSetPointKnown = false; + setPointKnown = false; +} + void HSIDisplay::paintEvent(QPaintEvent * event) { Q_UNUSED(event); //paintGL(); - static quint64 interval = 0; - //qDebug() << "INTERVAL:" << MG::TIME::getGroundTimeNow() - interval << __FILE__ << __LINE__; - interval = MG::TIME::getGroundTimeNow(); +// static quint64 interval = 0; +// //qDebug() << "INTERVAL:" << MG::TIME::getGroundTimeNow() - interval << __FILE__ << __LINE__; +// interval = MG::TIME::getGroundTimeNow(); renderOverlay(); } @@ -149,7 +188,7 @@ void HSIDisplay::renderOverlay() #endif // Center location of the HSI gauge items - float bottomMargin = 3.0f; + //float bottomMargin = 3.0f; // Size of the ring instrument //const float margin = 0.1f; // 10% margin of total width on each side @@ -167,10 +206,6 @@ void HSIDisplay::renderOverlay() painter.setRenderHint(QPainter::Antialiasing, true); painter.setRenderHint(QPainter::HighQualityAntialiasing, true); - // Draw background - //painter.fillRect(QRect(0, 0, width(), height()), backgroundColor); - - // Draw base instrument // ---------------------- painter.setBrush(Qt::NoBrush); @@ -182,27 +217,39 @@ void HSIDisplay::renderOverlay() const int ringCount = 2; for (int i = 0; i < ringCount; i++) { - float radius = (vwidth - topMargin - bottomMargin) / (2.0f * i+1) / 2.0f - bottomMargin / 2.0f; + float radius = (vwidth - (topMargin + bottomMargin)*0.3f) / (1.3f * i+1) / 2.0f - bottomMargin / 2.0f; drawCircle(xCenterPos, yCenterPos, radius, 0.1f, ringColor, &painter); } // Draw orientation labels // Translate and rotate coordinate frame painter.translate((xCenterPos)*scalingFactor, (yCenterPos)*scalingFactor); - painter.rotate((yaw/(M_PI))*180.0f); + const float yawDeg = ((yaw/M_PI)*180.0f); + int yawRotate = static_cast(yawDeg) % 360; + painter.rotate(-yawRotate); paintText(tr("N"), ringColor, 3.5f, - 1.0f, - baseRadius - 5.5f, &painter); paintText(tr("S"), ringColor, 3.5f, - 1.0f, + baseRadius + 1.5f, &painter); - paintText(tr("E"), ringColor, 3.5f, + baseRadius + 2.0f, - 1.75f, &painter); + paintText(tr("E"), ringColor, 3.5f, + baseRadius + 3.0f, - 1.25f, &painter); paintText(tr("W"), ringColor, 3.5f, - baseRadius - 5.5f, - 1.75f, &painter); - painter.rotate((-yaw/(M_PI))*180.0f); + painter.rotate(+yawRotate); painter.translate(-(xCenterPos)*scalingFactor, -(yCenterPos)*scalingFactor); // Draw center indicator - QPolygonF p(3); - p.replace(0, QPointF(xCenterPos, yCenterPos-2.8484f)); - p.replace(1, QPointF(xCenterPos-2.0f, yCenterPos+2.0f)); - p.replace(2, QPointF(xCenterPos+2.0f, yCenterPos+2.0f)); - drawPolygon(p, &painter); +// QPolygonF p(3); +// p.replace(0, QPointF(xCenterPos, yCenterPos-4.0f)); +// p.replace(1, QPointF(xCenterPos-4.0f, yCenterPos+3.5f)); +// p.replace(2, QPointF(xCenterPos+4.0f, yCenterPos+3.5f)); +// drawPolygon(p, &painter); + + if (uas) + { + // Translate to center + painter.translate((xCenterPos)*scalingFactor, (yCenterPos)*scalingFactor); + QColor uasColor = uas->getColor(); + MAV2DIcon::drawAirframePolygon(uas->getAirframe(), painter, static_cast((vwidth/4.0f)*scalingFactor*1.1f), uasColor, 0.0f); + // Translate back + painter.translate(-(xCenterPos)*scalingFactor, -(yCenterPos)*scalingFactor); + } // ---------------------- @@ -222,13 +269,13 @@ void HSIDisplay::renderOverlay() // Draw position setpoints in body coordinates - if (uiXSetCoordinate != 0 || uiYSetCoordinate != 0) + if (userSetPointSet) { QColor spColor(150, 150, 150); drawSetpointXY(uiXSetCoordinate, uiYSetCoordinate, uiYawSet, spColor, painter); } - if (bodyXSetCoordinate != 0 || bodyYSetCoordinate != 0) + if (positionSetPointKnown) { // Draw setpoint drawSetpointXY(bodyXSetCoordinate, bodyYSetCoordinate, bodyYawSet, QGC::colorCyan, painter); @@ -243,37 +290,56 @@ void HSIDisplay::renderOverlay() // Labels on outer part and bottom - if (localAvailable > 0) + // Draw waypoints + drawWaypoints(painter); + + // Draw status flags + drawStatusFlag(2, 1, tr("ATT"), attControlEnabled, attControlKnown, painter); + drawStatusFlag(22, 1, tr("PXY"), xyControlEnabled, xyControlKnown, painter); + drawStatusFlag(44, 1, tr("PZ"), zControlEnabled, zControlKnown, painter); + drawStatusFlag(66, 1, tr("YAW"), yawControlEnabled, yawControlKnown, painter); + + // Draw position lock indicators + drawPositionLock(2, 5, tr("POS"), positionFix, positionFixKnown, painter); + drawPositionLock(22, 5, tr("VIS"), visionFix, visionFixKnown, painter); + drawPositionLock(44, 5, tr("GPS"), gpsFix, gpsFixKnown, painter); + drawPositionLock(66, 5, tr("IRU"), iruFix, iruFixKnown, painter); + + // Draw speed to top left + paintText(tr("SPEED"), QGC::colorCyan, 2.2f, 2, 11, &painter); + paintText(tr("%1 m/s").arg(speed, 5, 'f', 2, '0'), Qt::white, 2.2f, 12, 11, &painter); + + // Draw crosstrack error to top right + float crossTrackError = 0; + paintText(tr("XTRACK"), QGC::colorCyan, 2.2f, 57, 11, &painter); + paintText(tr("%1 m").arg(crossTrackError, 5, 'f', 2, '0'), Qt::white, 2.2f, 70, 11, &painter); + + // Draw position to bottom left + if (localAvailable > 0 && globalAvailable == 0) { // Position QString str; str.sprintf("%05.2f %05.2f %05.2f m", x, y, z); - paintText(str, ringColor, 3.0f, xCenterPos + baseRadius - 30.75f, vheight - 5.0f, &painter); - - // Speed - str.sprintf("%05.2f m/s", speed); - paintText(str, ringColor, 3.0f, 10.0f, vheight - 5.0f, &painter); + paintText(tr("POS"), QGC::colorCyan, 2.6f, 2, vheight- 5.0f, &painter); + paintText(str, Qt::white, 2.6f, 10, vheight - 5.0f, &painter); } - // Draw waypoints - drawWaypoints(painter); - - // Draw status flags - drawStatusFlag(2, 1, tr("ATT"), attControlEnabled, painter); - drawStatusFlag(22, 1, tr("PXY"), xyControlEnabled, painter); - drawStatusFlag(44, 1, tr("PZ"), zControlEnabled, painter); - drawStatusFlag(66, 1, tr("YAW"), yawControlEnabled, painter); + if (globalAvailable > 0) + { + // Position + QString str; + str.sprintf("lat: %05.2f lon: %06.2f alt: %06.2f", lat, lon, alt); + paintText(tr("GPS"), QGC::colorCyan, 2.6f, 2, vheight- 5.0f, &painter); + paintText(str, Qt::white, 2.6f, 10, vheight - 5.0f, &painter); + } - // Draw position lock indicators - drawPositionLock(2, 5, tr("POS"), positionFix, painter); - drawPositionLock(22, 5, tr("VIS"), visionFix, painter); - drawPositionLock(44, 5, tr("GPS"), gpsFix, painter); - drawPositionLock(66, 5, tr("IRU"), iruFix, painter); + // Draw Field of view to bottom right + paintText(tr("FOV"), QGC::colorCyan, 2.6f, 62, vheight- 5.0f, &painter); } -void HSIDisplay::drawStatusFlag(float x, float y, QString label, bool status, QPainter& painter) +void HSIDisplay::drawStatusFlag(float x, float y, QString label, bool status, bool known, QPainter& painter) { - paintText(label, QGC::colorCyan, 2.6f, x, y+0.35f, &painter); + paintText(label, QGC::colorCyan, 2.6f, x, y+0.8f, &painter); QColor statusColor(250, 250, 250); if(status) { @@ -284,46 +350,98 @@ void HSIDisplay::drawStatusFlag(float x, float y, QString label, bool status, QP painter.setBrush(QGC::colorDarkYellow); } painter.setPen(Qt::NoPen); - painter.drawRect(QRect(refToScreenX(x+7.3f), refToScreenY(y+0.05), refToScreenX(7.0f), refToScreenY(4.0f))); - paintText((status) ? tr("ON") : tr("OFF"), statusColor, 2.6f, x+7.9f, y+0.35f, &painter); -} -void HSIDisplay::drawPositionLock(float x, float y, QString label, int status, QPainter& painter) -{ - paintText(label, QGC::colorCyan, 2.6f, x, y+0.35f, &painter); - QColor negStatusColor(200, 20, 20); - QColor posStatusColor(20, 200, 20); - QColor statusColor(250, 250, 250); - if(status > 0 && status < 4) - { - painter.setBrush(posStatusColor); - } - else - { - painter.setBrush(negStatusColor); - } + float indicatorWidth = refToScreenX(7.0f); + float indicatorHeight = refToScreenY(4.0f); - // Lock text - QString lockText; - switch (status) + painter.drawRect(QRect(refToScreenX(x+7.3f), refToScreenY(y+0.05), indicatorWidth, indicatorHeight)); + paintText((status) ? tr("ON") : tr("OFF"), statusColor, 2.6f, x+7.9f, y+0.8f, &painter); + // Cross out instrument if state unknown + if (!known) { - case 1: - lockText = tr("LOC"); - break; - case 2: - lockText = tr("2D"); - break; - case 3: - lockText = tr("3D"); - break; - default: - lockText = tr("NO"); - break; + QPen pen(Qt::yellow); + pen.setWidth(2); + painter.setPen(pen); + // Top left to bottom right + QPointF p1, p2, p3, p4; + p1.setX(refToScreenX(x)); + p1.setY(refToScreenX(y)); + p2.setX(p1.x()+indicatorWidth+refToScreenX(7.3f)); + p2.setY(p1.y()+indicatorHeight); + painter.drawLine(p1, p2); + // Bottom left to top right + p3.setX(refToScreenX(x)); + p3.setY(refToScreenX(y)+indicatorHeight); + p4.setX(p1.x()+indicatorWidth+refToScreenX(7.3f)); + p4.setY(p1.y()); + painter.drawLine(p3, p4); } +} - painter.setPen(Qt::NoPen); - painter.drawRect(QRect(refToScreenX(x+7.3f), refToScreenY(y+0.05), refToScreenX(7.0f), refToScreenY(4.0f))); - paintText(lockText, statusColor, 2.6f, x+7.9f, y+0.35f, &painter); +void HSIDisplay::drawPositionLock(float x, float y, QString label, int status, bool known, QPainter& painter) +{ + paintText(label, QGC::colorCyan, 2.6f, x, y+0.8f, &painter); + QColor negStatusColor(200, 20, 20); + QColor intermediateStatusColor (Qt::yellow); + QColor posStatusColor(20, 200, 20); + QColor statusColor(250, 250, 250); + if (status == 3) + { + painter.setBrush(posStatusColor); + } + else if (status == 2) + { + painter.setBrush(intermediateStatusColor.dark(150)); + } + else + { + painter.setBrush(negStatusColor); + } + + // Lock text + QString lockText; + switch (status) + { + case 1: + lockText = tr("LOC"); + break; + case 2: + lockText = tr("2D"); + break; + case 3: + lockText = tr("3D"); + break; + default: + lockText = tr("NO"); + break; + } + + float indicatorWidth = refToScreenX(7.0f); + float indicatorHeight = refToScreenY(4.0f); + + painter.setPen(Qt::NoPen); + painter.drawRect(QRect(refToScreenX(x+7.3f), refToScreenY(y+0.05), refToScreenX(7.0f), refToScreenY(4.0f))); + paintText(lockText, statusColor, 2.6f, x+7.9f, y+0.8f, &painter); + // Cross out instrument if state unknown + if (!known) + { + QPen pen(Qt::yellow); + pen.setWidth(2); + painter.setPen(pen); + // Top left to bottom right + QPointF p1, p2, p3, p4; + p1.setX(refToScreenX(x)); + p1.setY(refToScreenX(y)); + p2.setX(p1.x()+indicatorWidth+refToScreenX(7.3f)); + p2.setY(p1.y()+indicatorHeight); + painter.drawLine(p1, p2); + // Bottom left to top right + p3.setX(refToScreenX(x)); + p3.setY(refToScreenX(y)+indicatorHeight); + p4.setX(p1.x()+indicatorWidth+refToScreenX(7.3f)); + p4.setY(p1.y()); + painter.drawLine(p3, p4); + } } void HSIDisplay::updatePositionLock(UASInterface* uas, bool lock) @@ -335,16 +453,19 @@ void HSIDisplay::updatePositionLock(UASInterface* uas, bool lock) void HSIDisplay::updateAttitudeControllerEnabled(bool enabled) { attControlEnabled = enabled; + attControlKnown = true; } void HSIDisplay::updatePositionXYControllerEnabled(bool enabled) { xyControlEnabled = enabled; + xyControlKnown = true; } void HSIDisplay::updatePositionZControllerEnabled(bool enabled) { zControlEnabled = enabled; + zControlKnown = true; } QPointF HSIDisplay::metricWorldToBody(QPointF world) @@ -481,6 +602,8 @@ void HSIDisplay::setActiveUAS(UASInterface* uas) connect(uas, SIGNAL(irUltraSoundLocalizationChanged(UASInterface*,int)), this, SLOT(updateInfraredUltrasoundLocalization(UASInterface*,int))); this->uas = uas; + + resetMAVState(); } void HSIDisplay::updateSpeed(UASInterface* uas, double vx, double vy, double vz, quint64 time) @@ -550,6 +673,8 @@ void HSIDisplay::updatePositionSetpoints(int uasid, float xDesired, float yDesir bodyZSetCoordinate = zDesired; bodyYawSet = yawDesired; mavInitialized = true; + setPointKnown = true; + positionSetPointKnown = true; // qDebug() << "Received setpoint at x: " << x << "metric y:" << y; // posXSet = xDesired; @@ -592,6 +717,7 @@ void HSIDisplay::updateSatellite(int uasid, int satid, float elevation, float az void HSIDisplay::updatePositionYawControllerEnabled(bool enabled) { yawControlEnabled = enabled; + yawControlKnown = true; } /** @@ -601,6 +727,8 @@ void HSIDisplay::updateLocalization(UASInterface* uas, int fix) { Q_UNUSED(uas); positionFix = fix; + positionFixKnown = true; + //qDebug() << "LOCALIZATION FIX CALLED"; } /** * @param fix 0: lost, 1: at least one satellite, but no GPS fix, 2: 2D localization, 3: 3D localization @@ -609,6 +737,7 @@ void HSIDisplay::updateGpsLocalization(UASInterface* uas, int fix) { Q_UNUSED(uas); gpsFix = fix; + gpsFixKnown = true; } /** * @param fix 0: lost, 1: 2D local position hold, 2: 2D localization, 3: 3D localization @@ -617,6 +746,8 @@ void HSIDisplay::updateVisionLocalization(UASInterface* uas, int fix) { Q_UNUSED(uas); visionFix = fix; + visionFixKnown = true; + //qDebug() << "VISION FIX GOT CALLED"; } /** @@ -626,6 +757,7 @@ void HSIDisplay::updateInfraredUltrasoundLocalization(UASInterface* uas, int fix { Q_UNUSED(uas); iruFix = fix; + iruFixKnown = true; } QColor HSIDisplay::getColorForSNR(float snr) @@ -656,22 +788,25 @@ QColor HSIDisplay::getColorForSNR(float snr) void HSIDisplay::drawSetpointXY(float x, float y, float yaw, const QColor &color, QPainter &painter) { - float radius = vwidth / 20.0f; - QPen pen(color); - pen.setWidthF(refLineWidthToPen(0.4f)); - pen.setColor(color); - painter.setPen(pen); - painter.setBrush(Qt::NoBrush); - QPointF in(x, y); - // Transform from body to world coordinates - in = metricWorldToBody(in); - // Scale from metric to screen reference coordinates - QPointF p = metricBodyToRef(in); - drawCircle(p.x(), p.y(), radius, 0.4f, color, &painter); - radius *= 0.8; - drawLine(p.x(), p.y(), p.x()+sin(yaw) * radius, p.y()-cos(yaw) * radius, refLineWidthToPen(0.4f), color, &painter); - painter.setBrush(color); - drawCircle(p.x(), p.y(), radius * 0.1f, 0.1f, color, &painter); + if (setPointKnown) + { + float radius = vwidth / 20.0f; + QPen pen(color); + pen.setWidthF(refLineWidthToPen(0.4f)); + pen.setColor(color); + painter.setPen(pen); + painter.setBrush(Qt::NoBrush); + QPointF in(x, y); + // Transform from body to world coordinates + in = metricWorldToBody(in); + // Scale from metric to screen reference coordinates + QPointF p = metricBodyToRef(in); + drawCircle(p.x(), p.y(), radius, 0.4f, color, &painter); + radius *= 0.8f; + drawLine(p.x(), p.y(), p.x()+sin(yaw) * radius, p.y()-cos(yaw) * radius, refLineWidthToPen(0.4f), color, &painter); + painter.setBrush(color); + drawCircle(p.x(), p.y(), radius * 0.1f, 0.1f, color, &painter); + } } void HSIDisplay::drawWaypoints(QPainter& painter) @@ -699,7 +834,19 @@ void HSIDisplay::drawWaypoints(QPainter& painter) for (int i = 0; i < list.size(); i++) { - QPointF in(list.at(i)->getX(), list.at(i)->getY()); + QPointF in; + if (list.at(i)->getFrame() == MAV_FRAME_LOCAL) + { + // Do not transform + in = QPointF(list.at(i)->getX(), list.at(i)->getY()); + } + else + { + // Transform to local coordinates first + double x = list.at(i)->getX(); + double y = list.at(i)->getY(); + in = QPointF(x, y); + } // Transform from world to body coordinates in = metricWorldToBody(in); // Scale from metric to screen reference coordinates @@ -835,89 +982,98 @@ void HSIDisplay::drawObjects(QPainter &painter) void HSIDisplay::drawPositionDirection(float xRef, float yRef, float radius, const QColor& color, QPainter* painter) { - // Draw the needle - const float maxWidth = radius / 10.0f; - const float minWidth = maxWidth * 0.3f; + if (xyControlKnown && xyControlEnabled) + { + // Draw the needle + const float maxWidth = radius / 10.0f; + const float minWidth = maxWidth * 0.3f; - float angle = atan2(posXSet, -posYSet); - angle -= M_PI/2.0f; + float angle = atan2(posXSet, -posYSet); + angle -= (float)M_PI/2.0f; - QPolygonF p(6); + QPolygonF p(6); - //radius *= ((posXSaturation + posYSaturation) - sqrt(pow(posXSet, 2), pow(posYSet, 2))) / (2*posXSaturation); + //radius *= ((posXSaturation + posYSaturation) - sqrt(pow(posXSet, 2), pow(posYSet, 2))) / (2*posXSaturation); - radius *= sqrt(pow(posXSet, 2) + pow(posYSet, 2)) / sqrt(posXSaturation + posYSaturation); + radius *= sqrt(pow(posXSet, 2) + pow(posYSet, 2)) / sqrt(posXSaturation + posYSaturation); - p.replace(0, QPointF(xRef-maxWidth/2.0f, yRef-radius * 0.4f)); - p.replace(1, QPointF(xRef-minWidth/2.0f, yRef-radius * 0.9f)); - p.replace(2, QPointF(xRef+minWidth/2.0f, yRef-radius * 0.9f)); - p.replace(3, QPointF(xRef+maxWidth/2.0f, yRef-radius * 0.4f)); - p.replace(4, QPointF(xRef, yRef-radius * 0.36f)); - p.replace(5, QPointF(xRef-maxWidth/2.0f, yRef-radius * 0.4f)); + p.replace(0, QPointF(xRef-maxWidth/2.0f, yRef-radius * 0.4f)); + p.replace(1, QPointF(xRef-minWidth/2.0f, yRef-radius * 0.9f)); + p.replace(2, QPointF(xRef+minWidth/2.0f, yRef-radius * 0.9f)); + p.replace(3, QPointF(xRef+maxWidth/2.0f, yRef-radius * 0.4f)); + p.replace(4, QPointF(xRef, yRef-radius * 0.36f)); + p.replace(5, QPointF(xRef-maxWidth/2.0f, yRef-radius * 0.4f)); - rotatePolygonClockWiseRad(p, angle, QPointF(xRef, yRef)); + rotatePolygonClockWiseRad(p, angle, QPointF(xRef, yRef)); - QBrush indexBrush; - indexBrush.setColor(color); - indexBrush.setStyle(Qt::SolidPattern); - painter->setPen(Qt::SolidLine); - painter->setPen(color); - painter->setBrush(indexBrush); - drawPolygon(p, painter); + QBrush indexBrush; + indexBrush.setColor(color); + indexBrush.setStyle(Qt::SolidPattern); + painter->setPen(Qt::SolidLine); + painter->setPen(color); + painter->setBrush(indexBrush); + drawPolygon(p, painter); - //qDebug() << "DRAWING POS SETPOINT X:" << posXSet << "Y:" << posYSet << angle; + //qDebug() << "DRAWING POS SETPOINT X:" << posXSet << "Y:" << posYSet << angle; + } } void HSIDisplay::drawAttitudeDirection(float xRef, float yRef, float radius, const QColor& color, QPainter* painter) { - // Draw the needle - const float maxWidth = radius / 10.0f; - const float minWidth = maxWidth * 0.3f; + if (attControlKnown && attControlEnabled) + { + // Draw the needle + const float maxWidth = radius / 10.0f; + const float minWidth = maxWidth * 0.3f; - float angle = atan2(attXSet, attYSet); - angle -= M_PI/2.0f; + float angle = atan2(attXSet, attYSet); + angle -= (float)M_PI/2.0f; - radius *= sqrt(pow(attXSet, 2) + pow(attYSet, 2)) / sqrt(attXSaturation + attYSaturation); + radius *= sqrt(pow(attXSet, 2) + pow(attYSet, 2)) / sqrt(attXSaturation + attYSaturation); - QPolygonF p(6); + QPolygonF p(6); - p.replace(0, QPointF(xRef-maxWidth/2.0f, yRef-radius * 0.4f)); - p.replace(1, QPointF(xRef-minWidth/2.0f, yRef-radius * 0.9f)); - p.replace(2, QPointF(xRef+minWidth/2.0f, yRef-radius * 0.9f)); - p.replace(3, QPointF(xRef+maxWidth/2.0f, yRef-radius * 0.4f)); - p.replace(4, QPointF(xRef, yRef-radius * 0.36f)); - p.replace(5, QPointF(xRef-maxWidth/2.0f, yRef-radius * 0.4f)); + p.replace(0, QPointF(xRef-maxWidth/2.0f, yRef-radius * 0.4f)); + p.replace(1, QPointF(xRef-minWidth/2.0f, yRef-radius * 0.9f)); + p.replace(2, QPointF(xRef+minWidth/2.0f, yRef-radius * 0.9f)); + p.replace(3, QPointF(xRef+maxWidth/2.0f, yRef-radius * 0.4f)); + p.replace(4, QPointF(xRef, yRef-radius * 0.36f)); + p.replace(5, QPointF(xRef-maxWidth/2.0f, yRef-radius * 0.4f)); - rotatePolygonClockWiseRad(p, angle, QPointF(xRef, yRef)); + rotatePolygonClockWiseRad(p, angle, QPointF(xRef, yRef)); - QBrush indexBrush; - indexBrush.setColor(color); - indexBrush.setStyle(Qt::SolidPattern); - painter->setPen(Qt::SolidLine); - painter->setPen(color); - painter->setBrush(indexBrush); - drawPolygon(p, painter); + QBrush indexBrush; + indexBrush.setColor(color); + indexBrush.setStyle(Qt::SolidPattern); + painter->setPen(Qt::SolidLine); + painter->setPen(color); + painter->setBrush(indexBrush); + drawPolygon(p, painter); - // TODO Draw Yaw indicator + // TODO Draw Yaw indicator - //qDebug() << "DRAWING ATT SETPOINT X:" << attXSet << "Y:" << attYSet << angle; + //qDebug() << "DRAWING ATT SETPOINT X:" << attXSet << "Y:" << attYSet << angle; + } } void HSIDisplay::drawAltitudeSetpoint(float xRef, float yRef, float radius, const QColor& color, QPainter* painter) { - // Draw the circle - QPen circlePen(Qt::SolidLine); - circlePen.setWidth(refLineWidthToPen(0.5f)); - circlePen.setColor(color); - painter->setBrush(Qt::NoBrush); - painter->setPen(circlePen); - drawCircle(xRef, yRef, radius, 200.0f, color, painter); - //drawCircle(xRef, yRef, radius, 200.0f, 170.0f, 1.0f, color, painter); - - // // Draw the value - // QString label; - // label.sprintf("%05.1f", value); - // paintText(label, color, 4.5f, xRef-7.5f, yRef-2.0f, painter); + if (zControlKnown && zControlEnabled) + { + // Draw the circle + QPen circlePen(Qt::SolidLine); + circlePen.setWidth(refLineWidthToPen(0.5f)); + circlePen.setColor(color); + painter->setBrush(Qt::NoBrush); + painter->setPen(circlePen); + drawCircle(xRef, yRef, radius, 200.0f, color, painter); + //drawCircle(xRef, yRef, radius, 200.0f, 170.0f, 1.0f, color, painter); + + // // Draw the value + // QString label; + // label.sprintf("%05.1f", value); + // paintText(label, color, 4.5f, xRef-7.5f, yRef-2.0f, painter); + } } void HSIDisplay::wheelEvent(QWheelEvent* event) diff --git a/src/ui/HSIDisplay.h b/src/ui/HSIDisplay.h index c70598e41daccd19dfc9f2142ff2afb3179ae76f..7282e1da1792083548824b66b650c1c2b26f98b4 100644 --- a/src/ui/HSIDisplay.h +++ b/src/ui/HSIDisplay.h @@ -81,6 +81,8 @@ public slots: /** @brief Update state from joystick */ void updateJoystick(double roll, double pitch, double yaw, double thrust, int xHat, int yHat); void pressKey(int key); + /** @brief Reset the state of the view */ + void resetMAVState(); signals: void metricWidthChanged(double width); @@ -92,8 +94,10 @@ protected slots: void drawPositionDirection(float xRef, float yRef, float radius, const QColor& color, QPainter* painter); void drawAttitudeDirection(float xRef, float yRef, float radius, const QColor& color, QPainter* painter); void drawAltitudeSetpoint(float xRef, float yRef, float radius, const QColor& color, QPainter* painter); - void drawStatusFlag(float x, float y, QString label, bool status, QPainter& painter); - void drawPositionLock(float x, float y, QString label, int status, QPainter& painter); + /** @brief Draw a status flag indicator */ + void drawStatusFlag(float x, float y, QString label, bool status, bool known, QPainter& painter); + /** @brief Draw a position lock indicator */ + void drawPositionLock(float x, float y, QString label, int status, bool known, QPainter& painter); void setBodySetpointCoordinateXY(double x, double y); void setBodySetpointCoordinateZ(double z); /** @brief Send the current ui setpoint coordinates as new setpoint to the MAV */ @@ -231,6 +235,22 @@ protected: float bottomMargin; ///< Margin on the bottom of the page, in virtual coordinates float topMargin; ///< Margin on top of the page, in virtual coordinates + bool attControlKnown; ///< Attitude control status known flag + bool xyControlKnown; ///< XY control status known flag + bool zControlKnown; ///< Z control status known flag + bool yawControlKnown; ///< Yaw control status known flag + + // Position lock indicators + bool positionFixKnown; ///< Position fix status known flag + bool visionFixKnown; ///< Vision fix status known flag + bool gpsFixKnown; ///< GPS fix status known flag + bool iruFixKnown; ///< Infrared/Ultrasound fix status known flag + + // Data indicators + bool setPointKnown; ///< Controller setpoint known status flag + bool positionSetPointKnown; ///< Position setpoint known status flag + bool userSetPointSet; + private: }; diff --git a/src/ui/HUD.cc b/src/ui/HUD.cc index 11cd757dfa1974a81cb7ff7a28926fc10131cdae..9f8617259a1c5d6ad40343866645c3d25d62036e 100644 --- a/src/ui/HUD.cc +++ b/src/ui/HUD.cc @@ -37,9 +37,7 @@ This file is part of the QGROUNDCONTROL project #include #include -#ifndef M_PI -#define M_PI 3.14159265358979323846 -#endif +#include #include #include "UASManager.h" @@ -126,6 +124,9 @@ HUD::HUD(int width, int height, QWidget* parent) xSpeed(0.0), ySpeed(0.0), zSpeed(0.0), + lastSpeedUpdate(0), + totalSpeed(0.0), + totalAcc(0.0), lat(0.0), lon(0.0), alt(0.0), @@ -277,6 +278,7 @@ void HUD::setActiveUAS(UASInterface* uas) disconnect(this->uas, SIGNAL(heartbeat(UASInterface*)), this, SLOT(receiveHeartbeat(UASInterface*))); disconnect(this->uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64))); + disconnect(this->uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64))); disconnect(this->uas, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64))); disconnect(this->uas, SIGNAL(waypointSelected(int,int)), this, SLOT(selectWaypoint(int, int))); @@ -299,6 +301,7 @@ void HUD::setActiveUAS(UASInterface* uas) connect(uas, SIGNAL(heartbeat(UASInterface*)), this, SLOT(receiveHeartbeat(UASInterface*))); connect(uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64))); + connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64))); connect(uas, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64))); connect(uas, SIGNAL(waypointSelected(int,int)), this, SLOT(selectWaypoint(int, int))); @@ -334,12 +337,7 @@ void HUD::updateAttitude(UASInterface* uas, double roll, double pitch, double ya void HUD::updateBattery(UASInterface* uas, double voltage, double percent, int seconds) { Q_UNUSED(uas); -// this->voltage = voltage; -// this->timeRemaining = seconds; -// this->percentRemaining = percent; - - fuelStatus.sprintf("BAT [%02.0f \%% | %05.2fV] (%02dm:%02ds)", percent, voltage, seconds/60, seconds%60); - + fuelStatus = tr("BAT [%1% | %2V] (%3:%4)").arg(percent, 2, 'f', 0, QChar('0')).arg(voltage, 4, 'f', 1, QChar('0')).arg(seconds/60, 2, 10, QChar('0')).arg(seconds%60, 2, 10, QChar('0')); if (percent < 20.0f) { fuelColor = warningColor; @@ -390,6 +388,9 @@ void HUD::updateSpeed(UASInterface* uas,double x,double y,double z,quint64 times this->xSpeed = x; this->ySpeed = y; this->zSpeed = z; + double newTotalSpeed = sqrt(xSpeed*xSpeed + ySpeed*ySpeed + zSpeed*zSpeed); + totalAcc = (newTotalSpeed - totalSpeed) / ((double)(lastSpeedUpdate - timestamp)/1000.0); + totalSpeed = newTotalSpeed; } /** @@ -647,10 +648,10 @@ void HUD::paintHUD() yawInt += newYawDiff; - if (yawInt > M_PI) yawInt = M_PI; - if (yawInt < -M_PI) yawInt = -M_PI; + if (yawInt > M_PI) yawInt = (float)M_PI; + if (yawInt < -M_PI) yawInt = (float)-M_PI; - float yawTrans = yawInt * (double)maxYawTrans; + float yawTrans = yawInt * (float)maxYawTrans; yawInt *= 0.6f; if ((yawTrans < 5.0) && (yawTrans > -5.0)) yawTrans = 0; @@ -692,6 +693,8 @@ void HUD::paintHUD() nextOfflineImage = ""; } + glRasterPos2i(0, 0); + glPixelZoom(xImageFactor, yImageFactor); // Resize to correct size and fill with image glDrawPixels(glImage.width(), glImage.height(), GL_RGBA, GL_UNSIGNED_BYTE, glImage.bits()); @@ -803,23 +806,35 @@ void HUD::paintHUD() // const float yawDeg = ((values.value("yaw", 0.0f)/M_PI)*180.0f)+180.f; // YAW is in compass-human readable format, so 0 - 360deg. This is normal in aviation, not -180 - +180. - const float yawDeg = ((yawLP/M_PI)*180.0f)+180.0f; - yawAngle.sprintf("%03d", (int)yawDeg); - paintText(yawAngle, defaultColor, 3.5f, -3.7f, compassY+ 0.9f, &painter); + const float yawDeg = ((yawLP/M_PI)*180.0f)+180.0f+180.0f; + int yawCompass = static_cast(yawDeg) % 360; + yawAngle.sprintf("%03d", yawCompass); + paintText(yawAngle, defaultColor, 3.5f, -4.3f, compassY+ 0.97f, &painter); // CHANGE RATE STRIPS drawChangeRateStrip(-51.0f, -50.0f, 15.0f, -1.0f, 1.0f, -zSpeed, &painter); // CHANGE RATE STRIPS - drawChangeRateStrip(49.0f, -50.0f, 15.0f, -1.0f, 1.0f, xSpeed, &painter); + drawChangeRateStrip(49.0f, -50.0f, 15.0f, -1.0f, 1.0f, totalAcc, &painter); // GAUGES // Left altitude gauge - drawChangeIndicatorGauge(-vGaugeSpacing, -15.0f, 10.0f, 2.0f, -zPos, defaultColor, &painter, false); + float gaugeAltitude; + + if (this->alt != 0) + { + gaugeAltitude = alt; + } + else + { + gaugeAltitude = -zPos; + } + + drawChangeIndicatorGauge(-vGaugeSpacing, -15.0f, 10.0f, 2.0f, gaugeAltitude, defaultColor, &painter, false); // Right speed gauge - drawChangeIndicatorGauge(vGaugeSpacing, -15.0f, 10.0f, 5.0f, xSpeed, defaultColor, &painter, false); + drawChangeIndicatorGauge(vGaugeSpacing, -15.0f, 10.0f, 5.0f, totalSpeed, defaultColor, &painter, false); // Waypoint name @@ -833,7 +848,7 @@ void HUD::paintHUD() // Rotate view and draw all roll-dependent indicators painter.rotate((rollLP/M_PI)* -180.0f); - painter.translate(0, (-pitchLP/M_PI)* -180.0f * refToScreenY(1.8)); + painter.translate(0, (-pitchLP/(float)M_PI)* -180.0f * refToScreenY(1.8f)); //qDebug() << "ROLL" << roll << "PITCH" << pitch << "YAW DIFF" << valuesDot.value("roll", 0.0f); @@ -1294,7 +1309,7 @@ void HUD::drawChangeRateStrip(float xRef, float yRef, float height, float minRat // Text QString label; - label.sprintf("< %06.2f", value); + label.sprintf("< %+06.2f", value); paintText(label, defaultColor, 3.0f, xRef+width/2.0f, yRef+height-((scaledValue - minRate)/(maxRate-minRate))*height - 1.6f, painter); } diff --git a/src/ui/HUD.h b/src/ui/HUD.h index 8866f784abd4ee73692e34aefa922999831cb7c0..b476fac5dc45d076f79bbc9925b9ded1d22e4fd9 100644 --- a/src/ui/HUD.h +++ b/src/ui/HUD.h @@ -130,7 +130,7 @@ protected: void contextMenuEvent (QContextMenuEvent* event); void createActions(); - static const int updateInterval = 50; + static const int updateInterval = 40; QImage* image; ///< Double buffer image QImage glImage; ///< The background / camera image @@ -198,6 +198,9 @@ protected: double xSpeed; double ySpeed; double zSpeed; + quint64 lastSpeedUpdate; + double totalSpeed; + double totalAcc; double lat; double lon; double alt; diff --git a/src/ui/MAVLinkSettingsWidget.cc b/src/ui/MAVLinkSettingsWidget.cc index 7cb49ec6d639341bb0d004ae507de37c5d982c27..ae2fdceb51b48eb0eedaba426c7df5bbe8a4e681 100644 --- a/src/ui/MAVLinkSettingsWidget.cc +++ b/src/ui/MAVLinkSettingsWidget.cc @@ -48,15 +48,47 @@ MAVLinkSettingsWidget::MAVLinkSettingsWidget(MAVLinkProtocol* protocol, QWidget m_ui->heartbeatCheckBox->setChecked(protocol->heartbeatsEnabled()); m_ui->loggingCheckBox->setChecked(protocol->loggingEnabled()); m_ui->versionCheckBox->setChecked(protocol->versionCheckEnabled()); + m_ui->multiplexingCheckBox->setChecked(protocol->multiplexingEnabled()); + m_ui->systemIdSpinBox->setValue(protocol->getSystemId()); + + m_ui->paramGuardCheckBox->setChecked(protocol->paramGuardEnabled()); + m_ui->paramRetransmissionSpinBox->setValue(protocol->getParamRetransmissionTimeout()); + m_ui->paramRewriteSpinBox->setValue(protocol->getParamRewriteTimeout()); + + m_ui->actionGuardCheckBox->setChecked(protocol->actionGuardEnabled()); + m_ui->actionRetransmissionSpinBox->setValue(protocol->getActionRetransmissionTimeout()); // Connect actions + // Heartbeat connect(protocol, SIGNAL(heartbeatChanged(bool)), m_ui->heartbeatCheckBox, SLOT(setChecked(bool))); connect(m_ui->heartbeatCheckBox, SIGNAL(toggled(bool)), protocol, SLOT(enableHeartbeats(bool))); + // Logging connect(protocol, SIGNAL(loggingChanged(bool)), m_ui->loggingCheckBox, SLOT(setChecked(bool))); connect(m_ui->loggingCheckBox, SIGNAL(toggled(bool)), protocol, SLOT(enableLogging(bool))); + // Version check connect(protocol, SIGNAL(versionCheckChanged(bool)), m_ui->versionCheckBox, SLOT(setChecked(bool))); connect(m_ui->versionCheckBox, SIGNAL(toggled(bool)), protocol, SLOT(enableVersionCheck(bool))); + // Logfile connect(m_ui->logFileButton, SIGNAL(clicked()), this, SLOT(chooseLogfileName())); + // System ID + connect(protocol, SIGNAL(systemIdChanged(int)), m_ui->systemIdSpinBox, SLOT(setValue(int))); + connect(m_ui->systemIdSpinBox, SIGNAL(valueChanged(int)), protocol, SLOT(setSystemId(int))); + // Multiplexing + connect(protocol, SIGNAL(multiplexingChanged(bool)), m_ui->multiplexingCheckBox, SLOT(setChecked(bool))); + connect(m_ui->multiplexingCheckBox, SIGNAL(toggled(bool)), protocol, SLOT(enableMultiplexing(bool))); + // Parameter guard + connect(protocol, SIGNAL(paramGuardChanged(bool)), m_ui->paramGuardCheckBox, SLOT(setChecked(bool))); + connect(m_ui->paramGuardCheckBox, SIGNAL(toggled(bool)), protocol, SLOT(enableParamGuard(bool))); + connect(protocol, SIGNAL(paramRetransmissionTimeoutChanged(int)), m_ui->paramRetransmissionSpinBox, SLOT(setValue(int))); + connect(m_ui->paramRetransmissionSpinBox, SIGNAL(valueChanged(int)), protocol, SLOT(setParamRetransmissionTimeout(int))); + connect(protocol, SIGNAL(paramRewriteTimeoutChanged(int)), m_ui->paramRewriteSpinBox, SLOT(setValue(int))); + connect(m_ui->paramRewriteSpinBox, SIGNAL(valueChanged(int)), protocol, SLOT(setParamRewriteTimeout(int))); + // Action guard + connect(protocol, SIGNAL(actionGuardChanged(bool)), m_ui->actionGuardCheckBox, SLOT(setChecked(bool))); + connect(m_ui->actionGuardCheckBox, SIGNAL(toggled(bool)), protocol, SLOT(enableActionGuard(bool))); + connect(protocol, SIGNAL(actionRetransmissionTimeoutChanged(int)), m_ui->actionRetransmissionSpinBox, SLOT(setValue(int))); + connect(m_ui->actionRetransmissionSpinBox, SIGNAL(valueChanged(int)), protocol, SLOT(setActionRetransmissionTimeout(int))); + // Update values m_ui->versionLabel->setText(tr("MAVLINK_VERSION: %1").arg(protocol->getVersion())); @@ -65,17 +97,40 @@ MAVLinkSettingsWidget::MAVLinkSettingsWidget(MAVLinkProtocol* protocol, QWidget // Connect visibility updates connect(protocol, SIGNAL(versionCheckChanged(bool)), m_ui->versionLabel, SLOT(setVisible(bool))); m_ui->versionLabel->setVisible(protocol->versionCheckEnabled()); - //connect(m_ui->versionCheckBox, SIGNAL(toggled(bool)), m_ui->versionSpacer, SLOT(setVisible(bool))); - //connect(m_ui->loggingCheckBox, SIGNAL(toggled(bool)), m_ui->logFileSpacer, SLOT(setVisible(bool))); + // Logging visibility connect(protocol, SIGNAL(loggingChanged(bool)), m_ui->logFileLabel, SLOT(setVisible(bool))); m_ui->logFileLabel->setVisible(protocol->loggingEnabled()); connect(protocol, SIGNAL(loggingChanged(bool)), m_ui->logFileButton, SLOT(setVisible(bool))); m_ui->logFileButton->setVisible(protocol->loggingEnabled()); - - // Update settings - m_ui->loggingCheckBox->setChecked(protocol->loggingEnabled()); - m_ui->heartbeatCheckBox->setChecked(protocol->heartbeatsEnabled()); - m_ui->versionCheckBox->setChecked(protocol->versionCheckEnabled()); +// // Multiplexing visibility +// connect(protocol, SIGNAL(multiplexingChanged(bool)), m_ui->multiplexingFilterCheckBox, SLOT(setVisible(bool))); +// m_ui->multiplexingFilterCheckBox->setVisible(protocol->multiplexingEnabled()); +// connect(protocol, SIGNAL(multiplexingChanged(bool)), m_ui->multiplexingFilterLineEdit, SLOT(setVisible(bool))); +// m_ui->multiplexingFilterLineEdit->setVisible(protocol->multiplexingEnabled()); + // Param guard visibility + connect(protocol, SIGNAL(paramGuardChanged(bool)), m_ui->paramRetransmissionSpinBox, SLOT(setVisible(bool))); + m_ui->paramRetransmissionSpinBox->setVisible(protocol->paramGuardEnabled()); + connect(protocol, SIGNAL(paramGuardChanged(bool)), m_ui->paramRetransmissionLabel, SLOT(setVisible(bool))); + m_ui->paramRetransmissionLabel->setVisible(protocol->paramGuardEnabled()); + connect(protocol, SIGNAL(paramGuardChanged(bool)), m_ui->paramRewriteSpinBox, SLOT(setVisible(bool))); + m_ui->paramRewriteSpinBox->setVisible(protocol->paramGuardEnabled()); + connect(protocol, SIGNAL(paramGuardChanged(bool)), m_ui->paramRewriteLabel, SLOT(setVisible(bool))); + m_ui->paramRewriteLabel->setVisible(protocol->paramGuardEnabled()); + // Action guard visibility + connect(protocol, SIGNAL(actionGuardChanged(bool)), m_ui->actionRetransmissionSpinBox, SLOT(setVisible(bool))); + m_ui->actionRetransmissionSpinBox->setVisible(protocol->actionGuardEnabled()); + connect(protocol, SIGNAL(actionGuardChanged(bool)), m_ui->actionRetransmissionLabel, SLOT(setVisible(bool))); + m_ui->actionRetransmissionLabel->setVisible(protocol->actionGuardEnabled()); + + // TODO implement filtering + // and then remove these two lines + m_ui->multiplexingFilterCheckBox->setVisible(false); + m_ui->multiplexingFilterLineEdit->setVisible(false); + +// // Update settings +// m_ui->loggingCheckBox->setChecked(protocol->loggingEnabled()); +// m_ui->heartbeatCheckBox->setChecked(protocol->heartbeatsEnabled()); +// m_ui->versionCheckBox->setChecked(protocol->versionCheckEnabled()); } void MAVLinkSettingsWidget::updateLogfileName(const QString& fileName) @@ -119,7 +174,8 @@ MAVLinkSettingsWidget::~MAVLinkSettingsWidget() void MAVLinkSettingsWidget::changeEvent(QEvent *e) { QWidget::changeEvent(e); - switch (e->type()) { + switch (e->type()) + { case QEvent::LanguageChange: m_ui->retranslateUi(this); break; @@ -127,3 +183,9 @@ void MAVLinkSettingsWidget::changeEvent(QEvent *e) break; } } + +void MAVLinkSettingsWidget::hideEvent(QHideEvent* event) +{ + Q_UNUSED(event); + protocol->storeSettings(); +} diff --git a/src/ui/MAVLinkSettingsWidget.h b/src/ui/MAVLinkSettingsWidget.h index c6f28c7b913abd76dd5a5bc44c2244282591d3e3..c1d99eb72ac2f0cfdc648121c39ef9eb97818c1f 100644 --- a/src/ui/MAVLinkSettingsWidget.h +++ b/src/ui/MAVLinkSettingsWidget.h @@ -24,6 +24,7 @@ public slots: protected: MAVLinkProtocol* protocol; void changeEvent(QEvent *e); + void hideEvent(QHideEvent* event); private: Ui::MAVLinkSettingsWidget *m_ui; diff --git a/src/ui/MAVLinkSettingsWidget.ui b/src/ui/MAVLinkSettingsWidget.ui index 5f96e685ae3652ee9413b42bddcc58f8d3e84ed7..c3e023b782fbe992b23c9f179cf0343e418f35ce 100644 --- a/src/ui/MAVLinkSettingsWidget.ui +++ b/src/ui/MAVLinkSettingsWidget.ui @@ -6,29 +6,29 @@ 0 0 - 361 - 145 + 431 + 387 Form - + Emit heartbeat - + Log all MAVLink packets - + Qt::Horizontal @@ -44,14 +44,14 @@ - + Only accept MAVs with same protocol version - + Qt::Horizontal @@ -64,27 +64,230 @@ - + MAVLINK_VERSION: - + Logfile name - + Select.. + + + + Set the groundstation number + + + Set the groundstation number + + + 1 + + + 255 + + + + + + + The system ID is the number the MAV associates with this computer + + + The system ID is the number the MAV associates with this computer + + + Groundstation MAVLink System ID + + + + + + + Qt::Horizontal + + + QSizePolicy::MinimumExpanding + + + + 8 + 0 + + + + + + + + Enter a comma-separated list of allowed packets + + + + + + + Filter multiplexed packets: Only forward selected IDs + + + + + + + Enable Multiplexing: Forward packets to all other links + + + + + + + Enable retransmission of parameter read/write requests + + + + + + + Qt::Horizontal + + + + 8 + 0 + + + + + + + + Qt::Horizontal + + + + 8 + 0 + + + + + + + + Read request retransmission timeout + + + + + + + Write request retransmission timeout + + + + + + + Time in milliseconds after which a not acknowledged read request is sent again. + + + Time in milliseconds after which a not acknowledged read request is sent again. + + + ms + + + 50 + + + 60000 + + + 50 + + + 50 + + + + + + + Time in milliseconds after which a not acknowledged write request is sent again. + + + Time in milliseconds after which a not acknowledged write request is sent again. + + + ms + + + 50 + + + 60000 + + + 50 + + + + + + + Enable retransmission of actions / commands + + + + + + + Qt::Horizontal + + + + 8 + 0 + + + + + + + + Action request retransmission timeout + + + + + + + ms + + + 20 + + + 1000 + + + 10 + + + diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index 9e21b9d926da0c4e1dc87689f097536977063c74..7f10e8094b240d4f286b1610d93daed95b0c1246 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -28,6 +28,7 @@ #include "GAudioOutput.h" #include "QGCToolWidget.h" #include "QGCMAVLinkLogPlayer.h" +#include "QGCSettingsWidget.h" #ifdef QGC_OSG_ENABLED #include "Q3DWidgetFactory.h" @@ -66,8 +67,12 @@ MainWindow::MainWindow(QWidget *parent): toolsMenuActions(), currentView(VIEW_UNCONNECTED), aboutToCloseFlag(false), - changingViewsFlag(false) + changingViewsFlag(false), + styleFileName(QCoreApplication::applicationDirPath() + "/style-indoor.css"), + autoReconnect(false), + currentStyle(QGC_MAINWINDOW_STYLE_INDOOR) { + loadSettings(); if (!settings.contains("CURRENT_VIEW")) { // Set this view as default view @@ -102,18 +107,20 @@ MainWindow::MainWindow(QWidget *parent): configureWindowName(); - // Set the application style (not the same as a style sheet) - // Set the style to Plastique - qApp->setStyle("plastique"); + loadStyle(currentStyle); - // Set style sheet as last step - QFile* styleSheet = new QFile(":/images/style-mission.css"); - if (styleSheet->open(QIODevice::ReadOnly | QIODevice::Text)) - { - QString style = QString(styleSheet->readAll()); - style.replace("ICONDIR", QCoreApplication::applicationDirPath()+ "/images/"); - qApp->setStyleSheet(style); - } +// // Set the application style (not the same as a style sheet) +// // Set the style to Plastique +// qApp->setStyle("plastique"); + +// // Set style sheet as last step +// QFile* styleSheet = new QFile(":/images/style-mission.css"); +// if (styleSheet->open(QIODevice::ReadOnly | QIODevice::Text)) +// { +// QString style = QString(styleSheet->readAll()); +// style.replace("ICONDIR", QCoreApplication::applicationDirPath()+ "/images/"); +// qApp->setStyleSheet(style); +// } // Create actions connectCommonActions(); @@ -155,11 +162,43 @@ MainWindow::MainWindow(QWidget *parent): // Enable and update view presentView(); + + // Connect link + if (autoReconnect) + { + SerialLink* link = new SerialLink(); + // Add to registry + LinkManager::instance()->add(link); + LinkManager::instance()->addProtocol(link, mavlink); + link->connect(); + } } MainWindow::~MainWindow() { + // Store settings + storeSettings(); + + delete mavlink; + delete joystick; + // Get and delete all dockwidgets and contained + // widgets + QObjectList childList( this->children() ); + + QObjectList::iterator i; + QDockWidget* dockWidget; + for (i = childList.begin(); i != childList.end(); ++i) + { + dockWidget = dynamic_cast(*i); + if (dockWidget) + { + // Remove dock widget from main window + removeDockWidget(dockWidget); + delete dockWidget->widget(); + delete dockWidget; + } + } } /** @@ -191,6 +230,8 @@ void MainWindow::setDefaultSettingsForAp() settings.setValue(buildMenuKey(SUB_SECTION_CHECKED,MainWindow::MENU_UAS_LIST,VIEW_OPERATOR), true); // ENABLE HUD TOOL WIDGET settings.setValue(buildMenuKey(SUB_SECTION_CHECKED,MainWindow::MENU_HUD,VIEW_OPERATOR), true); + // ENABLE WAYPOINTS + settings.setValue(buildMenuKey(SUB_SECTION_CHECKED,MainWindow::MENU_WAYPOINTS,VIEW_OPERATOR), true); } // ENGINEER VIEW DEFAULT @@ -234,12 +275,13 @@ void MainWindow::resizeEvent(QResizeEvent * event) QString MainWindow::getWindowStateKey() { - return QString::number(currentView)+"/windowstate"; + return QString::number(currentView)+"_windowstate"; } QString MainWindow::getWindowGeometryKey() { - return QString::number(currentView)+"/geometry"; + //return QString::number(currentView)+"_geometry"; + return "_geometry"; } void MainWindow::buildCustomWidget() @@ -260,7 +302,6 @@ void MainWindow::buildCustomWidget() for(int i = 0; i < widgets.size(); ++i) { - qDebug() << "ADDING WIDGET #" << i << widgets.at(i); // Check if this widget already has a parent, do not create it in this case QDockWidget* dock = dynamic_cast(widgets.at(i)->parentWidget()); if (!dock) @@ -304,10 +345,10 @@ void MainWindow::buildCommonWidgets() if (!waypointsDockWidget) { - waypointsDockWidget = new QDockWidget(tr("Waypoint List"), this); + waypointsDockWidget = new QDockWidget(tr("Mission Plan"), this); waypointsDockWidget->setWidget( new QGCWaypointListMulti(this) ); waypointsDockWidget->setObjectName("WAYPOINT_LIST_DOCKWIDGET"); - addToToolsMenu (waypointsDockWidget, tr("Waypoints List"), SLOT(showToolWidget(bool)), MENU_WAYPOINTS, Qt::BottomDockWidgetArea); + addToToolsMenu (waypointsDockWidget, tr("Mission Plan"), SLOT(showToolWidget(bool)), MENU_WAYPOINTS, Qt::BottomDockWidgetArea); } if (!infoDockWidget) @@ -350,7 +391,7 @@ void MainWindow::buildCommonWidgets() if (!dataplotWidget) { dataplotWidget = new QGCDataPlot2D(this); - addToCentralWidgetsMenu (dataplotWidget, "Data Plot", SLOT(showCentralWidget()),CENTRAL_DATA_PLOT); + addToCentralWidgetsMenu (dataplotWidget, "Logfile Plot", SLOT(showCentralWidget()),CENTRAL_DATA_PLOT); } } @@ -359,45 +400,58 @@ void MainWindow::buildPxWidgets() { //FIXME: memory of acceptList will never be freed again QStringList* acceptList = new QStringList(); - acceptList->append("-105,roll,deg,+105,s"); - acceptList->append("-105,pitch,deg,+105,s"); - acceptList->append("-105,yaw,deg,+105,s"); - - acceptList->append("-260,rollspeed,deg/s,+260,s"); - acceptList->append("-260,pitchspeed,deg/s,+260,s"); - acceptList->append("-260,yawspeed,deg/s,+260,s"); + acceptList->append("-105,roll deg,deg,+105,s"); + acceptList->append("-105,pitch deg,deg,+105,s"); + acceptList->append("-105,heading deg,deg,+105,s"); + + acceptList->append("-60,rollspeed d/s,deg/s,+60,s"); + acceptList->append("-60,pitchspeed d/s,deg/s,+60,s"); + acceptList->append("-60,yawspeed d/s,deg/s,+60,s"); + acceptList->append("0,airspeed,m/s,30"); + acceptList->append("0,groundspeed,m/s,30"); + acceptList->append("0,climbrate,m/s,30"); + acceptList->append("0,throttle,%,100"); //FIXME: memory of acceptList2 will never be freed again QStringList* acceptList2 = new QStringList(); + acceptList2->append("900,servo #1,us,2100,s"); + acceptList2->append("900,servo #2,us,2100,s"); + acceptList2->append("900,servo #3,us,2100,s"); + acceptList2->append("900,servo #4,us,2100,s"); + acceptList2->append("900,servo #5,us,2100,s"); + acceptList2->append("900,servo #6,us,2100,s"); + acceptList2->append("900,servo #7,us,2100,s"); + acceptList2->append("900,servo #8,us,2100,s"); acceptList2->append("0,abs pressure,hPa,65500"); - acceptList2->append("-999,accel. X,raw,999,s"); - acceptList2->append("-999,accel. Y,raw,999,s"); + //acceptList2->append("-2048,accel. x,raw,2048,s"); + //acceptList2->append("-2048,accel. y,raw,2048,s"); + //acceptList2->append("-2048,accel. z,raw,2048,s"); if (!linechartWidget) { // Center widgets linechartWidget = new Linecharts(this); - addToCentralWidgetsMenu(linechartWidget, "Line Plots", SLOT(showCentralWidget()), CENTRAL_LINECHART); + addToCentralWidgetsMenu(linechartWidget, tr("Realtime Plot"), SLOT(showCentralWidget()), CENTRAL_LINECHART); } if (!hudWidget) { hudWidget = new HUD(320, 240, this); - addToCentralWidgetsMenu(hudWidget, "HUD", SLOT(showCentralWidget()), CENTRAL_HUD); + addToCentralWidgetsMenu(hudWidget, tr("Head Up Display"), SLOT(showCentralWidget()), CENTRAL_HUD); } if (!dataplotWidget) { dataplotWidget = new QGCDataPlot2D(this); - addToCentralWidgetsMenu(dataplotWidget, "Data Plots", SLOT(showCentralWidget()), CENTRAL_DATA_PLOT); + addToCentralWidgetsMenu(dataplotWidget, "Logfile Plot", SLOT(showCentralWidget()), CENTRAL_DATA_PLOT); } #ifdef QGC_OSG_ENABLED if (!_3DWidget) { _3DWidget = Q3DWidgetFactory::get("PIXHAWK"); - addToCentralWidgetsMenu(_3DWidget, "Local 3D", SLOT(showCentralWidget()), CENTRAL_3D_LOCAL); + addToCentralWidgetsMenu(_3DWidget, tr("Local 3D"), SLOT(showCentralWidget()), CENTRAL_3D_LOCAL); } #endif @@ -405,7 +459,7 @@ void MainWindow::buildPxWidgets() if (!_3DMapWidget) { _3DMapWidget = Q3DWidgetFactory::get("MAP3D"); - addToCentralWidgetsMenu(_3DMapWidget, "OSG Earth 3D", SLOT(showCentralWidget()), CENTRAL_OSGEARTH); + addToCentralWidgetsMenu(_3DMapWidget, tr("OSG Earth 3D"), SLOT(showCentralWidget()), CENTRAL_OSGEARTH); } #endif @@ -413,7 +467,7 @@ void MainWindow::buildPxWidgets() if (!gEarthWidget) { gEarthWidget = new QGCGoogleEarthView(this); - addToCentralWidgetsMenu(gEarthWidget, "Google Earth", SLOT(showCentralWidget()), CENTRAL_GOOGLE_EARTH); + addToCentralWidgetsMenu(gEarthWidget, tr("Google Earth"), SLOT(showCentralWidget()), CENTRAL_GOOGLE_EARTH); } #endif @@ -449,7 +503,7 @@ void MainWindow::buildPxWidgets() hsiDockWidget = new QDockWidget(tr("Horizontal Situation Indicator"), this); hsiDockWidget->setWidget( new HSIDisplay(this) ); hsiDockWidget->setObjectName("HORIZONTAL_SITUATION_INDICATOR_DOCK_WIDGET"); - addToToolsMenu (hsiDockWidget, tr("HSI"), SLOT(showToolWidget(bool)), MENU_HSI, Qt::BottomDockWidgetArea); + addToToolsMenu (hsiDockWidget, tr("Horizontal Situation"), SLOT(showToolWidget(bool)), MENU_HSI, Qt::BottomDockWidgetArea); } if (!headDown1DockWidget) @@ -462,10 +516,10 @@ void MainWindow::buildPxWidgets() if (!headDown2DockWidget) { - headDown2DockWidget = new QDockWidget(tr("Payload Status"), this); - headDown2DockWidget->setWidget( new HDDisplay(acceptList2, "Payload Status", this) ); + headDown2DockWidget = new QDockWidget(tr("Actuator Status"), this); + headDown2DockWidget->setWidget( new HDDisplay(acceptList2, "Actuator Status", this) ); headDown2DockWidget->setObjectName("HEAD_DOWN_DISPLAY_2_DOCK_WIDGET"); - addToToolsMenu (headDown2DockWidget, tr("Payload Status"), SLOT(showToolWidget(bool)), MENU_HDD_2, Qt::RightDockWidgetArea); + addToToolsMenu (headDown2DockWidget, tr("Actuator Status"), SLOT(showToolWidget(bool)), MENU_HDD_2, Qt::RightDockWidgetArea); } if (!rcViewDockWidget) @@ -518,6 +572,7 @@ void MainWindow::buildSlugsWidgets() { // Center widgets linechartWidget = new Linecharts(this); + addToCentralWidgetsMenu(linechartWidget, tr("Realtime Plot"), SLOT(showCentralWidget()), CENTRAL_LINECHART); } if (!headUpDockWidget) @@ -526,7 +581,7 @@ void MainWindow::buildSlugsWidgets() headUpDockWidget = new QDockWidget(tr("Control Indicator"), this); headUpDockWidget->setWidget( new HUD(320, 240, this)); headUpDockWidget->setObjectName("HEAD_UP_DISPLAY_DOCK_WIDGET"); - addToToolsMenu (headUpDockWidget, tr("HUD"), SLOT(showToolWidget(bool)), MENU_HUD, Qt::LeftDockWidgetArea); + addToToolsMenu (headUpDockWidget, tr("Head Up Display"), SLOT(showToolWidget(bool)), MENU_HUD, Qt::LeftDockWidgetArea); } if (!rcViewDockWidget) @@ -753,20 +808,16 @@ void MainWindow::showToolWidget(bool visible) QDockWidget* dockWidget = qobject_cast (dockWidgets[tool]); - qDebug() << "DATA:" << tool << "FLOATING" << dockWidget->isFloating() << "checked" << action->isChecked() << "visible" << dockWidget->isVisible() << "action vis:" << action->isVisible(); if (dockWidget && dockWidget->isVisible() != visible) { if (visible) { - qDebug() << "DOCK WIDGET ADDED"; addDockWidget(dockWidgetLocations[tool], dockWidget); dockWidget->show(); } else { - qDebug() << "DOCK WIDGET REMOVED"; removeDockWidget(dockWidget); - //dockWidget->hide(); } QHashIterator i(dockWidgets); @@ -777,7 +828,7 @@ void MainWindow::showToolWidget(bool visible) { QString chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(i.key()), currentView); settings.setValue(chKey,visible); - qDebug() << "showToolWidget(): Set key" << chKey << "to" << visible; + //qDebug() << "showToolWidget(): Set key" << chKey << "to" << visible; break; } } @@ -786,14 +837,14 @@ void MainWindow::showToolWidget(bool visible) QDockWidget* dockWidget = qobject_cast(QObject::sender()); - qDebug() << "Trying to cast dockwidget" << dockWidget << "isvisible" << visible; + //qDebug() << "Trying to cast dockwidget" << dockWidget << "isvisible" << visible; if (dockWidget) { // Get action int tool = dockWidgets.key(dockWidget); - qDebug() << "Updating widget setting" << tool << "to" << visible; + //qDebug() << "Updating widget setting" << tool << "to" << visible; QAction* action = toolsMenuActions[tool]; action->blockSignals(true); @@ -808,7 +859,7 @@ void MainWindow::showToolWidget(bool visible) { QString chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(i.key()), currentView); settings.setValue(chKey,visible); - qDebug() << "showToolWidget(): Set key" << chKey << "to" << visible; + // qDebug() << "showToolWidget(): Set key" << chKey << "to" << visible; break; } } @@ -825,7 +876,7 @@ void MainWindow::showTheWidget (TOOLS_WIDGET_NAMES widget, VIEW_SECTIONS view) tempVisible = settings.value(buildMenuKey(SUB_SECTION_CHECKED,widget,view), false).toBool(); - qDebug() << "showTheWidget(): Set key" << buildMenuKey(SUB_SECTION_CHECKED,widget,view) << "to" << tempVisible; + //qDebug() << "showTheWidget(): Set key" << buildMenuKey(SUB_SECTION_CHECKED,widget,view) << "to" << tempVisible; if (tempWidget) { @@ -837,14 +888,6 @@ void MainWindow::showTheWidget (TOOLS_WIDGET_NAMES widget, VIEW_SECTIONS view) tempLocation = static_cast (settings.value(buildMenuKey (SUB_SECTION_LOCATION,widget, view), QVariant(Qt::RightDockWidgetArea)).toInt()); - // if (widget == MainWindow::MENU_UAS_LIST) - // { - // if (!settings.contains(buildMenuKey (SUB_SECTION_LOCATION,widget, view))) - // { - // tempLocation = Qt::RightDockWidgetArea; - // } - // } - if (tempWidget != NULL) { if (tempVisible) @@ -875,16 +918,9 @@ QString MainWindow::buildMenuKey(SETTINGS_SECTIONS section, TOOLS_WIDGET_NAMES t void MainWindow::closeEvent(QCloseEvent *event) { - settings.setValue(getWindowGeometryKey(), saveGeometry()); - //settings.setValue("windowState", saveState()); + storeSettings(); aboutToCloseFlag = true; - // Save the last current view in any case - settings.setValue("CURRENT_VIEW", currentView); - // Save the current window state, but only if a system is connected (else no real number of widgets would be present) - if (UASManager::instance()->getActiveUAS()) settings.setValue(getWindowStateKey(), saveState(QGC::applicationVersion())); - // Save the current view only if a UAS is connected - if (UASManager::instance()->getUASList().length() > 0) settings.setValue("CURRENT_VIEW_WITH_UAS_CONNECTED", currentView); - settings.sync(); + mavlink->storeSettings(); QMainWindow::closeEvent(event); } @@ -978,7 +1014,6 @@ void MainWindow::connectCommonWidgets() void MainWindow::createCustomWidget() { - //qDebug() << "ADDING CUSTOM WIDGET"; QGCToolWidget* tool = new QGCToolWidget("Unnamed Tool", this); if (QGCToolWidget::instances()->size() < 2) @@ -1070,6 +1105,32 @@ void MainWindow::arrangeSlugsCenterStack() } +void MainWindow::loadSettings() +{ + QSettings settings; + settings.beginGroup("QGC_MAINWINDOW"); + autoReconnect = settings.value("AUTO_RECONNECT", autoReconnect).toBool(); + currentStyle = (QGC_MAINWINDOW_STYLE)settings.value("CURRENT_STYLE", currentStyle).toInt(); + settings.endGroup(); +} + +void MainWindow::storeSettings() +{ + QSettings settings; + settings.beginGroup("QGC_MAINWINDOW"); + settings.setValue("AUTO_RECONNECT", autoReconnect); + settings.setValue("CURRENT_STYLE", currentStyle); + settings.endGroup(); + settings.setValue(getWindowGeometryKey(), saveGeometry()); + // Save the last current view in any case + settings.setValue("CURRENT_VIEW", currentView); + // Save the current window state, but only if a system is connected (else no real number of widgets would be present) + if (UASManager::instance()->getUASList().length() > 0) settings.setValue(getWindowStateKey(), saveState(QGC::applicationVersion())); + // Save the current view only if a UAS is connected + if (UASManager::instance()->getUASList().length() > 0) settings.setValue("CURRENT_VIEW_WITH_UAS_CONNECTED", currentView); + settings.sync(); +} + void MainWindow::configureWindowName() { QList hostAddresses = QNetworkInterface::allAddresses(); @@ -1133,20 +1194,63 @@ void MainWindow::saveScreen() } } -/** - * Reload the style sheet from disk. The function tries to load "qgroundcontrol.css" from the application - * directory (which by default does not exist). If it fails, it will load the bundled default CSS - * from memory. - * To customize the application, just create a qgroundcontrol.css file in the application directory - */ -void MainWindow::reloadStylesheet() +void MainWindow::enableAutoReconnect(bool enabled) { - QString fileName = QCoreApplication::applicationDirPath() + "/style-indoor.css"; + autoReconnect = enabled; +} +void MainWindow::loadNativeStyle() +{ + loadStyle(QGC_MAINWINDOW_STYLE_NATIVE); +} + +void MainWindow::loadIndoorStyle() +{ + loadStyle(QGC_MAINWINDOW_STYLE_INDOOR); +} + +void MainWindow::loadOutdoorStyle() +{ + loadStyle(QGC_MAINWINDOW_STYLE_OUTDOOR); +} + +void MainWindow::loadStyle(QGC_MAINWINDOW_STYLE style) +{ + switch (style) + { + case QGC_MAINWINDOW_STYLE_NATIVE: + { + // Native mode means setting no style + // so if we were already in native mode + // take no action + // Only if a style was set, remove it. + if (style != currentStyle) + { + qApp->setStyleSheet(""); + showInfoMessage(tr("Please restart QGroundControl"), tr("Please restart QGroundControl to switch to fully native look and feel. Currently you have loaded Qt's plastique style.")); + } + } + break; + case QGC_MAINWINDOW_STYLE_INDOOR: + qApp->setStyle("plastique"); + styleFileName = ":/images/style-mission.css"; + reloadStylesheet(); + break; + case QGC_MAINWINDOW_STYLE_OUTDOOR: + qApp->setStyle("plastique"); + styleFileName = ":/images/style-outdoor.css"; + reloadStylesheet(); + break; + } + currentStyle = style; +} + +void MainWindow::selectStylesheet() +{ // Let user select style sheet - fileName = QFileDialog::getOpenFileName(this, tr("Specify stylesheet"), fileName, tr("CSS Stylesheet (*.css);;")); + styleFileName = QFileDialog::getOpenFileName(this, tr("Specify stylesheet"), styleFileName, tr("CSS Stylesheet (*.css);;")); - if (!fileName.endsWith(".css")) + if (!styleFileName.endsWith(".css")) { QMessageBox msgBox; msgBox.setIcon(QMessageBox::Information); @@ -1159,7 +1263,13 @@ void MainWindow::reloadStylesheet() } // Load style sheet - QFile* styleSheet = new QFile(fileName); + reloadStylesheet(); +} + +void MainWindow::reloadStylesheet() +{ + // Load style sheet + QFile* styleSheet = new QFile(styleFileName); if (!styleSheet->exists()) { styleSheet = new QFile(":/images/style-mission.css"); @@ -1175,7 +1285,7 @@ void MainWindow::reloadStylesheet() QMessageBox msgBox; msgBox.setIcon(QMessageBox::Information); msgBox.setText(tr("QGroundControl did lot load a new style")); - msgBox.setInformativeText(tr("Stylesheet file %1 was not readable").arg(fileName)); + msgBox.setInformativeText(tr("Stylesheet file %1 was not readable").arg(styleFileName)); msgBox.setStandardButtons(QMessageBox::Ok); msgBox.setDefaultButton(QMessageBox::Ok); msgBox.exec(); @@ -1285,7 +1395,8 @@ void MainWindow::connectCommonActions() connect(ui.actionUnconnectedView, SIGNAL(triggered()), this, SLOT(loadUnconnectedView())); connect(ui.actionMavlinkView, SIGNAL(triggered()), this, SLOT(loadMAVLinkView())); - connect(ui.actionReloadStyle, SIGNAL(triggered()), this, SLOT(reloadStylesheet())); + connect(ui.actionReloadStylesheet, SIGNAL(triggered()), this, SLOT(reloadStylesheet())); + connect(ui.actionSelectStylesheet, SIGNAL(triggered()), this, SLOT(selectStylesheet())); // Help Actions connect(ui.actionOnline_Documentation, SIGNAL(triggered()), this, SLOT(showHelp())); @@ -1297,6 +1408,7 @@ void MainWindow::connectCommonActions() // Audio output ui.actionMuteAudioOutput->setChecked(GAudioOutput::instance()->isMuted()); + connect(GAudioOutput::instance(), SIGNAL(mutedChanged(bool)), ui.actionMuteAudioOutput, SLOT(setChecked(bool))); connect(ui.actionMuteAudioOutput, SIGNAL(triggered(bool)), GAudioOutput::instance(), SLOT(mute(bool))); // User interaction @@ -1305,8 +1417,12 @@ void MainWindow::connectCommonActions() // unless it is actually used // so no ressources spend on this. ui.actionJoystickSettings->setVisible(true); - // Joystick configuration + + // Configuration + // Joystick connect(ui.actionJoystickSettings, SIGNAL(triggered()), this, SLOT(configure())); + // Application Settings + connect(ui.actionSettings, SIGNAL(triggered()), this, SLOT(showSettings())); } void MainWindow::connectPxActions() @@ -1375,6 +1491,12 @@ void MainWindow::configure() joystickWidget->show(); } +void MainWindow::showSettings() +{ + QGCSettingsWidget* settings = new QGCSettingsWidget(this); + settings->show(); +} + void MainWindow::addLink() { SerialLink* link = new SerialLink(); @@ -1398,23 +1520,43 @@ void MainWindow::addLink() void MainWindow::addLink(LinkInterface *link) { + // IMPORTANT! KEEP THESE TWO LINES + // THEY MAKE SURE THE LINK IS PROPERLY REGISTERED + // BEFORE LINKING THE UI AGAINST IT + // Register (does nothing if already registered) LinkManager::instance()->add(link); LinkManager::instance()->addProtocol(link, mavlink); - CommConfigurationWindow* commWidget = new CommConfigurationWindow(link, mavlink, this); - ui.menuNetwork->addAction(commWidget->getAction()); + // Go fishing for this link's configuration window + QList actions = ui.menuNetwork->actions(); - // Error handling - connect(link, SIGNAL(communicationError(QString,QString)), this, SLOT(showCriticalMessage(QString,QString)), Qt::QueuedConnection); + bool found = false; - //qDebug() << "ADDING LINK:" << link->getName() << "ACTION IS: " << commWidget->getAction(); + foreach (QAction* act, actions) + { + if (act->data().toInt() == LinkManager::instance()->getLinks().indexOf(link)) + { + found = true; + } + } + + UDPLink* udp = dynamic_cast(link); - // Special case for simulationlink - MAVLinkSimulationLink* sim = dynamic_cast(link); - if (sim) + if (!found || udp) { - //connect(sim, SIGNAL(valueChanged(int,QString,double,quint64)), linechart, SLOT(appendData(int,QString,double,quint64))); - connect(ui.actionSimulate, SIGNAL(triggered(bool)), sim, SLOT(connectLink(bool))); + CommConfigurationWindow* commWidget = new CommConfigurationWindow(link, mavlink, this); + QAction* action = commWidget->getAction(); + ui.menuNetwork->addAction(action); + + // Error handling + connect(link, SIGNAL(communicationError(QString,QString)), this, SLOT(showCriticalMessage(QString,QString)), Qt::QueuedConnection); + // Special case for simulationlink + MAVLinkSimulationLink* sim = dynamic_cast(link); + if (sim) + { + //connect(sim, SIGNAL(valueChanged(int,QString,double,quint64)), linechart, SLOT(appendData(int,QString,double,quint64))); + connect(ui.actionSimulate, SIGNAL(triggered(bool)), sim, SLOT(connectLink(bool))); + } } } @@ -1580,8 +1722,6 @@ void MainWindow::UASCreated(UASInterface* uas) // the currently active UAS if (UASManager::instance()->getUASList().size() == 1) { - //qDebug() << "UPDATING THE VIEW SINCE THIS IS THE FIRST CONNECTED SYSTEM"; - // Load last view if setting is present if (settings.contains("CURRENT_VIEW_WITH_UAS_CONNECTED")) { @@ -1640,12 +1780,12 @@ void MainWindow::clearView() if (temp) { - qDebug() << "TOOL:" << chKey << "IS:" << temp->isChecked(); + //qDebug() << "TOOL:" << chKey << "IS:" << temp->isChecked(); settings.setValue(chKey,temp->isChecked()); } else { - qDebug() << "TOOL:" << chKey << "IS DEFAULT AND UNCHECKED"; + //qDebug() << "TOOL:" << chKey << "IS DEFAULT AND UNCHECKED"; settings.setValue(chKey,false); } } @@ -1832,8 +1972,6 @@ void MainWindow::presentView() // VIDEO 2 showTheWidget(MENU_VIDEO_STREAM_2, currentView); - this->show(); - // Restore window state if (UASManager::instance()->getUASList().count() > 0) { @@ -1849,6 +1987,18 @@ void MainWindow::presentView() restoreState(settings.value(getWindowStateKey()).toByteArray(), QGC::applicationVersion()); } } + + // ACTIVATE MAP WIDGET + if (headUpDockWidget) + { + HUD* tmpHud = dynamic_cast( headUpDockWidget->widget() ); + if (tmpHud) + { + + } + } + + this->show(); } void MainWindow::showTheCentralWidget (TOOLS_WIDGET_NAMES centralWidget, VIEW_SECTIONS view) @@ -1857,16 +2007,16 @@ void MainWindow::showTheCentralWidget (TOOLS_WIDGET_NAMES centralWidget, VIEW_SE QWidget* tempWidget = dockWidgets[centralWidget]; tempVisible = settings.value(buildMenuKey(SUB_SECTION_CHECKED,centralWidget,view), false).toBool(); - qDebug() << buildMenuKey (SUB_SECTION_CHECKED,centralWidget,view) << tempVisible; + //qDebug() << buildMenuKey (SUB_SECTION_CHECKED,centralWidget,view) << tempVisible; if (toolsMenuActions[centralWidget]) { - qDebug() << "SETTING TO:" << tempVisible; + //qDebug() << "SETTING TO:" << tempVisible; toolsMenuActions[centralWidget]->setChecked(tempVisible); } if (centerStack && tempWidget && tempVisible) { - qDebug() << "ACTIVATING MAIN WIDGET"; + //qDebug() << "ACTIVATING MAIN WIDGET"; centerStack->setCurrentWidget(tempWidget); } } diff --git a/src/ui/MainWindow.h b/src/ui/MainWindow.h index 4946deba02cc15b31b32978c2a43e14e445eee00..88e79e7326e96a94a8b02bf4f5c9a7ab035e0804 100644 --- a/src/ui/MainWindow.h +++ b/src/ui/MainWindow.h @@ -88,6 +88,18 @@ public: static MainWindow* instance(); ~MainWindow(); + enum QGC_MAINWINDOW_STYLE + { + QGC_MAINWINDOW_STYLE_NATIVE, + QGC_MAINWINDOW_STYLE_INDOOR, + QGC_MAINWINDOW_STYLE_OUTDOOR + }; + + /** @brief Get current visual style */ + int getStyle() { return currentStyle; } + /** @brief Get auto link reconnect setting */ + bool autoReconnectEnabled() { return autoReconnect; } + public slots: // /** @brief Store the mainwindow settings */ // void storeSettings(); @@ -101,6 +113,9 @@ public slots: /** @brief Shows an info message as popup or as widget */ void showInfoMessage(const QString& title, const QString& message); + /** @brief Show the application settings */ + void showSettings(); + /** @brief Add a communication link */ void addLink(); void addLink(LinkInterface* link); void configure(); @@ -137,32 +152,27 @@ public slots: /** @brief Reload the CSS style sheet */ void reloadStylesheet(); + /** @brief Let the user select the CSS style sheet */ + void selectStylesheet(); + /** @brief Automatically reconnect last link */ + void enableAutoReconnect(bool enabled); + /** @brief Switch to native application style */ + void loadNativeStyle(); + /** @brief Switch to indoor mission style */ + void loadIndoorStyle(); + /** @brief Switch to outdoor mission style */ + void loadOutdoorStyle(); + /** @brief Load a specific style */ + void loadStyle(QGC_MAINWINDOW_STYLE style); /** @brief Add a custom tool widget */ void createCustomWidget(); void closeEvent(QCloseEvent* event); - /* - ========================================================== - Potentially Deprecated - ========================================================== - */ - -// void loadWidgets(); - /** @brief Load data view, allowing to plot flight data */ void loadDataView(QString fileName); -// /** @brief Load 3D map view */ -// void load3DMapView(); - -// /** @brief Load 3D Google Earth view */ -// void loadGoogleEarthView(); - -// /** @brief Load 3D view */ -// void load3DView(); - /** * @brief Shows a Docked Widget based on the action sender * @@ -338,6 +348,8 @@ protected: void configureWindowName(); + void loadSettings(); + void storeSettings(); // TODO Should be moved elsewhere, as the protocol does not belong to the UI MAVLinkProtocol* mavlink; @@ -411,6 +423,9 @@ protected: LogCompressor* comp; QString screenFileName; QTimer* videoTimer; + QString styleFileName; + bool autoReconnect; + QGC_MAINWINDOW_STYLE currentStyle; private: Ui::MainWindow ui; diff --git a/src/ui/MainWindow.ui b/src/ui/MainWindow.ui index e50845e0b7444625bc31fabeea567caead410647..d042ddaebd66512645c4f1c83522f7bf99819c0c 100644 --- a/src/ui/MainWindow.ui +++ b/src/ui/MainWindow.ui @@ -58,13 +58,20 @@ File + + + Display + + + + - - + + @@ -338,13 +345,13 @@ Meta+M - + :/images/categories/applications-internet.svg:/images/categories/applications-internet.svg - Load Stylesheet + Select Stylesheet @@ -387,18 +394,6 @@ Mute Audio Output - - - - :/images/categories/preferences-system.svg:/images/categories/preferences-system.svg - - - Preferences - - - QGroundControl global settings - - true @@ -429,6 +424,19 @@ Shutdown the onboard computer - works not during flight + + + Reload Stylesheet + + + Meta+R + + + + + Settings + + diff --git a/src/ui/MapWidget.cc b/src/ui/MapWidget.cc index de875253764f44b9d1b9ad9c05d8b312a23655a3..a8035879b1a8c338e52793986df4ce61c09c483f 100644 --- a/src/ui/MapWidget.cc +++ b/src/ui/MapWidget.cc @@ -28,227 +28,252 @@ MapWidget::MapWidget(QWidget *parent) : QWidget(parent), + mc(NULL), zoomLevel(0), uasIcons(), uasTrails(), mav(NULL), lastUpdate(0), + initialized(false), m_ui(new Ui::MapWidget) { m_ui->setupUi(this); - mc = new qmapcontrol::MapControl(this->size()); - - // VISUAL MAP STYLE - QString buttonStyle("QAbstractButton { background-color: rgba(20, 20, 20, 45%); border-color: rgba(10, 10, 10, 50%)} QAbstractButton:checked { border: 2px solid #379AC3; }"); - mc->setPen(QGC::colorCyan.darker(400)); - - - + init(); +} +void MapWidget::init() +{ + if (!initialized) + { + mc = new qmapcontrol::MapControl(this->size()); + // display the MapControl in the application + QGridLayout* layout = new QGridLayout(this); + layout->setMargin(0); + layout->setSpacing(0); + layout->addWidget(mc, 0, 0); + setLayout(layout); + + // VISUAL MAP STYLE + QString buttonStyle("QAbstractButton { background-color: rgba(20, 20, 20, 45%); border-color: rgba(10, 10, 10, 50%)} QAbstractButton:checked { border: 2px solid #379AC3; }"); + mc->setPen(QGC::colorCyan.darker(400)); + waypointIsDrag = false; + // Accept focus by clicking or keyboard + this->setFocusPolicy(Qt::StrongFocus); + // create MapControl + mc->showScale(true); + mc->showCoord(true); + mc->enablePersistentCache(); + mc->setMouseTracking(true); // required to update the mouse position for diplay and capture - waypointIsDrag = false; + // create MapAdapter to get maps from + //TileMapAdapter* osmAdapter = new TileMapAdapter("tile.openstreetmap.org", "/%1/%2/%3.png", 256, 0, 17); - // Accept focus by clicking or keyboard - this->setFocusPolicy(Qt::StrongFocus); - - // create MapControl - - mc->showScale(true); - mc->showCoord(true); - mc->enablePersistentCache(); - mc->setMouseTracking(true); // required to update the mouse position for diplay and capture - - // create MapAdapter to get maps from - //TileMapAdapter* osmAdapter = new TileMapAdapter("tile.openstreetmap.org", "/%1/%2/%3.png", 256, 0, 17); - - qmapcontrol::MapAdapter* mapadapter_overlay = new qmapcontrol::YahooMapAdapter("us.maps3.yimg.com", "/aerial.maps.yimg.com/png?v=2.2&t=h&s=256&x=%2&y=%3&z=%1"); - - // MAP BACKGROUND - mapadapter = new qmapcontrol::GoogleSatMapAdapter(); - l = new qmapcontrol::MapLayer("Google Satellite", mapadapter); - mc->addLayer(l); - - // STREET OVERLAY - overlay = new qmapcontrol::MapLayer("Overlay", mapadapter_overlay); - overlay->setVisible(false); - mc->addLayer(overlay); - - // MAV FLIGHT TRACKS - tracks = new qmapcontrol::MapLayer("Tracking", mapadapter); - mc->addLayer(tracks); - - // WAYPOINT LAYER - // create a layer with the mapadapter and type GeometryLayer (for waypoints) - geomLayer = new qmapcontrol::GeometryLayer("Waypoints", mapadapter); - mc->addLayer(geomLayer); - - - - // - // Layer* gsatLayer = new Layer("Google Satellite", gsat, Layer::MapLayer); - // mc->addLayer(gsatLayer); - - // SET INITIAL POSITION AND ZOOM - // Set default zoom level - mc->setZoom(16); - // Zurich, ETH - mc->setView(QPointF(8.548056,47.376889)); - - // Veracruz Mexico - //mc->setView(QPointF(-96.105208,19.138955)); - - // Add controls to select map provider - ///////////////////////////////////////////////// - QActionGroup* mapproviderGroup = new QActionGroup(this); - osmAction = new QAction(QIcon(":/images/mapproviders/openstreetmap.png"), tr("OpenStreetMap"), mapproviderGroup); - yahooActionMap = new QAction(QIcon(":/images/mapproviders/yahoo.png"), tr("Yahoo: Map"), mapproviderGroup); - yahooActionSatellite = new QAction(QIcon(":/images/mapproviders/yahoo.png"), tr("Yahoo: Satellite"), mapproviderGroup); - googleActionMap = new QAction(QIcon(":/images/mapproviders/google.png"), tr("Google: Map"), mapproviderGroup); - googleSatAction = new QAction(QIcon(":/images/mapproviders/google.png"), tr("Google: Sat"), mapproviderGroup); - osmAction->setCheckable(true); - yahooActionMap->setCheckable(true); - yahooActionSatellite->setCheckable(true); - googleActionMap->setCheckable(true); - googleSatAction->setCheckable(true); - googleSatAction->setChecked(true); - connect(mapproviderGroup, SIGNAL(triggered(QAction*)), - this, SLOT(mapproviderSelected(QAction*))); - - // Overlay seems currently broken - // yahooActionOverlay = new QAction(tr("Yahoo: street overlay"), this); - // yahooActionOverlay->setCheckable(true); - // yahooActionOverlay->setChecked(overlay->isVisible()); - // connect(yahooActionOverlay, SIGNAL(toggled(bool)), - // overlay, SLOT(setVisible(bool))); - - // mapproviderGroup->addAction(googleSatAction); - // mapproviderGroup->addAction(osmAction); - // mapproviderGroup->addAction(yahooActionOverlay); - // mapproviderGroup->addAction(googleActionMap); - // mapproviderGroup->addAction(yahooActionMap); - // mapproviderGroup->addAction(yahooActionSatellite); - - // Create map provider selection menu - mapMenu = new QMenu(this); - mapMenu->addActions(mapproviderGroup->actions()); - mapMenu->addSeparator(); - // mapMenu->addAction(yahooActionOverlay); - - mapButton = new QPushButton(this); - mapButton->setText("Map Source"); - mapButton->setMenu(mapMenu); - mapButton->setStyleSheet(buttonStyle); - - // display the MapControl in the application - QGridLayout* layout = new QGridLayout(this); - layout->setMargin(0); - layout->setSpacing(0); - layout->addWidget(mc, 0, 0); - setLayout(layout); - - // create buttons to control the map (zoom, GPS tracking and WP capture) - QPushButton* zoomin = new QPushButton(QIcon(":/images/actions/list-add.svg"), "", this); - zoomin->setStyleSheet(buttonStyle); - QPushButton* zoomout = new QPushButton(QIcon(":/images/actions/list-remove.svg"), "", this); - zoomout->setStyleSheet(buttonStyle); - createPath = new QPushButton(QIcon(":/images/actions/go-bottom.svg"), "", this); - createPath->setStyleSheet(buttonStyle); -// clearTracking = new QPushButton(QIcon(""), "", this); -// clearTracking->setStyleSheet(buttonStyle); - followgps = new QPushButton(QIcon(":/images/actions/system-lock-screen.svg"), "", this); - followgps->setStyleSheet(buttonStyle); - QPushButton* goToButton = new QPushButton(QIcon(""), "T", this); - goToButton->setStyleSheet(buttonStyle); - - zoomin->setMaximumWidth(30); - zoomout->setMaximumWidth(30); - createPath->setMaximumWidth(30); -// clearTracking->setMaximumWidth(30); - followgps->setMaximumWidth(30); - goToButton->setMaximumWidth(30); - - // Set checkable buttons - // TODO: Currently checked buttons are are very difficult to distinguish when checked. - // create a style and the slots to change the background so it is easier to distinguish - followgps->setCheckable(true); - createPath->setCheckable(true); - - // add buttons to control the map (zoom, GPS tracking and WP capture) - QGridLayout* innerlayout = new QGridLayout(mc); - innerlayout->setMargin(3); - innerlayout->setSpacing(3); - innerlayout->addWidget(zoomin, 0, 0); - innerlayout->addWidget(zoomout, 1, 0); - innerlayout->addWidget(followgps, 2, 0); - innerlayout->addWidget(createPath, 3, 0); - //innerlayout->addWidget(clearTracking, 4, 0); - // Add spacers to compress buttons on the top left - innerlayout->addItem(new QSpacerItem(0, 0, QSizePolicy::Expanding, QSizePolicy::Expanding), 5, 0); - innerlayout->addItem(new QSpacerItem(0, 0, QSizePolicy::Expanding, QSizePolicy::Expanding), 0, 1, 0, 7); - innerlayout->addWidget(mapButton, 0, 6); - innerlayout->addWidget(goToButton, 0, 7); - innerlayout->setRowStretch(0, 1); - innerlayout->setRowStretch(1, 100); - mc->setLayout(innerlayout); - - - // Connect the required signals-slots - connect(zoomin, SIGNAL(clicked(bool)), - mc, SLOT(zoomIn())); - - connect(zoomout, SIGNAL(clicked(bool)), - mc, SLOT(zoomOut())); - - connect(goToButton, SIGNAL(clicked()), this, SLOT(goTo())); - - QList systems = UASManager::instance()->getUASList(); - foreach(UASInterface* system, systems) - { - addUAS(system); - } + qmapcontrol::MapAdapter* mapadapter_overlay = new qmapcontrol::YahooMapAdapter("us.maps3.yimg.com", "/aerial.maps.yimg.com/png?v=2.2&t=h&s=256&x=%2&y=%3&z=%1"); - connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)), - this, SLOT(addUAS(UASInterface*))); - - activeUASSet(UASManager::instance()->getActiveUAS()); - connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(activeUASSet(UASInterface*))); + // MAP BACKGROUND + mapadapter = new qmapcontrol::GoogleSatMapAdapter(); + l = new qmapcontrol::MapLayer("Google Satellite", mapadapter); + mc->addLayer(l); - connect(mc, SIGNAL(mouseEventCoordinate(const QMouseEvent*, const QPointF)), - this, SLOT(captureMapClick(const QMouseEvent*, const QPointF))); + // STREET OVERLAY + overlay = new qmapcontrol::MapLayer("Overlay", mapadapter_overlay); + overlay->setVisible(false); + mc->addLayer(overlay); + + // MAV FLIGHT TRACKS + tracks = new qmapcontrol::MapLayer("Tracking", mapadapter); + mc->addLayer(tracks); + + // WAYPOINT LAYER + // create a layer with the mapadapter and type GeometryLayer (for waypoints) + geomLayer = new qmapcontrol::GeometryLayer("Waypoints", mapadapter); + mc->addLayer(geomLayer); + + + + // + // Layer* gsatLayer = new Layer("Google Satellite", gsat, Layer::MapLayer); + // mc->addLayer(gsatLayer); + + + // Zurich, ETH + + int lastZoom = 16; + double lastLat = 47.376889; + double lastLon = 8.548056; + + QSettings settings; + settings.beginGroup("QGC_MAPWIDGET"); + lastLat = settings.value("LAST_LATITUDE", lastLat).toDouble(); + lastLon = settings.value("LAST_LONGITUDE", lastLon).toDouble(); + lastZoom = settings.value("LAST_ZOOM", lastZoom).toInt(); + settings.endGroup(); + + // SET INITIAL POSITION AND ZOOM + // Set default zoom level + mc->setZoom(lastZoom); + mc->setView(QPointF(lastLon, lastLat)); + + // Veracruz Mexico + //mc->setView(QPointF(-96.105208,19.138955)); + + // Add controls to select map provider + ///////////////////////////////////////////////// + QActionGroup* mapproviderGroup = new QActionGroup(this); + osmAction = new QAction(QIcon(":/images/mapproviders/openstreetmap.png"), tr("OpenStreetMap"), mapproviderGroup); + yahooActionMap = new QAction(QIcon(":/images/mapproviders/yahoo.png"), tr("Yahoo: Map"), mapproviderGroup); + yahooActionSatellite = new QAction(QIcon(":/images/mapproviders/yahoo.png"), tr("Yahoo: Satellite"), mapproviderGroup); + googleActionMap = new QAction(QIcon(":/images/mapproviders/google.png"), tr("Google: Map"), mapproviderGroup); + googleSatAction = new QAction(QIcon(":/images/mapproviders/google.png"), tr("Google: Sat"), mapproviderGroup); + osmAction->setCheckable(true); + yahooActionMap->setCheckable(true); + yahooActionSatellite->setCheckable(true); + googleActionMap->setCheckable(true); + googleSatAction->setCheckable(true); + googleSatAction->setChecked(true); + connect(mapproviderGroup, SIGNAL(triggered(QAction*)), + this, SLOT(mapproviderSelected(QAction*))); + + // Overlay seems currently broken + // yahooActionOverlay = new QAction(tr("Yahoo: street overlay"), this); + // yahooActionOverlay->setCheckable(true); + // yahooActionOverlay->setChecked(overlay->isVisible()); + // connect(yahooActionOverlay, SIGNAL(toggled(bool)), + // overlay, SLOT(setVisible(bool))); + + // mapproviderGroup->addAction(googleSatAction); + // mapproviderGroup->addAction(osmAction); + // mapproviderGroup->addAction(yahooActionOverlay); + // mapproviderGroup->addAction(googleActionMap); + // mapproviderGroup->addAction(yahooActionMap); + // mapproviderGroup->addAction(yahooActionSatellite); + + // Create map provider selection menu + mapMenu = new QMenu(this); + mapMenu->addActions(mapproviderGroup->actions()); + mapMenu->addSeparator(); + // mapMenu->addAction(yahooActionOverlay); + + mapButton = new QPushButton(this); + mapButton->setText("Map Source"); + mapButton->setMenu(mapMenu); + mapButton->setStyleSheet(buttonStyle); + + // create buttons to control the map (zoom, GPS tracking and WP capture) + QPushButton* zoomin = new QPushButton(QIcon(":/images/actions/list-add.svg"), "", this); + zoomin->setStyleSheet(buttonStyle); + QPushButton* zoomout = new QPushButton(QIcon(":/images/actions/list-remove.svg"), "", this); + zoomout->setStyleSheet(buttonStyle); + createPath = new QPushButton(QIcon(":/images/actions/go-bottom.svg"), "", this); + createPath->setStyleSheet(buttonStyle); + createPath->setToolTip(tr("Start / end waypoint add mode")); + createPath->setStatusTip(tr("Start / end waypoint add mode")); + // clearTracking = new QPushButton(QIcon(""), "", this); + // clearTracking->setStyleSheet(buttonStyle); + followgps = new QPushButton(QIcon(":/images/actions/system-lock-screen.svg"), "", this); + followgps->setStyleSheet(buttonStyle); + followgps->setToolTip(tr("Follow the position of the current MAV with the map center")); + followgps->setStatusTip(tr("Follow the position of the current MAV with the map center")); + QPushButton* goToButton = new QPushButton(QIcon(""), "T", this); + goToButton->setStyleSheet(buttonStyle); + goToButton->setToolTip(tr("Enter a latitude/longitude position to move the map to")); + goToButton->setStatusTip(tr("Enter a latitude/longitude position to move the map to")); + + zoomin->setMaximumWidth(30); + zoomout->setMaximumWidth(30); + createPath->setMaximumWidth(30); + // clearTracking->setMaximumWidth(30); + followgps->setMaximumWidth(30); + goToButton->setMaximumWidth(30); + + // Set checkable buttons + // TODO: Currently checked buttons are are very difficult to distinguish when checked. + // create a style and the slots to change the background so it is easier to distinguish + followgps->setCheckable(true); + createPath->setCheckable(true); + + // add buttons to control the map (zoom, GPS tracking and WP capture) + QGridLayout* innerlayout = new QGridLayout(mc); + innerlayout->setMargin(3); + innerlayout->setSpacing(3); + innerlayout->addWidget(zoomin, 0, 0); + innerlayout->addWidget(zoomout, 1, 0); + innerlayout->addWidget(followgps, 2, 0); + innerlayout->addWidget(createPath, 3, 0); + //innerlayout->addWidget(clearTracking, 4, 0); + // Add spacers to compress buttons on the top left + innerlayout->addItem(new QSpacerItem(0, 0, QSizePolicy::Expanding, QSizePolicy::Expanding), 5, 0); + innerlayout->addItem(new QSpacerItem(0, 0, QSizePolicy::Expanding, QSizePolicy::Expanding), 0, 1, 0, 7); + innerlayout->addWidget(mapButton, 0, 6); + innerlayout->addWidget(goToButton, 0, 7); + innerlayout->setRowStretch(0, 1); + innerlayout->setRowStretch(1, 100); + mc->setLayout(innerlayout); + + // Configure the WP Path's pen + pointPen = new QPen(QColor(0, 255,0)); + pointPen->setWidth(3); + waypointPath = new qmapcontrol::LineString (wps, "Waypoint path", pointPen); + mc->layer("Waypoints")->addGeometry(waypointPath); + + //Camera Control + // CAMERA INDICATOR LAYER + // create a layer with the mapadapter and type GeometryLayer (for camera indicator) + camLayer = new qmapcontrol::GeometryLayer("Camera", mapadapter); + mc->addLayer(camLayer); + + //camLine = new qmapcontrol::LineString(camPoints,"Camera Eje", camBorderPen); + + drawCamBorder = false; + radioCamera = 10; + + // Done set state + initialized = true; + + + // Connect the required signals-slots + connect(zoomin, SIGNAL(clicked(bool)), + mc, SLOT(zoomIn())); + + connect(zoomout, SIGNAL(clicked(bool)), + mc, SLOT(zoomOut())); + + connect(goToButton, SIGNAL(clicked()), this, SLOT(goTo())); + + QList systems = UASManager::instance()->getUASList(); + foreach(UASInterface* system, systems) + { + addUAS(system); + } - connect(createPath, SIGNAL(clicked(bool)), - this, SLOT(createPathButtonClicked(bool))); + connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)), + this, SLOT(addUAS(UASInterface*))); + activeUASSet(UASManager::instance()->getActiveUAS()); + connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(activeUASSet(UASInterface*))); - connect(geomLayer, SIGNAL(geometryClicked(Geometry*,QPoint)), - this, SLOT(captureGeometryClick(Geometry*, QPoint))); + connect(mc, SIGNAL(mouseEventCoordinate(const QMouseEvent*, const QPointF)), + this, SLOT(captureMapClick(const QMouseEvent*, const QPointF))); - connect(geomLayer, SIGNAL(geometryDragged(Geometry*, QPointF)), - this, SLOT(captureGeometryDrag(Geometry*, QPointF))); + connect(createPath, SIGNAL(clicked(bool)), + this, SLOT(createPathButtonClicked(bool))); - connect(geomLayer, SIGNAL(geometryEndDrag(Geometry*, QPointF)), - this, SLOT(captureGeometryEndDrag(Geometry*, QPointF))); - // Configure the WP Path's pen - pointPen = new QPen(QColor(0, 255,0)); - pointPen->setWidth(3); - waypointPath = new qmapcontrol::LineString (wps, "Waypoint path", pointPen); - mc->layer("Waypoints")->addGeometry(waypointPath); + connect(geomLayer, SIGNAL(geometryClicked(Geometry*,QPoint)), + this, SLOT(captureGeometryClick(Geometry*, QPoint))); - //Camera Control - // CAMERA INDICATOR LAYER - // create a layer with the mapadapter and type GeometryLayer (for camera indicator) - camLayer = new qmapcontrol::GeometryLayer("Camera", mapadapter); - mc->addLayer(camLayer); + connect(geomLayer, SIGNAL(geometryDragged(Geometry*, QPointF)), + this, SLOT(captureGeometryDrag(Geometry*, QPointF))); - //camLine = new qmapcontrol::LineString(camPoints,"Camera Eje", camBorderPen); + connect(geomLayer, SIGNAL(geometryEndDrag(Geometry*, QPointF)), + this, SLOT(captureGeometryEndDrag(Geometry*, QPointF))); - drawCamBorder = false; - radioCamera = 10; + qDebug() << "CHECK END"; + } } void MapWidget::goTo() @@ -256,7 +281,7 @@ void MapWidget::goTo() bool ok; QString text = QInputDialog::getText(this, tr("Please enter coordinates"), tr("Coordinates (Lat,Lon):"), QLineEdit::Normal, - QString("%1,%2").arg(mc->currentCoordinate().x()).arg(mc->currentCoordinate().y()), &ok); + QString("%1,%2").arg(mc->currentCoordinate().y()).arg(mc->currentCoordinate().x()), &ok); if (ok && !text.isEmpty()) { QStringList split = text.split(","); @@ -270,7 +295,7 @@ void MapWidget::goTo() if (ok) { - mc->setView(QPointF(latitude, longitude)); + mc->setView(QPointF(longitude, latitude)); } } } @@ -279,116 +304,123 @@ void MapWidget::goTo() void MapWidget::mapproviderSelected(QAction* action) { - //delete mapadapter; - mapButton->setText(action->text()); - if (action == osmAction) - { - int zoom = mapadapter->adaptedZoom(); - mc->setZoom(0); - - mapadapter = new qmapcontrol::OSMMapAdapter(); - l->setMapAdapter(mapadapter); - geomLayer->setMapAdapter(mapadapter); - - if (isVisible()) mc->updateRequestNew(); - mc->setZoom(zoom); - // yahooActionOverlay->setEnabled(false); - overlay->setVisible(false); - // yahooActionOverlay->setChecked(false); - - } - else if (action == yahooActionMap) + if (mc) { - int zoom = mapadapter->adaptedZoom(); - mc->setZoom(0); + //delete mapadapter; + mapButton->setText(action->text()); + if (action == osmAction) + { + int zoom = mapadapter->adaptedZoom(); + mc->setZoom(0); - mapadapter = new qmapcontrol::YahooMapAdapter(); - l->setMapAdapter(mapadapter); - geomLayer->setMapAdapter(mapadapter); + mapadapter = new qmapcontrol::OSMMapAdapter(); + l->setMapAdapter(mapadapter); + geomLayer->setMapAdapter(mapadapter); - if (isVisible()) mc->updateRequestNew(); - mc->setZoom(zoom); - // yahooActionOverlay->setEnabled(false); - overlay->setVisible(false); - // yahooActionOverlay->setChecked(false); - } - else if (action == yahooActionSatellite) - { - int zoom = mapadapter->adaptedZoom(); - QPointF a = mc->currentCoordinate(); - mc->setZoom(0); + if (isVisible()) mc->updateRequestNew(); + mc->setZoom(zoom); + // yahooActionOverlay->setEnabled(false); + overlay->setVisible(false); + // yahooActionOverlay->setChecked(false); - mapadapter = new qmapcontrol::YahooMapAdapter("us.maps3.yimg.com", "/aerial.maps.yimg.com/png?v=1.7&t=a&s=256&x=%2&y=%3&z=%1"); - l->setMapAdapter(mapadapter); - - if (isVisible()) mc->updateRequestNew(); - mc->setZoom(zoom); - // yahooActionOverlay->setEnabled(true); - } - else if (action == googleActionMap) - { - int zoom = mapadapter->adaptedZoom(); - mc->setZoom(0); - mapadapter = new qmapcontrol::GoogleMapAdapter(); - l->setMapAdapter(mapadapter); - geomLayer->setMapAdapter(mapadapter); - - if (isVisible()) mc->updateRequestNew(); - mc->setZoom(zoom); - // yahooActionOverlay->setEnabled(false); - overlay->setVisible(false); - // yahooActionOverlay->setChecked(false); - } - else if (action == googleSatAction) - { - int zoom = mapadapter->adaptedZoom(); - mc->setZoom(0); - mapadapter = new qmapcontrol::GoogleSatMapAdapter(); - l->setMapAdapter(mapadapter); - geomLayer->setMapAdapter(mapadapter); - - if (isVisible()) mc->updateRequestNew(); - mc->setZoom(zoom); - // yahooActionOverlay->setEnabled(false); - overlay->setVisible(false); - // yahooActionOverlay->setChecked(false); - } - else - { - mapButton->setText("Select.."); + } + else if (action == yahooActionMap) + { + int zoom = mapadapter->adaptedZoom(); + mc->setZoom(0); + + mapadapter = new qmapcontrol::YahooMapAdapter(); + l->setMapAdapter(mapadapter); + geomLayer->setMapAdapter(mapadapter); + + if (isVisible()) mc->updateRequestNew(); + mc->setZoom(zoom); + // yahooActionOverlay->setEnabled(false); + overlay->setVisible(false); + // yahooActionOverlay->setChecked(false); + } + else if (action == yahooActionSatellite) + { + int zoom = mapadapter->adaptedZoom(); + QPointF a = mc->currentCoordinate(); + mc->setZoom(0); + + mapadapter = new qmapcontrol::YahooMapAdapter("us.maps3.yimg.com", "/aerial.maps.yimg.com/png?v=1.7&t=a&s=256&x=%2&y=%3&z=%1"); + l->setMapAdapter(mapadapter); + geomLayer->setMapAdapter(mapadapter); + + if (isVisible()) mc->updateRequestNew(); + mc->setZoom(zoom); + overlay->setVisible(false); + // yahooActionOverlay->setEnabled(true); + } + else if (action == googleActionMap) + { + int zoom = mapadapter->adaptedZoom(); + mc->setZoom(0); + mapadapter = new qmapcontrol::GoogleMapAdapter(); + l->setMapAdapter(mapadapter); + geomLayer->setMapAdapter(mapadapter); + + if (isVisible()) mc->updateRequestNew(); + mc->setZoom(zoom); + // yahooActionOverlay->setEnabled(false); + overlay->setVisible(false); + // yahooActionOverlay->setChecked(false); + } + else if (action == googleSatAction) + { + int zoom = mapadapter->adaptedZoom(); + mc->setZoom(0); + mapadapter = new qmapcontrol::GoogleSatMapAdapter(); + l->setMapAdapter(mapadapter); + geomLayer->setMapAdapter(mapadapter); + + if (isVisible()) mc->updateRequestNew(); + mc->setZoom(zoom); + // yahooActionOverlay->setEnabled(false); + overlay->setVisible(false); + // yahooActionOverlay->setChecked(false); + } + else + { + mapButton->setText("Select.."); + } } } void MapWidget::createPathButtonClicked(bool checked) { - Q_UNUSED(checked); - - if (createPath->isChecked()) + if (mc) { - // change the cursor shape - this->setCursor(Qt::PointingHandCursor); - mc->setMouseMode(qmapcontrol::MapControl::None); + Q_UNUSED(checked); + if (createPath->isChecked()) + { + // change the cursor shape + this->setCursor(Qt::PointingHandCursor); + mc->setMouseMode(qmapcontrol::MapControl::None); - // emit signal start to create a Waypoint global - //emit createGlobalWP(true, mc->currentCoordinate()); - // // Clear the previous WP track - // // TODO: Move this to an actual clear track button and add a warning dialog - // mc->layer("Waypoints")->clearGeometries(); - // wps.clear(); - // path->setPoints(wps); - // mc->layer("Waypoints")->addGeometry(path); - // wpIndex.clear(); - } - else - { + // emit signal start to create a Waypoint global + //emit createGlobalWP(true, mc->currentCoordinate()); - this->setCursor(Qt::ArrowCursor); - mc->setMouseMode(qmapcontrol::MapControl::Panning); - } + // // Clear the previous WP track + // // TODO: Move this to an actual clear track button and add a warning dialog + // mc->layer("Waypoints")->clearGeometries(); + // wps.clear(); + // path->setPoints(wps); + // mc->layer("Waypoints")->addGeometry(path); + // wpIndex.clear(); + } + else + { + this->setCursor(Qt::ArrowCursor); + mc->setMouseMode(qmapcontrol::MapControl::Panning); + } + } } /** @@ -411,7 +443,15 @@ void MapWidget::captureMapClick(const QMouseEvent* event, const QPointF coordina if (mav) { - mav->getWaypointManager()->addWaypoint(new Waypoint(mav->getWaypointManager()->getWaypointList().count(), coordinate.x(), coordinate.y(), 0.0f, 0.0f, true)); + double altitude = 0.0; + double yaw = 0.0; + int wpListCount = mav->getWaypointManager()->getWaypointList().count(); + if (wpListCount > 0) + { + altitude = mav->getWaypointManager()->getWaypointList().at(wpListCount-1)->getAltitude(); + yaw = mav->getWaypointManager()->getWaypointList().at(wpListCount-1)->getYaw(); + } + mav->getWaypointManager()->addWaypoint(new Waypoint(wpListCount, coordinate.y(), coordinate.x(), altitude, yaw, true)); } else { @@ -436,66 +476,91 @@ void MapWidget::captureMapClick(const QMouseEvent* event, const QPointF coordina void MapWidget::updateWaypoint(int uas, Waypoint* wp) { + // Update waypoint list and redraw map (last parameter) updateWaypoint(uas, wp, true); } +/** + * This function is called if a a single waypoint is updated and + * also if the whole list changes. + */ void MapWidget::updateWaypoint(int uas, Waypoint* wp, bool updateView) { - qDebug() << "UPDATING WP" << wp->getId() << wp << __FILE__ << __LINE__; - if (uas == this->mav->getUASID()) + if (mc) { - int wpindex = UASManager::instance()->getUASForId(uas)->getWaypointManager()->getIndexOf(wp); - if (wpindex == -1) return; - // Create waypoint name - //QString str = QString("%1").arg(wpindex); - // Check if wp exists yet - if (!(wpIcons.count() > wpindex)) - { - QPointF coordinate; - coordinate.setX(wp->getX()); - coordinate.setY(wp->getY()); - createWaypointGraphAtMap(wpindex, coordinate); - } - else + // Make sure this is the right UAS + if (uas == this->mav->getUASID()) { - // Waypoint exists, update it - if(!waypointIsDrag) + // Only accept waypoints in global coordinate frame + if (wp->getFrame() == MAV_FRAME_GLOBAL) { - qDebug() <<"indice WP= "<< wpindex <<"\n"; + // We're good, this is a global waypoint - QPointF coordinate; - coordinate.setX(wp->getX()); - coordinate.setY(wp->getY()); + // Get the index of this waypoint + // note the call to getGlobalFrameIndexOf() + // as we're only handling global waypoints + int wpindex = UASManager::instance()->getUASForId(uas)->getWaypointManager()->getGlobalFrameIndexOf(wp); + // If not found, return (this should never happen, but helps safety) + if (wpindex == -1) return; - Point* waypoint; - waypoint = wps.at(wpindex);//wpIndex[str]; - if (waypoint) + // Check if wp exists yet in map + if (!(wpIcons.count() > wpindex)) { - // First set waypoint coordinate - waypoint->setCoordinate(coordinate); - // Now update icon position - //mc->layer("Waypoints")->removeGeometry(wpIcons.at(wpindex)); - wpIcons.at(wpindex)->setCoordinate(coordinate); - //mc->layer("Waypoints")->addGeometry(wpIcons.at(wpindex)); - // Then waypoint line coordinate - Point* linesegment = NULL; - if (waypointPath->points().size() > wpindex) - { - linesegment = waypointPath->points().at(wpindex); - } - else - { - waypointPath->addPoint(waypoint); - } - - if (linesegment) + // Waypoint is new, a new icon is created + QPointF coordinate; + coordinate.setX(wp->getLongitude()); + coordinate.setY(wp->getLatitude()); + createWaypointGraphAtMap(wpindex, coordinate); + } + else + { + // Waypoint exists, update it if we're not + // currently dragging it with the mouse + if(!waypointIsDrag) { - linesegment->setCoordinate(coordinate); + QPointF coordinate; + coordinate.setX(wp->getLongitude()); + coordinate.setY(wp->getLatitude()); + + Point* waypoint; + waypoint = wps.at(wpindex); + if (waypoint) + { + // First set waypoint coordinate + waypoint->setCoordinate(coordinate); + // Now update icon position + wpIcons.at(wpindex)->setCoordinate(coordinate); + // Update pen + wpIcons.at(wpindex)->setPen(mavPens.value(uas)); + // Then waypoint line coordinate + Point* linesegment = NULL; + // If the line segment already exists, just update it + // else create a new one + if (waypointPath->points().size() > wpindex) + { + linesegment = waypointPath->points().at(wpindex); + if (linesegment) linesegment->setCoordinate(coordinate); + } + else + { + waypointPath->addPoint(waypoint); + } + + // Update view + if (updateView) if (isVisible()) mc->updateRequest(waypoint->boundingBox().toRect()); + } } - - //point2Find = dynamic_cast (mc->layer("Waypoints")->get_Geometry(wpindex)); - //point2Find->setCoordinate(coordinate); - if (updateView) if (isVisible()) mc->updateRequest(waypoint->boundingBox().toRect()); + } + } + else + { + // Check if the index of this waypoint is larger than the global + // waypoint list. This implies that the coordinate frame of this + // waypoint was changed and the list containing only global + // waypoints was shortened. Thus update the whole list + if (waypointPath->points().count() > UASManager::instance()->getUASForId(uas)->getWaypointManager()->getGlobalFrameCount()) + { + updateWaypointList(uas); } } } @@ -504,7 +569,7 @@ void MapWidget::updateWaypoint(int uas, Waypoint* wp, bool updateView) void MapWidget::createWaypointGraphAtMap(int id, const QPointF coordinate) { - if (!wpExists(coordinate)) + //if (!wpExists(coordinate)) { // Create waypoint name QString str; @@ -547,11 +612,14 @@ void MapWidget::createWaypointGraphAtMap(int id, const QPointF coordinate) int MapWidget::wpExists(const QPointF coordinate) { - for (int i = 0; i < wps.size(); i++){ - if (wps.at(i)->latitude() == coordinate.y() && - wps.at(i)->longitude()== coordinate.x()) - { - return 1; + if (mc) + { + for (int i = 0; i < wps.size(); i++){ + if (wps.at(i)->latitude() == coordinate.y() && + wps.at(i)->longitude()== coordinate.x()) + { + return 1; + } } } return 0; @@ -563,7 +631,7 @@ void MapWidget::captureGeometryClick(Geometry* geom, QPoint point) Q_UNUSED(geom); Q_UNUSED(point); - mc->setMouseMode(qmapcontrol::MapControl::None); + if (mc) mc->setMouseMode(qmapcontrol::MapControl::None); } void MapWidget::captureGeometryDrag(Geometry* geom, QPointF coordinate) @@ -591,13 +659,14 @@ void MapWidget::captureGeometryDrag(Geometry* geom, QPointF coordinate) // Update waypoint data storage if (mav) { - QVector wps = mav->getWaypointManager()->getWaypointList(); + QVector wps = mav->getWaypointManager()->getGlobalFrameWaypointList(); if (wps.size() > index) { - wps.at(index)->setX(coordinate.x()); - wps.at(index)->setY(coordinate.y()); - mav->getWaypointManager()->notifyOfChange(wps.at(index)); + Waypoint* wp = wps.at(index); + wp->setLatitude(coordinate.y()); + wp->setLongitude(coordinate.x()); + mav->getWaypointManager()->notifyOfChange(wp); } } @@ -627,6 +696,7 @@ void MapWidget::captureGeometryEndDrag(Geometry* geom, QPointF coordinate) MapWidget::~MapWidget() { + delete mc; delete m_ui; } /** @@ -640,46 +710,61 @@ void MapWidget::addUAS(UASInterface* uas) connect(uas, SIGNAL(systemSpecsChanged(int)), this, SLOT(updateSystemSpecs(int))); } +/** + * Update the whole list of waypoints. This is e.g. necessary if the list order changed. + * The UAS manager will emit the appropriate signal whenever updating the list + * is necessary. + */ void MapWidget::updateWaypointList(int uas) { - // Get already existing waypoints - UASInterface* uasInstance = UASManager::instance()->getUASForId(uas); - if (uasInstance) + if (mc) { - // Get update rect of old content - QRect updateRect = waypointPath->boundingBox().toRect(); + // Get already existing waypoints + UASInterface* uasInstance = UASManager::instance()->getUASForId(uas); + if (uasInstance) + { + // Get update rect of old content, this is what will be redrawn + // in the last step + QRect updateRect = waypointPath->boundingBox().toRect(); - QVector wpList = uasInstance->getWaypointManager()->getWaypointList(); + // Get all waypoints, including non-global waypoints + QVector wpList = uasInstance->getWaypointManager()->getWaypointList(); - // Clear if necessary - if (wpList.count() == 0) - { - clearWaypoints(uas); - return; - } + // Clear if necessary + if (wpList.count() == 0) + { + clearWaypoints(uas); + return; + } - // Load all existing waypoints into map view - foreach (Waypoint* wp, wpList) - { - // Block updates, since we update everything in the next step - updateWaypoint(mav->getUASID(), wp, false); - } + // Trim internal list to number of global waypoints in the waypoint manager list + int overSize = waypointPath->points().count() - uasInstance->getWaypointManager()->getGlobalFrameCount(); + if (overSize > 0) + { + // Remove n waypoints at the end of the list + // the remaining waypoints will be updated + // in the next step + for (int i = 0; i < overSize; ++i) + { + wps.removeLast(); + mc->layer("Waypoints")->removeGeometry(wpIcons.last()); + wpIcons.removeLast(); + waypointPath->points().removeLast(); + } + } - // Delete now unused wps - if (waypointPath->points().count() > wpList.count()) - { - int overSize = waypointPath->points().count() - wpList.count(); - for (int i = 0; i < overSize; ++i) + // Load all existing waypoints into map view + foreach (Waypoint* wp, wpList) { - wps.removeLast(); - mc->layer("Waypoints")->removeGeometry(wpIcons.last()); - wpIcons.removeLast(); - waypointPath->points().removeLast(); + // Block map draw updates, since we update everything in the next step + // but update internal data structures. + // Please note that updateWaypoint() ignores non-global waypoints + updateWaypoint(mav->getUASID(), wp, false); } - } - // Update view - if (isVisible()) mc->updateRequest(updateRect); + // Update view + if (isVisible()) mc->updateRequest(updateRect); + } } } @@ -711,7 +796,7 @@ void MapWidget::activeUASSet(UASInterface* uas) disconnect(this, SIGNAL(waypointCreated(Waypoint*)), mav->getWaypointManager(), SLOT(addWaypoint(Waypoint*))); } - if (uas) + if (uas && mc) { mav = uas; QColor color = mav->getColor(); @@ -739,27 +824,33 @@ void MapWidget::activeUASSet(UASInterface* uas) void MapWidget::updateSystemSpecs(int uas) { - foreach (qmapcontrol::Point* p, uasIcons.values()) + if (mc) { - MAV2DIcon* icon = dynamic_cast(p); - if (icon && icon->getUASId() == uas) + foreach (qmapcontrol::Point* p, uasIcons.values()) { - // Set new airframe - icon->setAirframe(UASManager::instance()->getUASForId(uas)->getAirframe()); - icon->drawIcon(); + MAV2DIcon* icon = dynamic_cast(p); + if (icon && icon->getUASId() == uas) + { + // Set new airframe + icon->setAirframe(UASManager::instance()->getUASForId(uas)->getAirframe()); + icon->drawIcon(); + } } } } void MapWidget::updateSelectedSystem(int uas) { - foreach (qmapcontrol::Point* p, uasIcons.values()) + if (mc) { - MAV2DIcon* icon = dynamic_cast(p); - if (icon) + foreach (qmapcontrol::Point* p, uasIcons.values()) { - // Set as selected if ids match - icon->setSelectedUAS((icon->getUASId() == uas)); + MAV2DIcon* icon = dynamic_cast(p); + if (icon) + { + // Set as selected if ids match + icon->setSelectedUAS((icon->getUASId() == uas)); + } } } } @@ -769,13 +860,16 @@ void MapWidget::updateAttitude(UASInterface* uas, double roll, double pitch, dou Q_UNUSED(roll); Q_UNUSED(pitch); Q_UNUSED(usec); - - if (uas) + if (mc) { - MAV2DIcon* icon = dynamic_cast(uasIcons.value(uas->getUASID(), NULL)); - if (icon) + + if (uas) { - icon->setYaw(yaw); + MAV2DIcon* icon = dynamic_cast(uasIcons.value(uas->getUASID(), NULL)); + if (icon) + { + icon->setYaw(yaw); + } } } } @@ -793,79 +887,81 @@ void MapWidget::updateGlobalPosition(UASInterface* uas, double lat, double lon, { Q_UNUSED(usec); Q_UNUSED(alt); // FIXME Use altitude - - // create a LineString - //QList points; - // Points with a circle - // A QPen can be used to customize the - //pointpen->setWidth(3); - //points.append(new CirclePoint(lat, lon, 10, uas->getUASName(), Point::Middle, pointpen)); - - qmapcontrol::Point* p; - QPointF coordinate; - coordinate.setX(lat); - coordinate.setY(lon); - - if (!uasIcons.contains(uas->getUASID())) + if (mc) { - // Get the UAS color - QColor uasColor = uas->getColor(); - - // Icon - //QPen* pointpen = new QPen(uasColor); - qDebug() << "2D MAP: ADDING" << uas->getUASName() << __FILE__ << __LINE__; - p = new MAV2DIcon(uas, 50, uas->getSystemType(), uas->getColor(), QString("%1").arg(uas->getUASID()), qmapcontrol::Point::Middle); - uasIcons.insert(uas->getUASID(), p); - mc->layer("Waypoints")->addGeometry(p); - - // Line - // A QPen also can use transparency - - // QList points; - // points.append(new qmapcontrol::Point(coordinate.x(), coordinate.y())); - // QPen* linepen = new QPen(uasColor.darker()); - // linepen->setWidth(2); - - // // Create tracking line string - // qmapcontrol::LineString* ls = new qmapcontrol::LineString(points, QString("%1").arg(uas->getUASID()), linepen); - // uasTrails.insert(uas->getUASID(), ls); - - // // Add the LineString to the layer - // mc->layer("Waypoints")->addGeometry(ls); - } - else - { - // p = dynamic_cast(uasIcons.value(uas->getUASID())); - // if (p) - // { - p = uasIcons.value(uas->getUASID()); - p->setCoordinate(QPointF(lat, lon)); - //p->setYaw(uas->getYaw()); - // } - // Extend trail - // uasTrails.value(uas->getUASID())->addPoint(new qmapcontrol::Point(coordinate.x(), coordinate.y())); - } + // create a LineString + //QList points; + // Points with a circle + // A QPen can be used to customize the + //pointpen->setWidth(3); + //points.append(new CirclePoint(lat, lon, 10, uas->getUASName(), Point::Middle, pointpen)); + + qmapcontrol::Point* p; + QPointF coordinate; + coordinate.setX(lon); + coordinate.setY(lat); + + if (!uasIcons.contains(uas->getUASID())) + { + // Get the UAS color + QColor uasColor = uas->getColor(); + + // Icon + //QPen* pointpen = new QPen(uasColor); + qDebug() << "2D MAP: ADDING" << uas->getUASName() << __FILE__ << __LINE__; + p = new MAV2DIcon(uas, 68, uas->getSystemType(), uas->getColor(), QString("%1").arg(uas->getUASID()), qmapcontrol::Point::Middle); + uasIcons.insert(uas->getUASID(), p); + mc->layer("Waypoints")->addGeometry(p); + + // Line + // A QPen also can use transparency + + // QList points; + // points.append(new qmapcontrol::Point(coordinate.x(), coordinate.y())); + // QPen* linepen = new QPen(uasColor.darker()); + // linepen->setWidth(2); + + // // Create tracking line string + // qmapcontrol::LineString* ls = new qmapcontrol::LineString(points, QString("%1").arg(uas->getUASID()), linepen); + // uasTrails.insert(uas->getUASID(), ls); + + // // Add the LineString to the layer + // mc->layer("Waypoints")->addGeometry(ls); + } + else + { + // p = dynamic_cast(uasIcons.value(uas->getUASID())); + // if (p) + // { + p = uasIcons.value(uas->getUASID()); + p->setCoordinate(QPointF(lon, lat)); + //p->setYaw(uas->getYaw()); + // } + // Extend trail + // uasTrails.value(uas->getUASID())->addPoint(new qmapcontrol::Point(coordinate.x(), coordinate.y())); + } - if (isVisible()) mc->updateRequest(p->boundingBox().toRect()); + if (isVisible()) mc->updateRequest(p->boundingBox().toRect()); - //if (isVisible()) mc->updateRequestNew();//(uasTrails.value(uas->getUASID())->boundingBox().toRect()); + //if (isVisible()) mc->updateRequestNew();//(uasTrails.value(uas->getUASID())->boundingBox().toRect()); - if (this->mav && uas->getUASID() == this->mav->getUASID()) - { - // Limit the position update rate - quint64 currTime = MG::TIME::getGroundTimeNow(); - if (currTime - lastUpdate > 120) + if (this->mav && uas->getUASID() == this->mav->getUASID()) { - lastUpdate = currTime; - // Sets the view to the interesting area - if (followgps->isChecked()) + // Limit the position update rate + quint64 currTime = MG::TIME::getGroundTimeNow(); + if (currTime - lastUpdate > 120) { - updatePosition(0, lat, lon); - } - else - { - // Refresh the screen - //if (isVisible()) mc->updateRequestNew(); + lastUpdate = currTime; + // Sets the view to the interesting area + if (followgps->isChecked()) + { + updatePosition(0, lon, lat); + } + else + { + // Refresh the screen + //if (isVisible()) mc->updateRequestNew(); + } } } } @@ -878,69 +974,96 @@ void MapWidget::updatePosition(float time, double lat, double lon) { Q_UNUSED(time); //gpsposition->setText(QString::number(time) + " / " + QString::number(lat) + " / " + QString::number(lon)); - if (followgps->isChecked() && isVisible()) + if (followgps->isChecked() && isVisible() && mc) { - mc->setView(QPointF(lat, lon)); + if (mc) mc->setView(QPointF(lat, lon)); } } void MapWidget::wheelEvent(QWheelEvent *event) { - int numDegrees = event->delta() / 8; - int numSteps = numDegrees / 15; - // Calculate new zoom level - int newZoom = mc->currentZoom()+numSteps; - // Set new zoom level, level is bounded by map control - mc->setZoom(newZoom); - // Detail zoom level is the number of steps zoomed in further - // after the bounding has taken effect - detailZoom = qAbs(qMin(0, mc->currentZoom()-newZoom)); - - // visual field of camera - updateCameraPosition(20*newZoom,0,"no"); - + if (mc) + { + int numDegrees = event->delta() / 8; + int numSteps = numDegrees / 15; + // Calculate new zoom level + int newZoom = mc->currentZoom()+numSteps; + // Set new zoom level, level is bounded by map control + mc->setZoom(newZoom); + // Detail zoom level is the number of steps zoomed in further + // after the bounding has taken effect + detailZoom = qAbs(qMin(0, mc->currentZoom()-newZoom)); + + // visual field of camera + updateCameraPosition(20*newZoom,0,"no"); + } } void MapWidget::keyPressEvent(QKeyEvent *event) { - switch (event->key()) { - case Qt::Key_Plus: - mc->zoomIn(); - break; - case Qt::Key_Minus: - mc->zoomOut(); - break; - case Qt::Key_Left: - mc->scrollLeft(this->width()/scrollStep); - break; - case Qt::Key_Right: - mc->scrollRight(this->width()/scrollStep); - break; - case Qt::Key_Down: - mc->scrollDown(this->width()/scrollStep); - break; - case Qt::Key_Up: - mc->scrollUp(this->width()/scrollStep); - break; - default: - QWidget::keyPressEvent(event); + if (mc) + { + switch (event->key()) { + case Qt::Key_Plus: + mc->zoomIn(); + break; + case Qt::Key_Minus: + mc->zoomOut(); + break; + case Qt::Key_Left: + mc->scrollLeft(this->width()/scrollStep); + break; + case Qt::Key_Right: + mc->scrollRight(this->width()/scrollStep); + break; + case Qt::Key_Down: + mc->scrollDown(this->width()/scrollStep); + break; + case Qt::Key_Up: + mc->scrollUp(this->width()/scrollStep); + break; + default: + QWidget::keyPressEvent(event); + } } } void MapWidget::resizeEvent(QResizeEvent* event ) { Q_UNUSED(event); - mc->resize(this->size()); + if (!initialized) + { + init(); + } + if (mc) mc->resize(this->size()); } void MapWidget::showEvent(QShowEvent* event) { Q_UNUSED(event); +// if (isVisible()) +// { +// if (!initialized) +// { +// init(); +// } +// } } void MapWidget::hideEvent(QHideEvent* event) { Q_UNUSED(event); + if (mc) + { + QSettings settings; + settings.beginGroup("QGC_MAPWIDGET"); + QPointF currentPos = mc->currentCoordinate(); + settings.setValue("LAST_LATITUDE", currentPos.y()); + settings.setValue("LAST_LONGITUDE", currentPos.x()); + settings.setValue("LAST_ZOOM", mc->currentZoom()); + settings.endGroup(); + settings.sync(); + } } @@ -958,98 +1081,103 @@ void MapWidget::changeEvent(QEvent *e) void MapWidget::clearWaypoints(int uas) { - Q_UNUSED(uas); - // Clear the previous WP track - - //mc->layer("Waypoints")->clearGeometries(); - wps.clear(); - foreach (Point* p, wpIcons) + if (mc) { - mc->layer("Waypoints")->removeGeometry(p); - } - wpIcons.clear(); + Q_UNUSED(uas); + // Clear the previous WP track - // Get bounding box of this object BEFORE deleting the content - QRect box = waypointPath->boundingBox().toRect(); + //mc->layer("Waypoints")->clearGeometries(); + wps.clear(); + foreach (Point* p, wpIcons) + { + mc->layer("Waypoints")->removeGeometry(p); + } + wpIcons.clear(); - // Delete the content - waypointPath->points().clear(); + // Get bounding box of this object BEFORE deleting the content + QRect box = waypointPath->boundingBox().toRect(); - //delete waypointPath; - //waypointPath = new - //mc->layer("Waypoints")->addGeometry(waypointPath); - //wpIndex.clear(); - if (isVisible()) mc->updateRequest(box);//(waypointPath->boundingBox().toRect()); + // Delete the content + waypointPath->points().clear(); - if(createPath->isChecked()) - { - createPath->click(); - } + //delete waypointPath; + //waypointPath = new + //mc->layer("Waypoints")->addGeometry(waypointPath); + //wpIndex.clear(); + if (isVisible()) mc->updateRequest(box);//(waypointPath->boundingBox().toRect()); - qDebug() << "CLEARING WAYPOINTS"; + if(createPath->isChecked()) + { + createPath->click(); + } + } } void MapWidget::clearPath(int uas) { Q_UNUSED(uas); - mc->layer("Tracking")->clearGeometries(); - foreach (qmapcontrol::LineString* ls, uasTrails) + if (mc) { - QPen* linepen = ls->pen(); - delete ls; - qmapcontrol::LineString* lsNew = new qmapcontrol::LineString(QList(), "", linepen); - mc->layer("Tracking")->addGeometry(lsNew); + mc->layer("Tracking")->clearGeometries(); + foreach (qmapcontrol::LineString* ls, uasTrails) + { + QPen* linepen = ls->pen(); + delete ls; + qmapcontrol::LineString* lsNew = new qmapcontrol::LineString(QList(), "", linepen); + mc->layer("Tracking")->addGeometry(lsNew); + } + // FIXME update this with update request only for bounding box of trails + if (isVisible()) mc->updateRequestNew();//(QRect(0, 0, width(), height())); } - // FIXME update this with update request only for bounding box of trails - if (isVisible()) mc->updateRequestNew();//(QRect(0, 0, width(), height())); } void MapWidget::updateCameraPosition(double radio, double bearing, QString dir) { Q_UNUSED(dir); Q_UNUSED(bearing); - // FIXME Mariano - //camPoints.clear(); - QPointF currentPos = mc->currentCoordinate(); - // QPointF actualPos = getPointxBearing_Range(currentPos.y(),currentPos.x(),bearing,distance); + if (mc) + { + // FIXME Mariano + //camPoints.clear(); + QPointF currentPos = mc->currentCoordinate(); + // QPointF actualPos = getPointxBearing_Range(currentPos.y(),currentPos.x(),bearing,distance); - // qmapcontrol::Point* tempPoint1 = new qmapcontrol::Point(currentPos.x(), currentPos.y(),"inicial",qmapcontrol::Point::Middle); - // qmapcontrol::Point* tempPoint2 = new qmapcontrol::Point(actualPos.x(), actualPos.y(),"final",qmapcontrol::Point::Middle); + // qmapcontrol::Point* tempPoint1 = new qmapcontrol::Point(currentPos.x(), currentPos.y(),"inicial",qmapcontrol::Point::Middle); + // qmapcontrol::Point* tempPoint2 = new qmapcontrol::Point(actualPos.x(), actualPos.y(),"final",qmapcontrol::Point::Middle); - // camPoints.append(tempPoint1); - // camPoints.append(tempPoint2); + // camPoints.append(tempPoint1); + // camPoints.append(tempPoint2); - // camLine->setPoints(camPoints); + // camLine->setPoints(camPoints); - QPen* camBorderPen = new QPen(QColor(255,0,0)); - camBorderPen->setWidth(2); + QPen* camBorderPen = new QPen(QColor(255,0,0)); + camBorderPen->setWidth(2); - //radio = mc->currentZoom() + //radio = mc->currentZoom() - if(drawCamBorder) - { - //clear camera borders - mc->layer("Camera")->clearGeometries(); + if(drawCamBorder) + { + //clear camera borders + mc->layer("Camera")->clearGeometries(); - //create a camera borders - qmapcontrol::CirclePoint* camBorder = new qmapcontrol::CirclePoint(currentPos.x(), currentPos.y(), radio, "camBorder", qmapcontrol::Point::Middle, camBorderPen); + //create a camera borders + qmapcontrol::CirclePoint* camBorder = new qmapcontrol::CirclePoint(currentPos.x(), currentPos.y(), radio, "camBorder", qmapcontrol::Point::Middle, camBorderPen); - //camBorder->setCoordinate(currentPos); + //camBorder->setCoordinate(currentPos); - mc->layer("Camera")->addGeometry(camBorder); - // mc->layer("Camera")->addGeometry(camLine); - if (isVisible()) mc->updateRequestNew(); + mc->layer("Camera")->addGeometry(camBorder); + // mc->layer("Camera")->addGeometry(camLine); + if (isVisible()) mc->updateRequestNew(); - } - else - { - //clear camera borders - mc->layer("Camera")->clearGeometries(); - if (isVisible()) mc->updateRequestNew(); + } + else + { + //clear camera borders + mc->layer("Camera")->clearGeometries(); + if (isVisible()) mc->updateRequestNew(); + } } - - } void MapWidget::drawBorderCamAtMap(bool status) diff --git a/src/ui/MapWidget.h b/src/ui/MapWidget.h index 1490e593fbbf6c5eabeafc203d3a90a472594805..fd8608b54670813296d359d99390d5be30dc487b 100644 --- a/src/ui/MapWidget.h +++ b/src/ui/MapWidget.h @@ -146,6 +146,10 @@ protected: //QMap waypointPaths; UASInterface* mav; quint64 lastUpdate; + bool initialized; + + /** @brief Initialize view */ + void init(); protected slots: void captureMapClick (const QMouseEvent* event, const QPointF coordinate); diff --git a/src/ui/ParameterInterface.cc b/src/ui/ParameterInterface.cc index 5a0f194ebf77684eb27ee9ae2c0464e0da01d209..8d435449c9927ab08967a3f3c22921aa26ac4a78 100644 --- a/src/ui/ParameterInterface.cc +++ b/src/ui/ParameterInterface.cc @@ -44,12 +44,6 @@ ParameterInterface::ParameterInterface(QWidget *parent) : m_ui(new Ui::parameterWidget) { m_ui->setupUi(this); - // Make sure the combo box is empty - // because else indices get messed up - //m_ui->vehicleComboBox->clear(); - - // Setup UI connections - //connect(m_ui->vehicleComboBox, SIGNAL(activated(int)), this, SLOT(selectUAS(int))); // Get current MAV list QList systems = UASManager::instance()->getUASList(); @@ -84,8 +78,6 @@ void ParameterInterface::selectUAS(int index) */ void ParameterInterface::addUAS(UASInterface* uas) { - //m_ui->vehicleComboBox->addItem(uas->getUASName()); - QGCParamWidget* param = new QGCParamWidget(uas, this); paramWidgets->insert(uas->getUASID(), param); m_ui->stackedWidget->addWidget(param); diff --git a/src/ui/QGCParamWidget.cc b/src/ui/QGCParamWidget.cc index 7fef09c2b92995fc31440a23bfcccb920769cbfe..d28933862feb89160dd47c861498812d3cef3797 100644 --- a/src/ui/QGCParamWidget.cc +++ b/src/ui/QGCParamWidget.cc @@ -30,6 +30,8 @@ This file is part of the QGROUNDCONTROL project #include #include #include +#include +#include #include "QGCParamWidget.h" #include "UASInterface.h" @@ -46,10 +48,21 @@ QGCParamWidget::QGCParamWidget(UASInterface* uas, QWidget *parent) : components(new QMap()), paramGroups(), changedValues(), - parameters() + parameters(), + transmissionListMode(false), + transmissionActive(false), + transmissionTimeout(0), + retransmissionTimeout(350), + rewriteTimeout(500), + retransmissionBurstRequestSize(2) { + // Load settings + loadSettings(); + // Create tree widget tree = new QTreeWidget(this); + statusLabel = new QLabel(); + statusLabel->setAutoFillBackground(true); tree->setColumnWidth(0, 150); // Set tree widget as widget onto this component @@ -60,42 +73,50 @@ QGCParamWidget::QGCParamWidget(UASInterface* uas, QWidget *parent) : horizontalLayout->setMargin(0); horizontalLayout->setSizeConstraint(QLayout::SetMinimumSize); + // Parameter tree horizontalLayout->addWidget(tree, 0, 0, 1, 3); + + // Status line + statusLabel->setText(tr("Click refresh to download parameters")); + horizontalLayout->addWidget(statusLabel, 1, 0, 1, 3); + + + // BUTTONS QPushButton* refreshButton = new QPushButton(tr("Refresh")); refreshButton->setToolTip(tr("Load parameters currently in non-permanent memory of aircraft.")); refreshButton->setWhatsThis(tr("Load parameters currently in non-permanent memory of aircraft.")); connect(refreshButton, SIGNAL(clicked()), this, SLOT(requestParameterList())); - horizontalLayout->addWidget(refreshButton, 1, 0); + horizontalLayout->addWidget(refreshButton, 2, 0); QPushButton* setButton = new QPushButton(tr("Transmit")); setButton->setToolTip(tr("Set current parameters in non-permanent onboard memory")); setButton->setWhatsThis(tr("Set current parameters in non-permanent onboard memory")); connect(setButton, SIGNAL(clicked()), this, SLOT(setParameters())); - horizontalLayout->addWidget(setButton, 1, 1); + horizontalLayout->addWidget(setButton, 2, 1); QPushButton* writeButton = new QPushButton(tr("Write (ROM)")); writeButton->setToolTip(tr("Copy current parameters in non-permanent memory of the aircraft to permanent memory. Transmit your parameters first to write these.")); writeButton->setWhatsThis(tr("Copy current parameters in non-permanent memory of the aircraft to permanent memory. Transmit your parameters first to write these.")); connect(writeButton, SIGNAL(clicked()), this, SLOT(writeParameters())); - horizontalLayout->addWidget(writeButton, 1, 2); - - QPushButton* readButton = new QPushButton(tr("Read (ROM)")); - readButton->setToolTip(tr("Copy parameters from permanent memory to non-permanent current memory of aircraft. DOES NOT update the parameters in this view, click refresh after copying them to get them.")); - readButton->setWhatsThis(tr("Copy parameters from permanent memory to non-permanent current memory of aircraft. DOES NOT update the parameters in this view, click refresh after copying them to get them.")); - connect(readButton, SIGNAL(clicked()), this, SLOT(readParameters())); - horizontalLayout->addWidget(readButton, 2, 2); + horizontalLayout->addWidget(writeButton, 2, 2); QPushButton* loadFileButton = new QPushButton(tr("Load File")); loadFileButton->setToolTip(tr("Load parameters from a file on this computer in the view. To write them to the aircraft, use transmit after loading them.")); loadFileButton->setWhatsThis(tr("Load parameters from a file on this computer in the view. To write them to the aircraft, use transmit after loading them.")); connect(loadFileButton, SIGNAL(clicked()), this, SLOT(loadParameters())); - horizontalLayout->addWidget(loadFileButton, 2, 0); + horizontalLayout->addWidget(loadFileButton, 3, 0); QPushButton* saveFileButton = new QPushButton(tr("Save File")); saveFileButton->setToolTip(tr("Save parameters in this view to a file on this computer.")); saveFileButton->setWhatsThis(tr("Save parameters in this view to a file on this computer.")); connect(saveFileButton, SIGNAL(clicked()), this, SLOT(saveParameters())); - horizontalLayout->addWidget(saveFileButton, 2, 1); + horizontalLayout->addWidget(saveFileButton, 3, 1); + + QPushButton* readButton = new QPushButton(tr("Read (ROM)")); + readButton->setToolTip(tr("Copy parameters from permanent memory to non-permanent current memory of aircraft. DOES NOT update the parameters in this view, click refresh after copying them to get them.")); + readButton->setWhatsThis(tr("Copy parameters from permanent memory to non-permanent current memory of aircraft. DOES NOT update the parameters in this view, click refresh after copying them to get them.")); + connect(readButton, SIGNAL(clicked()), this, SLOT(readParameters())); + horizontalLayout->addWidget(readButton, 3, 2); // Set layout this->setLayout(horizontalLayout); @@ -113,7 +134,23 @@ QGCParamWidget::QGCParamWidget(UASInterface* uas, QWidget *parent) : connect(tree, SIGNAL(itemChanged(QTreeWidgetItem*,int)), this, SLOT(parameterItemChanged(QTreeWidgetItem*,int))); // New parameters from UAS - connect(uas, SIGNAL(parameterChanged(int,int,QString,float)), this, SLOT(addParameter(int,int,QString,float))); + connect(uas, SIGNAL(parameterChanged(int,int,int,int,QString,float)), this, SLOT(addParameter(int,int,int,int,QString,float))); + + // Connect retransmission guard + connect(this, SIGNAL(requestParameter(int,int)), uas, SLOT(requestParameter(int,int))); + connect(&retransmissionTimer, SIGNAL(timeout()), this, SLOT(retransmissionGuardTick())); +} + +void QGCParamWidget::loadSettings() +{ + QSettings settings; + settings.beginGroup("QGC_MAVLINK_PROTOCOL"); + bool ok; + int temp = settings.value("PARAMETER_RETRANSMISSION_TIMEOUT", retransmissionTimeout).toInt(&ok); + if (ok) retransmissionTimeout = temp; + temp = settings.value("PARAMETER_REWRITE_TIMEOUT", rewriteTimeout).toInt(&ok); + if (ok) rewriteTimeout = temp; + settings.endGroup(); } /** @@ -165,6 +202,140 @@ void QGCParamWidget::addComponent(int uas, int component, QString componentName) } } +/** + * @param uas System which has the component + * @param component id of the component + * @param parameterName human friendly name of the parameter + */ +void QGCParamWidget::addParameter(int uas, int component, int paramCount, int paramId, QString parameterName, float value) +{ + addParameter(uas, component, parameterName, value); + + // Missing packets list has to be instantiated for all components + if (!transmissionMissingPackets.contains(component)) + { + transmissionMissingPackets.insert(component, new QList()); + } + + // List mode is different from single parameter transfers + if (transmissionListMode) + { + // Only accept the list size once on the first packet from + // each component + if (!transmissionListSizeKnown.contains(component)) + { + // Mark list size as known + transmissionListSizeKnown.insert(component, true); + + // Mark all parameters as missing + for (int i = 0; i < paramCount; ++i) + { + if (!transmissionMissingPackets.value(component)->contains(i)) + { + transmissionMissingPackets.value(component)->append(i); + } + } + + // There is only one transmission timeout for all components + // since components do not manage their transmission, + // the longest timeout is safe for all components. + quint64 thisTransmissionTimeout = QGC::groundTimeMilliseconds() + ((paramCount/retransmissionBurstRequestSize+5)*retransmissionTimeout); + if (thisTransmissionTimeout > transmissionTimeout) + { + transmissionTimeout = thisTransmissionTimeout; + } + } + + // Start retransmission guard + // or reset timer + setRetransmissionGuardEnabled(true); + } + + // Mark this parameter as received in read list + int index = transmissionMissingPackets.value(component)->indexOf(paramId); + // If the MAV sent the parameter without request, it wont be in missing list + if (index != -1) transmissionMissingPackets.value(component)->removeAt(index); + + bool justWritten = false; + bool writeMismatch = false; + //bool lastWritten = false; + // Mark this parameter as received in write ACK list + QMap* map = transmissionMissingWriteAckPackets.value(component); + if (map && map->contains(parameterName)) + { + justWritten = true; + if (map->value(parameterName) != value) + { + writeMismatch = true; + } + map->remove(parameterName); + } + + int missCount = 0; + foreach (int key, transmissionMissingPackets.keys()) + { + missCount += transmissionMissingPackets.value(key)->count(); + } + + int missWriteCount = 0; + foreach (int key, transmissionMissingWriteAckPackets.keys()) + { + missWriteCount += transmissionMissingWriteAckPackets.value(key)->count(); + } + + if (justWritten && !writeMismatch && missWriteCount == 0) + { + // Just wrote one and count went to 0 - this was the last missing write parameter + statusLabel->setText(tr("SUCCESS: WROTE ALL PARAMETERS")); + QPalette pal = statusLabel->palette(); + pal.setColor(backgroundRole(), QGC::colorGreen); + statusLabel->setPalette(pal); + } + else if (justWritten && !writeMismatch) + { + statusLabel->setText(tr("SUCCESS: Wrote %2 (#%1/%4): %3").arg(paramId+1).arg(parameterName).arg(value).arg(paramCount)); + QPalette pal = statusLabel->palette(); + pal.setColor(backgroundRole(), QGC::colorGreen); + statusLabel->setPalette(pal); + } + else if (justWritten && writeMismatch) + { + // Mismatch, tell user + QPalette pal = statusLabel->palette(); + pal.setColor(backgroundRole(), QGC::colorRed); + statusLabel->setPalette(pal); + statusLabel->setText(tr("FAILURE: Wrote %1: sent %2 != onboard %3").arg(parameterName).arg(map->value(parameterName)).arg(value)); + } + else + { + if (missCount > 0) + { + QPalette pal = statusLabel->palette(); + pal.setColor(backgroundRole(), QGC::colorOrange); + statusLabel->setPalette(pal); + } + else + { + QPalette pal = statusLabel->palette(); + pal.setColor(backgroundRole(), QGC::colorGreen); + statusLabel->setPalette(pal); + } + statusLabel->setText(tr("Got %2 (#%1/%5): %3 (%4 missing)").arg(paramId+1).arg(parameterName).arg(value).arg(missCount).arg(paramCount)); + } + + // Check if last parameter was received + if (missCount == 0 && missWriteCount == 0) + { + this->transmissionActive = false; + this->transmissionListMode = false; + transmissionListSizeKnown.clear(); + foreach (int key, transmissionMissingPackets.keys()) + { + transmissionMissingPackets.value(key)->clear(); + } + } +} + /** * @param uas System which has the component * @param component id of the component @@ -179,7 +350,21 @@ void QGCParamWidget::addParameter(int uas, int component, QString parameterName, // Get component if (!components->contains(component)) { - addComponent(uas, component, "Component #" + QString::number(component)); + QString componentName; + switch (component) + { + case MAV_COMP_ID_CAMERA: + componentName = tr("Camera (#%1)").arg(component); + break; + case MAV_COMP_ID_IMU: + componentName = tr("IMU (#%1)").arg(component); + break; + default: + componentName = tr("Component #").arg(component); + break; + } + + addComponent(uas, component, componentName); } // Replace value in map @@ -227,7 +412,9 @@ void QGCParamWidget::addParameter(int uas, int component, QString parameterName, QStringList plist; plist.append(parameterName); plist.append(QString::number(value)); + // CREATE PARAMETER ITEM parameterItem = new QTreeWidgetItem(plist); + // CONFIGURE PARAMETER ITEM compParamGroups->value(parent)->addChild(parameterItem); parameterItem->setFlags(parameterItem->flags() | Qt::ItemIsEditable); @@ -256,7 +443,9 @@ void QGCParamWidget::addParameter(int uas, int component, QString parameterName, QStringList plist; plist.append(parameterName); plist.append(QString::number(value)); + // CREATE PARAMETER ITEM parameterItem = new QTreeWidgetItem(plist); + // CONFIGURE PARAMETER ITEM components->value(component)->addChild(parameterItem); parameterItem->setFlags(parameterItem->flags() | Qt::ItemIsEditable); @@ -276,10 +465,32 @@ void QGCParamWidget::addParameter(int uas, int component, QString parameterName, */ void QGCParamWidget::requestParameterList() { + // FIXME This call does not belong here + // Once the comm handling is moved to a new + // Param manager class the settings can be directly + // loaded from MAVLink protocol + loadSettings(); + // End of FIXME + // Clear view and request param list clear(); parameters.clear(); received.clear(); + // Clear transmission state + transmissionListMode = true; + transmissionListSizeKnown.clear(); + foreach (int key, transmissionMissingPackets.keys()) + { + transmissionMissingPackets.value(key)->clear(); + } + transmissionActive = true; + + // Set status text + statusLabel->setText(tr("Requested param list.. waiting")); + + // Request twice as mean of forward error correction + mav->requestParameters(); + QGC::SLEEP::msleep(10); mav->requestParameters(); } @@ -304,12 +515,13 @@ void QGCParamWidget::parameterItemChanged(QTreeWidgetItem* current, int column) bool ok; QString str = current->data(0, Qt::DisplayRole).toString(); float value = current->data(1, Qt::DisplayRole).toDouble(&ok); - // Send parameter to MAV + // Set parameter on changed list to be transmitted to MAV if (ok) { if (ok) { - qDebug() << "PARAM CHANGED: COMP:" << key << "KEY:" << str << "VALUE:" << value; + statusLabel->setText(tr("Changed Param %1:%2: %3").arg(key).arg(str).arg(value)); + //qDebug() << "PARAM CHANGED: COMP:" << key << "KEY:" << str << "VALUE:" << value; // Changed values list if (map->contains(str)) map->remove(str); map->insert(str, value); @@ -317,8 +529,8 @@ void QGCParamWidget::parameterItemChanged(QTreeWidgetItem* current, int column) // Check if the value was numerically changed if (!parameters.value(key)->contains(str) || parameters.value(key)->value(str, 0.0f) != value) { - current->setBackground(0, QBrush(QColor(QGC::colorGreen))); - current->setBackground(1, QBrush(QColor(QGC::colorGreen))); + current->setBackground(0, QBrush(QColor(QGC::colorOrange))); + current->setBackground(1, QBrush(QColor(QGC::colorOrange))); } // All parameters list @@ -356,7 +568,9 @@ void QGCParamWidget::saveParameters() QMap::iterator j; for (j = comp->begin(); j != comp->end(); ++j) { - in << mav->getUASID() << "\t" << compid << "\t" << j.key() << "\t" << j.value() << "\n"; + QString paramValue("%1"); + paramValue = paramValue.arg(j.value(), 25, 'g', 12); + in << mav->getUASID() << "\t" << compid << "\t" << j.key() << "\t" << paramValue << "\n"; in.flush(); } } @@ -428,6 +642,119 @@ void QGCParamWidget::loadParameters() } +/** + * Enabling the retransmission guard enables the parameter widget to track + * dropped parameters and to re-request them. This works for both individual + * parameter reads as well for whole list requests. + * + * @param enabled True if retransmission checking should be enabled, false else + */ +void QGCParamWidget::setRetransmissionGuardEnabled(bool enabled) +{ + if (enabled) + { + retransmissionTimer.start(retransmissionTimeout); + } + else + { + retransmissionTimer.stop(); + } +} + +void QGCParamWidget::retransmissionGuardTick() +{ + if (transmissionActive) + { + qDebug() << __FILE__ << __LINE__ << "RETRANSMISSION GUARD ACTIVE, CHECKING FOR DROPS.."; + + // Check for timeout + // stop retransmission attempts on timeout + if (QGC::groundTimeMilliseconds() > transmissionTimeout) + { + setRetransmissionGuardEnabled(false); + transmissionActive = false; + + // Empty read retransmission list + // Empty write retransmission list + int missingReadCount = 0; + QList readKeys = transmissionMissingPackets.keys(); + foreach (int component, readKeys) + { + missingReadCount += transmissionMissingPackets.value(component)->count(); + transmissionMissingPackets.value(component)->clear(); + } + + // Empty write retransmission list + int missingWriteCount = 0; + QList writeKeys = transmissionMissingWriteAckPackets.keys(); + foreach (int component, writeKeys) + { + missingWriteCount += transmissionMissingWriteAckPackets.value(component)->count(); + transmissionMissingWriteAckPackets.value(component)->clear(); + } + statusLabel->setText(tr("TIMEOUT! MISSING: %1 read, %2 write.").arg(missingReadCount).arg(missingWriteCount)); + } + + // Re-request at maximum retransmissionBurstRequestSize parameters at once + // to prevent link flooding + QMap*>::iterator i; + for (i = parameters.begin(); i != parameters.end(); ++i) + { + // Iterate through the parameters of the component + int component = i.key(); + // Request n parameters from this component (at maximum) + QList * paramList = transmissionMissingPackets.value(component, NULL); + if (paramList) + { + int count = 0; + foreach (int id, *paramList) + { + if (count < retransmissionBurstRequestSize) + { + qDebug() << __FILE__ << __LINE__ << "RETRANSMISSION GUARD REQUESTS RETRANSMISSION OF PARAM #" << id << "FROM COMPONENT #" << component; + emit requestParameter(component, id); + statusLabel->setText(tr("Requested retransmission of #%1").arg(id+1)); + count++; + } + else + { + break; + } + } + } + } + + // Re-request at maximum retransmissionBurstRequestSize parameters at once + // to prevent write-request link flooding + // Empty write retransmission list + QList writeKeys = transmissionMissingWriteAckPackets.keys(); + foreach (int component, writeKeys) + { + int count = 0; + QMap * missingParams = transmissionMissingWriteAckPackets.value(component); + foreach (QString key, missingParams->keys()) + { + if (count < retransmissionBurstRequestSize) + { + // Re-request write operation + emit parameterChanged(component, key, missingParams->value(key)); + statusLabel->setText(tr("Requested rewrite of: %1: %2").arg(key).arg(missingParams->value(key))); + count++; + } + else + { + break; + } + } + } + } + else + { + qDebug() << __FILE__ << __LINE__ << "STOPPING RETRANSMISSION GUARD GRACEFULLY"; + setRetransmissionGuardEnabled(false); + } +} + /** * @param component the subsystem which has the parameter @@ -437,7 +764,25 @@ void QGCParamWidget::loadParameters() void QGCParamWidget::setParameter(int component, QString parameterName, float value) { emit parameterChanged(component, parameterName, value); - qDebug() << "SENT PARAM" << parameterName << value; + // Wait for parameter to be written back + // mark it therefore as missing + if (!transmissionMissingWriteAckPackets.contains(component)) + { + transmissionMissingWriteAckPackets.insert(component, new QMap()); + } + + // Insert it in missing write ACK list + transmissionMissingWriteAckPackets.value(component)->insert(parameterName, value); + + // Set timeouts + transmissionActive = true; + quint64 newTransmissionTimeout = QGC::groundTimeMilliseconds() + 5*rewriteTimeout; + if (newTransmissionTimeout > transmissionTimeout) + { + transmissionTimeout = newTransmissionTimeout; + } + // Enable guard / reset timeouts + setRetransmissionGuardEnabled(true); } /** @@ -446,6 +791,7 @@ void QGCParamWidget::setParameter(int component, QString parameterName, float va void QGCParamWidget::setParameters() { // Iterate through all components, through all parameters and emit them + int parametersSent = 0; QMap*>::iterator i; for (i = changedValues.begin(); i != changedValues.end(); ++i) { @@ -457,12 +803,31 @@ void QGCParamWidget::setParameters() for (j = comp->begin(); j != comp->end(); ++j) { setParameter(compid, j.key(), j.value()); + parametersSent++; } } } + // Change transmission status if necessary + if (parametersSent == 0) + { + statusLabel->setText(tr("No transmission: No changed values.")); + } + else + { + statusLabel->setText(tr("Transmitting %1 parameters.").arg(parametersSent)); + // Set timeouts + transmissionActive = true; + quint64 newTransmissionTimeout = QGC::groundTimeMilliseconds() + (parametersSent/retransmissionBurstRequestSize+5)*rewriteTimeout; + if (newTransmissionTimeout > transmissionTimeout) + { + transmissionTimeout = newTransmissionTimeout; + } + // Enable guard + setRetransmissionGuardEnabled(true); + } + changedValues.clear(); - qDebug() << __FILE__ << __LINE__ << "SETTING ALL PARAMETERS"; } /** diff --git a/src/ui/QGCParamWidget.h b/src/ui/QGCParamWidget.h index cf38850baca1be3986d8eccb17e59643e208c79b..c12b0892554ae0b443cb9da9604aa956b45fd47f 100644 --- a/src/ui/QGCParamWidget.h +++ b/src/ui/QGCParamWidget.h @@ -34,6 +34,8 @@ This file is part of the QGROUNDCONTROL project #include #include #include +#include +#include #include "UASInterface.h" @@ -50,9 +52,13 @@ public: signals: /** @brief A parameter was changed in the widget, NOT onboard */ void parameterChanged(int component, QString parametername, float value); + /** @brief Request a single parameter */ + void requestParameter(int component, int parameter); public slots: /** @brief Add a component to the list */ void addComponent(int uas, int component, QString componentName); + /** @brief Add a parameter to the list with retransmission / safety checks */ + void addParameter(int uas, int component, int paramCount, int paramId, QString parameterName, float value); /** @brief Add a parameter to the list */ void addParameter(int uas, int component, QString parameterName, float value); /** @brief Request list of parameters from MAV */ @@ -74,15 +80,34 @@ public slots: void saveParameters(); /** @brief Load parameters from a file */ void loadParameters(); + + /** @brief Check for missing parameters */ + void retransmissionGuardTick(); + protected: - UASInterface* mav; ///< The MAV this widget is controlling - QTreeWidget* tree; ///< The parameter tree + UASInterface* mav; ///< The MAV this widget is controlling + QTreeWidget* tree; ///< The parameter tree + QLabel* statusLabel; ///< Parameter transmission label QMap* components; ///< The list of components QMap* > paramGroups; ///< Parameter groups QMap* > changedValues; ///< Changed values QMap* > parameters; ///< All parameters QVector received; ///< Successfully received parameters + QMap* > transmissionMissingPackets; ///< Missing packets + QMap* > transmissionMissingWriteAckPackets; ///< Missing write ACK packets + bool transmissionListMode; ///< Currently requesting list + QMap transmissionListSizeKnown; ///< List size initialized? + bool transmissionActive; ///< Missing packets, working on list? + quint64 transmissionTimeout; ///< Timeout + QTimer retransmissionTimer; ///< Timer handling parameter retransmission + int retransmissionTimeout; ///< Retransmission request timeout, in milliseconds + int rewriteTimeout; ///< Write request timeout, in milliseconds + int retransmissionBurstRequestSize; ///< Number of packets requested for retransmission per burst + /** @brief Activate / deactivate parameter retransmission */ + void setRetransmissionGuardEnabled(bool enabled); + /** @brief Load settings */ + void loadSettings(); }; #endif // QGCPARAMWIDGET_H diff --git a/src/ui/QGCRemoteControlView.cc b/src/ui/QGCRemoteControlView.cc index 519598ab2fba406020f7285509006fae1c60814c..f7a693bd3e03a8733734723a9219d3b2736c4c5b 100644 --- a/src/ui/QGCRemoteControlView.cc +++ b/src/ui/QGCRemoteControlView.cc @@ -97,7 +97,7 @@ void QGCRemoteControlView::setUASId(int id) { // The UAS exists, disconnect any existing connections disconnect(uas, SIGNAL(remoteControlChannelRawChanged(int,float,float)), this, SLOT(setChannel(int,float,float))); - disconnect(uas, SIGNAL(remoteControlRSSIRawChanged(float)), this, SLOT(setRemoteRSSI(float))); + disconnect(uas, SIGNAL(remoteControlRSSIChanged(float)), this, SLOT(setRemoteRSSI(float))); disconnect(uas, SIGNAL(radioCalibrationRawReceived(const QPointer)), calibrationWindow, SLOT(receive(const QPointer&))); disconnect(uas, SIGNAL(remoteControlChannelRawChanged(int,float)), calibrationWindow, SLOT(setChannelRaw(int,float))); disconnect(uas, SIGNAL(remoteControlChannelScaledChanged(int,float,float)), calibrationWindow, SLOT(setChannelScaled(int,float))); @@ -115,10 +115,8 @@ void QGCRemoteControlView::setUASId(int id) connect(newUAS, SIGNAL(remoteControlRSSIChanged(float)), this, SLOT(setRemoteRSSI(float))); connect(newUAS, SIGNAL(remoteControlChannelRawChanged(int,float)), this, SLOT(setChannelRaw(int,float))); -// connect(newUAS, SIGNAL(remoteControlChannelRawChanged(int,float)), calibrationWindow, SLOT(setChannelRaw(int,float))); - + connect(newUAS, SIGNAL(remoteControlChannelRawChanged(int,float)), calibrationWindow, SLOT(setChannel(int,float))); connect(newUAS, SIGNAL(remoteControlChannelScaledChanged(int,float)), this, SLOT(setChannelScaled(int,float))); -// connect(newUAS, SIGNAL(remoteControlChannelScaledChanged(int,float)), calibrationWindow, SLOT(setChannelScaled(int,float))); } } @@ -205,6 +203,8 @@ void QGCRemoteControlView::redraw() { progressBars.at(i)->setValue(normalized.at(i)*100.0f); } + // Update RSSI + rssiBar->setValue(rssi*100); updated = false; } } diff --git a/src/ui/QGCSensorSettingsWidget.cc b/src/ui/QGCSensorSettingsWidget.cc index 68b55acfc8158bb292ee8905e6d76af9b2557d6e..0083b9faa36dd360c14e22fe819f3f7f2e1f2b19 100644 --- a/src/ui/QGCSensorSettingsWidget.cc +++ b/src/ui/QGCSensorSettingsWidget.cc @@ -37,6 +37,7 @@ QGCSensorSettingsWidget::QGCSensorSettingsWidget(UASInterface* uas, QWidget *par ui(new Ui::QGCSensorSettingsWidget) { ui->setupUi(this); + // FIXME James Goppert // XXX: This might be a bad idea sending a message every time the value changes connect(ui->spinBox_rawSensor, SIGNAL(valueChanged(int)), mav, SLOT(enableRawSensorDataTransmission(int))); connect(ui->spinBox_controller, SIGNAL(valueChanged(int)), mav, SLOT(enableRawControllerDataTransmission(int))); diff --git a/src/ui/QGCSensorSettingsWidget.ui b/src/ui/QGCSensorSettingsWidget.ui index b475dda88eedcfac776546fa9dd3b44ff4ef65cd..9c8267b99ae0e1035b197aa91f2de04fe1a79505 100644 --- a/src/ui/QGCSensorSettingsWidget.ui +++ b/src/ui/QGCSensorSettingsWidget.ui @@ -6,7 +6,7 @@ 0 0 - 237 + 307 221 @@ -134,7 +134,7 @@ - Extended + Ext. Status @@ -148,7 +148,7 @@ - Controller + Raw Contr. @@ -183,7 +183,7 @@ - RC Values + RC Chan. diff --git a/src/ui/QGCSettingsWidget.cc b/src/ui/QGCSettingsWidget.cc new file mode 100644 index 0000000000000000000000000000000000000000..4f6aed319d95a42932ad881a3e9ec48e5a4aba73 --- /dev/null +++ b/src/ui/QGCSettingsWidget.cc @@ -0,0 +1,71 @@ +#include + +#include "QGCSettingsWidget.h" +#include "MainWindow.h" +#include "ui_QGCSettingsWidget.h" + +#include "LinkManager.h" +#include "MAVLinkProtocol.h" +#include "MAVLinkSettingsWidget.h" +#include "GAudioOutput.h" + +//, Qt::WindowFlags flags + +QGCSettingsWidget::QGCSettingsWidget(QWidget *parent, Qt::WindowFlags flags) : + QDialog(parent, flags), + ui(new Ui::QGCSettingsWidget) +{ + ui->setupUi(this); + + // Add all protocols + QList protocols = LinkManager::instance()->getProtocols(); + foreach (ProtocolInterface* protocol, protocols) + { + MAVLinkProtocol* mavlink = dynamic_cast(protocol); + if (mavlink) + { + MAVLinkSettingsWidget* msettings = new MAVLinkSettingsWidget(mavlink, this); + ui->tabWidget->addTab(msettings, "MAVLink"); + } + } + + this->window()->setWindowTitle(tr("QGroundControl Settings")); + + // Audio preferences + ui->audioMuteCheckBox->setChecked(GAudioOutput::instance()->isMuted()); + connect(ui->audioMuteCheckBox, SIGNAL(toggled(bool)), GAudioOutput::instance(), SLOT(mute(bool))); + connect(GAudioOutput::instance(), SIGNAL(mutedChanged(bool)), ui->audioMuteCheckBox, SLOT(setChecked(bool))); + + // Reconnect + ui->reconnectCheckBox->setChecked(MainWindow::instance()->autoReconnectEnabled()); + connect(ui->reconnectCheckBox, SIGNAL(clicked(bool)), MainWindow::instance(), SLOT(enableAutoReconnect(bool))); + + // Style + MainWindow::QGC_MAINWINDOW_STYLE style = (MainWindow::QGC_MAINWINDOW_STYLE)MainWindow::instance()->getStyle(); + switch (style) + { + case MainWindow::QGC_MAINWINDOW_STYLE_NATIVE: + ui->nativeStyle->setChecked(true); + break; + case MainWindow::QGC_MAINWINDOW_STYLE_INDOOR: + ui->indoorStyle->setChecked(true); + break; + case MainWindow::QGC_MAINWINDOW_STYLE_OUTDOOR: + ui->outdoorStyle->setChecked(true); + break; + } + connect(ui->nativeStyle, SIGNAL(clicked()), MainWindow::instance(), SLOT(loadNativeStyle())); + connect(ui->indoorStyle, SIGNAL(clicked()), MainWindow::instance(), SLOT(loadIndoorStyle())); + connect(ui->outdoorStyle, SIGNAL(clicked()), MainWindow::instance(), SLOT(loadOutdoorStyle())); + + // Close / destroy + connect(ui->buttonBox, SIGNAL(accepted()), this, SLOT(deleteLater())); + + // Set layout options + ui->generalPaneGridLayout->setAlignment(Qt::AlignTop); +} + +QGCSettingsWidget::~QGCSettingsWidget() +{ + delete ui; +} diff --git a/src/ui/QGCSettingsWidget.h b/src/ui/QGCSettingsWidget.h new file mode 100644 index 0000000000000000000000000000000000000000..3216a6cec816557f792c1bb47d60d100d386f2af --- /dev/null +++ b/src/ui/QGCSettingsWidget.h @@ -0,0 +1,24 @@ +#ifndef QGCSETTINGSWIDGET_H +#define QGCSETTINGSWIDGET_H + +#include + +namespace Ui { + class QGCSettingsWidget; +} + +class QGCSettingsWidget : public QDialog +{ + Q_OBJECT + +public: + QGCSettingsWidget(QWidget *parent = 0, Qt::WindowFlags flags = Qt::Sheet); + ~QGCSettingsWidget(); + +public slots: + +private: + Ui::QGCSettingsWidget *ui; +}; + +#endif // QGCSETTINGSWIDGET_H diff --git a/src/ui/QGCSettingsWidget.ui b/src/ui/QGCSettingsWidget.ui new file mode 100644 index 0000000000000000000000000000000000000000..1be65e8ac5ebd0e94570414d8c0876334a59475f --- /dev/null +++ b/src/ui/QGCSettingsWidget.ui @@ -0,0 +1,126 @@ + + + QGCSettingsWidget + + + + 0 + 0 + 535 + 427 + + + + Dialog + + + + + + + General + + + General Settings + + + + + + Mute all audio output + + + + :/images/status/audio-volume-muted.svg:/images/status/audio-volume-muted.svg + + + + + + + Automatically reconnect last link on application startup + + + + :/images/devices/network-wireless.svg:/images/devices/network-wireless.svg + + + + + + + Use native platform look and feel (Windows/Linux/Mac OS) + + + true + + + + + + + Use indoor mission style (black background) + + + + + + + Use outdoor mission style (light background) + + + + + + + + + + + Qt::Horizontal + + + QDialogButtonBox::Close + + + + + + + + + + + buttonBox + accepted() + QGCSettingsWidget + accept() + + + 248 + 254 + + + 157 + 274 + + + + + buttonBox + rejected() + QGCSettingsWidget + reject() + + + 316 + 260 + + + 286 + 274 + + + + + diff --git a/src/ui/QGCUDPLinkConfiguration.cc b/src/ui/QGCUDPLinkConfiguration.cc new file mode 100644 index 0000000000000000000000000000000000000000..ff4c797e6e6c7f58338ea0d7f059d6be6f0c2e5d --- /dev/null +++ b/src/ui/QGCUDPLinkConfiguration.cc @@ -0,0 +1,42 @@ +#include + +#include "QGCUDPLinkConfiguration.h" +#include "ui_QGCUDPLinkConfiguration.h" + +QGCUDPLinkConfiguration::QGCUDPLinkConfiguration(UDPLink* link, QWidget *parent) : + QWidget(parent), + link(link), + ui(new Ui::QGCUDPLinkConfiguration) +{ + ui->setupUi(this); + ui->portSpinBox->setValue(link->getPort()); + connect(ui->portSpinBox, SIGNAL(valueChanged(int)), link, SLOT(setPort(int))); + connect(ui->addIPButton, SIGNAL(clicked()), this, SLOT(addHost())); +} + +QGCUDPLinkConfiguration::~QGCUDPLinkConfiguration() +{ + delete ui; +} + +void QGCUDPLinkConfiguration::changeEvent(QEvent *e) +{ + QWidget::changeEvent(e); + switch (e->type()) { + case QEvent::LanguageChange: + ui->retranslateUi(this); + break; + default: + break; + } +} + +void QGCUDPLinkConfiguration::addHost() +{ + bool ok; + QString hostName = QInputDialog::getText(this, tr("Add a new IP address / hostname to MAVLink"), + tr("Host (hostname:port):"), QLineEdit::Normal, + "localhost:14555", &ok); + if (ok && !hostName.isEmpty()) + link->addHost(hostName); +} diff --git a/src/ui/QGCUDPLinkConfiguration.h b/src/ui/QGCUDPLinkConfiguration.h new file mode 100644 index 0000000000000000000000000000000000000000..12343332d46781ac19a0de57d150008e6f143e5a --- /dev/null +++ b/src/ui/QGCUDPLinkConfiguration.h @@ -0,0 +1,32 @@ +#ifndef QGCUDPLINKCONFIGURATION_H +#define QGCUDPLINKCONFIGURATION_H + +#include + +#include "UDPLink.h" + +namespace Ui { + class QGCUDPLinkConfiguration; +} + +class QGCUDPLinkConfiguration : public QWidget +{ + Q_OBJECT + +public: + explicit QGCUDPLinkConfiguration(UDPLink* link, QWidget *parent = 0); + ~QGCUDPLinkConfiguration(); + +public slots: + void addHost(); + +protected: + void changeEvent(QEvent *e); + + UDPLink* link; ///< UDP link instance this widget configures + +private: + Ui::QGCUDPLinkConfiguration *ui; +}; + +#endif // QGCUDPLINKCONFIGURATION_H diff --git a/src/ui/QGCUDPLinkConfiguration.ui b/src/ui/QGCUDPLinkConfiguration.ui new file mode 100644 index 0000000000000000000000000000000000000000..156e0fd824e3b2bb5870056908486b3cf19c6906 --- /dev/null +++ b/src/ui/QGCUDPLinkConfiguration.ui @@ -0,0 +1,52 @@ + + + QGCUDPLinkConfiguration + + + + 0 + 0 + 400 + 300 + + + + Form + + + + + + UDP Port + + + + + + + 3000 + + + 100000 + + + + + + + Add remote communication target + + + + + + + Add IP + + + + + + + + diff --git a/src/ui/RadioCalibration/AirfoilServoCalibrator.cc b/src/ui/RadioCalibration/AirfoilServoCalibrator.cc index 4d7b587a11b5e925e8f84027cc736d657f8662cd..8361adc700a872a0b516a1f2212740cd8fcf42f9 100644 --- a/src/ui/RadioCalibration/AirfoilServoCalibrator.cc +++ b/src/ui/RadioCalibration/AirfoilServoCalibrator.cc @@ -43,21 +43,21 @@ AirfoilServoCalibrator::AirfoilServoCalibrator(AirfoilType type, QWidget *parent QLabel *lowPulseString; if (type == AILERON) { - highPulseString = new QLabel(tr("Bank Left")); + highPulseString = new QLabel(tr("Bank Right")); centerPulseString = new QLabel(tr("Center")); - lowPulseString = new QLabel(tr("Bank Right")); + lowPulseString = new QLabel(tr("Bank Left")); } else if (type == ELEVATOR) { - highPulseString = new QLabel(tr("Nose Down")); + highPulseString = new QLabel(tr("Nose Up")); centerPulseString = new QLabel(tr("Center")); - lowPulseString = new QLabel(tr("Nose Up")); + lowPulseString = new QLabel(tr("Nose Down")); } else if (type == RUDDER) { - highPulseString = new QLabel(tr("Nose Left")); + highPulseString = new QLabel(tr("Nose Right")); centerPulseString = new QLabel(tr("Center")); - lowPulseString = new QLabel(tr("Nose Right")); + lowPulseString = new QLabel(tr("Nose Left")); } else { @@ -90,8 +90,6 @@ AirfoilServoCalibrator::AirfoilServoCalibrator(AirfoilType type, QWidget *parent connect(lowButton, SIGNAL(clicked()), this, SLOT(setLow())); } - - void AirfoilServoCalibrator::setHigh() { highPulseWidth->setText(QString::number(static_cast(logExtrema()))); diff --git a/src/ui/RadioCalibration/RadioCalibrationData.h b/src/ui/RadioCalibration/RadioCalibrationData.h index f001bc340f8b3b0b1f196ef9813128eb66fce7eb..1e67d21b138938900a19525f9a03f4ac1158f2b3 100644 --- a/src/ui/RadioCalibration/RadioCalibrationData.h +++ b/src/ui/RadioCalibration/RadioCalibrationData.h @@ -67,8 +67,12 @@ public: }; const float* operator[](int i) const; +#ifdef _MSC_VER + const QVector& operator()(int i) const; +#else const QVector& operator()(int i) const throw(std::out_of_range); - void set(int element, int index, float value) {(*data)[element][index] = value;} +#endif + void set(int element, int index, float value) {(*data)[element][index] = value;} public slots: void setAileron(int index, float value) {set(AILERON, index, value);} diff --git a/src/ui/UASControl.ui b/src/ui/UASControl.ui index e506dbc1622d82c1b45b68137983b7f91e2157f1..12a4ee1a26331611ec217e714dfdf50671f7b78a 100644 --- a/src/ui/UASControl.ui +++ b/src/ui/UASControl.ui @@ -7,7 +7,7 @@ 0 0 200 - 150 + 228 @@ -25,7 +25,7 @@ 267 - 194 + 16777215 @@ -34,13 +34,10 @@ Control widget to send basic control actions to the micro air vehicle - + 4 - - 6 - @@ -62,6 +59,12 @@ 10 + + Currently controlled system + + + Currently controlled system + UNCONNECTED @@ -99,6 +102,12 @@ 16 + + Liftoff / Launch + + + Liftoff / Launch + Start @@ -116,6 +125,12 @@ 16 + + Fly straight to landing spot + + + Fly straight to landing spot + Land @@ -133,6 +148,12 @@ 16 + + Only active on the ground: Poweroff system + + + Only active on the ground: Poweroff system + Halt @@ -152,6 +173,12 @@ 16 + + Select MAV operation mode + + + Select MAV operation mode + @@ -162,6 +189,12 @@ 16 + + Transmit and enable mode on MAV + + + Transmit and enable mode on MAV + Set @@ -179,6 +212,12 @@ 10 + + Status label + + + Status label + No actions executed so far @@ -208,11 +247,30 @@ 30 + + Main control button + + + Main control button + Activate Engine + + + + Qt::Vertical + + + + 20 + 5 + + + + diff --git a/src/ui/UASView.ui b/src/ui/UASView.ui index acdc5dbd6d702e3bfec9f0d81b77e843f62b3813..b4d8569080e58b1714337dcd42ead1736edadd36 100644 --- a/src/ui/UASView.ui +++ b/src/ui/UASView.ui @@ -6,8 +6,8 @@ 0 0 - 260 - 111 + 360 + 121 @@ -56,10 +56,11 @@ QLabel#modeLabel { QLabel#stateLabel { font: 12px; + color: #3C7B9E; } -QLabel#gpsLabel { - font: 8px; +QLabel#navLabel { + font: 12px; } QLabel#positionLabel { @@ -110,11 +111,12 @@ QToolButton#typeButton { font-size: 12px; border: 0px solid #999999; border-radius: 5px; - min-width:44px; - max-width: 80px; - min-height: 44px; - max-height: 44px; + min-width:42px; + max-width: 42px; + min-height: 42px; + max-height: 42px; padding: 0px; + margin: 0px; background-color: none; } @@ -122,12 +124,13 @@ QPushButton { font-weight: bold; font-size: 12px; border: 1px solid #999999; - border-radius: 10px; + border-radius: 8px; min-width: 20px; - max-width: 80px; + max-width: 32px; min-height: 16px; max-height: 16px; padding: 2px; + spacing: 10px; background-color: qlineargradient(x1: 0, y1: 0, x2: 0, y2: 1, stop: 0 #777777, stop: 1 #555555); } @@ -161,7 +164,7 @@ QProgressBar:horizontal { border-radius: 4px; text-align: center; padding: 2px; - color: #DDDDDF; + color: #111111; background-color: #111118; } @@ -172,6 +175,7 @@ QProgressBar:vertical { font-size: 7px; padding: 2px; color: #DDDDDF; + min-width: 16px; background-color: #111118; } @@ -181,6 +185,7 @@ QProgressBar:horizontal { QProgressBar:horizontal QLabel { font-size: 9px; + color: #111111; } QProgressBar:vertical QLabel { @@ -235,9 +240,18 @@ QMenu::separator { 2 - + + 2 + + + 2 + + 2 + + 0 + @@ -252,12 +266,18 @@ QMenu::separator { 0 + + + 16777215 + 130 + + - + - 4 + 5 2 @@ -269,14 +289,14 @@ QMenu::separator { - 44 - 44 + 42 + 42 - 80 - 44 + 42 + 42 @@ -285,6 +305,12 @@ QMenu::separator { 30 + + System Type + + + System Type + ... @@ -300,23 +326,7 @@ QMenu::separator { - - - - Qt::Horizontal - - - QSizePolicy::MinimumExpanding - - - - 4 - 88 - - - - - + @@ -332,6 +342,12 @@ QMenu::separator { true + + System Name + + + System Name + UAS001 @@ -354,11 +370,14 @@ QMenu::separator { - + MODE + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - + @@ -368,12 +387,21 @@ QMenu::separator { false + + Remaining flight time + + + Remaining flight time + 00:00:00 + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + - + @@ -383,9 +411,18 @@ QMenu::separator { false + + Uptime + + + Uptime + 00:00:00 + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + @@ -395,12 +432,21 @@ QMenu::separator { -1 + + Current throttle + + + Current throttle + 0 + + throttle %p% + - + @@ -410,12 +456,21 @@ QMenu::separator { false + + Altitude + + + Altitude + 00.00 m + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + - + @@ -425,9 +480,18 @@ QMenu::separator { false + + Ground Speed + + + Ground Speed + 00.0 m/s + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + @@ -450,6 +514,12 @@ QMenu::separator { 40 + + Heartbeat + + + Heartbeat + @@ -474,6 +544,12 @@ QMenu::separator { -1 + + Battery Fuel + + + Battery Fuel + 0 @@ -482,8 +558,8 @@ QMenu::separator { - - + + -1 @@ -492,20 +568,11 @@ QMenu::separator { false - - + + Current Waypoint - - - - - - - -1 - 50 - false - false - + + Current Waypoint --- @@ -537,31 +604,19 @@ QMenu::separator { false - - 00.0 00.0 00.0 m - - - - - - - - 16777215 - 12 - + + Current Position - - - -1 - + + Current Position - + 00.0 00.0 00.0 m - - + + 4 @@ -576,6 +631,18 @@ QMenu::separator { 22 + + + 38 + 22 + + + + Liftoff / Launch + + + Liftoff / Launch + @@ -593,6 +660,18 @@ QMenu::separator { 22 + + + 38 + 22 + + + + Loiter / Wait at current position + + + Loiter / Wait at current position + @@ -610,6 +689,18 @@ QMenu::separator { 22 + + + 38 + 22 + + + + Continue flightplan + + + Continue flightplan + @@ -627,6 +718,18 @@ QMenu::separator { 22 + + + 38 + 22 + + + + Fly straight to landing location + + + Fly straight to landing location + @@ -638,6 +741,18 @@ QMenu::separator { + + + 38 + 22 + + + + Only in standby mode: Power off system + + + Only in standby mode: Power off system + @@ -649,12 +764,30 @@ QMenu::separator { + + + 0 + 0 + + 26 22 + + + 38 + 22 + + + + Emergency land system at closest possible site + + + Emergency land system at closest possible site + @@ -672,6 +805,18 @@ QMenu::separator { 22 + + + 38 + 22 + + + + Kill immediately all onboard power + + + Kill immediately all onboard power + @@ -683,6 +828,62 @@ QMenu::separator { + + + + Waiting for first status update.. + + + + + + + + 16777215 + 12 + + + + + -1 + 50 + false + false + + + + STATE + + + + + + + + -1 + 50 + false + false + + + + NAV + + + + + + + Qt::Horizontal + + + + 8 + 20 + + + + diff --git a/src/ui/WaypointList.cc b/src/ui/WaypointList.cc index 9ea5f05b8c128daa0213df32e9bcf6e38c2f3e40..e34d351dce7b213d067b8283ad412c3b33988639 100644 --- a/src/ui/WaypointList.cc +++ b/src/ui/WaypointList.cc @@ -129,8 +129,8 @@ void WaypointList::setUAS(UASInterface* uas) connect(uas->getWaypointManager(), SIGNAL(waypointListChanged(void)), this, SLOT(waypointListChanged(void))); connect(uas->getWaypointManager(), SIGNAL(waypointChanged(int,Waypoint*)), this, SLOT(updateWaypoint(int,Waypoint*))); connect(uas->getWaypointManager(), SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointChanged(quint16))); - connect(uas->getWaypointManager(),SIGNAL(loadWPFile()),this,SLOT(setIsLoadFileWP())); - connect(uas->getWaypointManager(),SIGNAL(readGlobalWPFromUAS(bool)),this,SLOT(setIsReadGlobalWP(bool))); + //connect(uas->getWaypointManager(),SIGNAL(loadWPFile()),this,SLOT(setIsLoadFileWP())); + //connect(uas->getWaypointManager(),SIGNAL(readGlobalWPFromUAS(bool)),this,SLOT(setIsReadGlobalWP(bool))); } } @@ -148,11 +148,9 @@ void WaypointList::loadWaypoints() { if (uas) { - - QString fileName = QFileDialog::getOpenFileName(this, tr("Load File"), ".", tr("Waypoint File (*.txt)")); uas->getWaypointManager()->loadWaypoints(fileName); - } + } } void WaypointList::transmit() @@ -175,48 +173,23 @@ void WaypointList::add() { if (uas) { -// if(isGlobalWP) -// { -// const QVector &waypoints = uas->getWaypointManager()->getWaypointList(); -// if (waypoints.size() > 0) -// { -// Waypoint *last = waypoints.at(waypoints.size()-1); -// Waypoint *wp = new Waypoint(0, centerMapCoordinate.x(), centerMapCoordinate.y(), last->getZ(), last->getYaw(), last->getAutoContinue(), false, last->getOrbit(), last->getHoldTime()); -// uas->getWaypointManager()->addWaypoint(wp); -// } -// else -// { -// Waypoint *wp = new Waypoint(0, centerMapCoordinate.x(), centerMapCoordinate.y(), -0.8, 0.0, true, true, 0.15, 2000); -// uas->getWaypointManager()->addWaypoint(wp); -// } -// -// emit createWaypointAtMap(centerMapCoordinate); -// } -// else - { - - const QVector &waypoints = uas->getWaypointManager()->getWaypointList(); - Waypoint *wp; + const QVector &waypoints = uas->getWaypointManager()->getWaypointList(); + Waypoint *wp; - if (waypoints.size() > 0) - { - // Create waypoint with last frame - Waypoint *last = waypoints.at(waypoints.size()-1); - wp = new Waypoint(0, last->getX(), last->getY(), last->getZ(), last->getYaw(), last->getAutoContinue(), false, last->getOrbit(), - last->getHoldTime(), last->getFrame(), last->getAction()); - uas->getWaypointManager()->addWaypoint(wp); - } - else - { - // Create global frame waypoint per default - wp = new Waypoint(0, uas->getLongitude(), uas->getLatitude(), uas->getAltitude(), 0.0, true, true, 0.15, 0, MAV_FRAME_GLOBAL, MAV_ACTION_NAVIGATE); - uas->getWaypointManager()->addWaypoint(wp); - } -// if (wp->getFrame() == MAV_FRAME_GLOBAL) -// { -// emit createWaypointAtMap(QPointF(wp->getX(), wp->getY())); -// } + if (waypoints.size() > 0) + { + // Create waypoint with last frame + Waypoint *last = waypoints.at(waypoints.size()-1); + wp = new Waypoint(0, last->getX(), last->getY(), last->getZ(), last->getParam1(), last->getParam2(), last->getParam3(), last->getParam4(), + last->getAutoContinue(), false, last->getFrame(), last->getAction()); + uas->getWaypointManager()->addWaypoint(wp); + } + else + { + // Create global frame waypoint per default + wp = new Waypoint(0, uas->getLatitude(), uas->getLongitude(), uas->getAltitude(), 0.0, 0.0, 0.0, 0.0, true, true, MAV_FRAME_GLOBAL, MAV_CMD_NAV_WAYPOINT); + uas->getWaypointManager()->addWaypoint(wp); } } } @@ -255,8 +228,6 @@ void WaypointList::addCurrentPositonWaypoint() void WaypointList::updateStatusLabel(const QString &string) { m_ui->statusLabel->setText(string); - - } void WaypointList::changeCurrentWaypoint(quint16 seq) @@ -307,58 +278,170 @@ void WaypointList::waypointListChanged() this->setUpdatesEnabled(false); const QVector &waypoints = uas->getWaypointManager()->getWaypointList(); - if (!wpViews.empty()) + if (!wpViews.empty()) + { + QMapIterator viewIt(wpViews); + viewIt.toFront(); + while(viewIt.hasNext()) { - QMapIterator viewIt(wpViews); - viewIt.toFront(); - while(viewIt.hasNext()) + viewIt.next(); + Waypoint *cur = viewIt.key(); + int i; + for (i = 0; i < waypoints.size(); i++) { - viewIt.next(); - Waypoint *cur = viewIt.key(); - int i; - for (i = 0; i < waypoints.size(); i++) + if (waypoints[i] == cur) { - if (waypoints[i] == cur) - { - break; - } - } - if (i == waypoints.size()) - { - WaypointView* widget = wpViews.find(cur).value(); - widget->hide(); - listLayout->removeWidget(widget); - wpViews.remove(cur); + break; } } - } - - // then add/update the views for each waypoint in the list - for(int i = 0; i < waypoints.size(); i++) - { - Waypoint *wp = waypoints[i]; - if (!wpViews.contains(wp)) + if (i == waypoints.size()) { - WaypointView* wpview = new WaypointView(wp, this); - wpViews.insert(wp, wpview); - connect(wpview, SIGNAL(moveDownWaypoint(Waypoint*)), this, SLOT(moveDown(Waypoint*))); - connect(wpview, SIGNAL(moveUpWaypoint(Waypoint*)), this, SLOT(moveUp(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(changeCurrentWaypoint(quint16))); + WaypointView* widget = wpViews.find(cur).value(); + widget->hide(); + listLayout->removeWidget(widget); + wpViews.remove(cur); } - WaypointView *wpv = wpViews.value(wp); - wpv->updateValues(); // update the values of the ui elements in the view - listLayout->addWidget(wpv); - } - this->setUpdatesEnabled(true); } + // then add/update the views for each waypoint in the list + for(int i = 0; i < waypoints.size(); i++) + { + Waypoint *wp = waypoints[i]; + if (!wpViews.contains(wp)) + { + WaypointView* wpview = new WaypointView(wp, this); + wpViews.insert(wp, wpview); + connect(wpview, SIGNAL(moveDownWaypoint(Waypoint*)), this, SLOT(moveDown(Waypoint*))); + connect(wpview, SIGNAL(moveUpWaypoint(Waypoint*)), this, SLOT(moveUp(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(changeCurrentWaypoint(quint16))); + listLayout->insertWidget(i, wpview); + } + WaypointView *wpv = wpViews.value(wp); + //check if ordering has changed + if(listLayout->itemAt(i)->widget() != wpv) + { + listLayout->removeWidget(wpv); + listLayout->insertWidget(i, wpv); + } + + wpv->updateValues(); // update the values of the ui elements in the view + } + this->setUpdatesEnabled(true); loadFileGlobalWP = false; + } } +//void WaypointList::waypointListChanged() +//{ +// if (uas) +// { +// // Prevent updates to prevent visual flicker +// this->setUpdatesEnabled(false); +// // Get all waypoints +// const QVector &waypoints = uas->getWaypointManager()->getWaypointList(); + +//// // Store the current state, then check which widgets to update +//// // and which ones to delete +//// QList oldWaypoints = wpViews.keys(); + +//// foreach (Waypoint* wp, waypoints) +//// { +//// WaypointView* wpview; +//// // Create any new waypoint +//// if (!wpViews.contains(wp)) +//// { +//// wpview = new WaypointView(wp, this); +//// wpViews.insert(wp, wpview); +//// connect(wpview, SIGNAL(moveDownWaypoint(Waypoint*)), this, SLOT(moveDown(Waypoint*))); +//// connect(wpview, SIGNAL(moveUpWaypoint(Waypoint*)), this, SLOT(moveUp(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(changeCurrentWaypoint(quint16))); +//// listLayout->addWidget(wpview); +//// } +//// else +//// { +//// // Update existing waypoints +//// wpview = wpViews.value(wp); + +//// } +//// // Mark as updated by removing from old list +//// oldWaypoints.removeAt(oldWaypoints.indexOf(wp)); + +//// wpview->updateValues(); // update the values of the ui elements in the view + +//// } + +//// // The old list now contains all wps to be deleted +//// foreach (Waypoint* wp, oldWaypoints) +//// { +//// // Delete waypoint view and entry in list +//// WaypointView* wpv = wpViews.value(wp); +//// if (wpv) +//// { +//// listLayout->removeWidget(wpv); +//// delete wpv; +//// } +//// wpViews.remove(wp); +//// } + +// if (!wpViews.empty()) +// { +// QMapIterator viewIt(wpViews); +// viewIt.toFront(); +// while(viewIt.hasNext()) +// { +// viewIt.next(); +// Waypoint *cur = viewIt.key(); +// int i; +// for (i = 0; i < waypoints.size(); i++) +// { +// if (waypoints[i] == cur) +// { +// break; +// } +// } +// if (i == waypoints.size()) +// { +// WaypointView* widget = wpViews.find(cur).value(); +// if (widget) +// { +// widget->hide(); +// listLayout->removeWidget(widget); +// } +// wpViews.remove(cur); +// } +// } +// } + +// // then add/update the views for each waypoint in the list +// for(int i = 0; i < waypoints.size(); i++) +// { +// Waypoint *wp = waypoints[i]; +// if (!wpViews.contains(wp)) +// { +// WaypointView* wpview = new WaypointView(wp, this); +// wpViews.insert(wp, wpview); +// connect(wpview, SIGNAL(moveDownWaypoint(Waypoint*)), this, SLOT(moveDown(Waypoint*))); +// connect(wpview, SIGNAL(moveUpWaypoint(Waypoint*)), this, SLOT(moveUp(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(changeCurrentWaypoint(quint16))); +// } +// WaypointView *wpv = wpViews.value(wp); +// wpv->updateValues(); // update the values of the ui elements in the view +// listLayout->addWidget(wpv); + +// } +// this->setUpdatesEnabled(true); +// } +//// loadFileGlobalWP = false; +//} + void WaypointList::moveUp(Waypoint* wp) { if (uas) @@ -447,27 +530,27 @@ void WaypointList::on_clearWPListButton_clicked() } } -/** @brief The MapWidget informs that a waypoint global was changed on the map */ +///** @brief The MapWidget informs that a waypoint global was changed on the map */ -void WaypointList::waypointGlobalChanged(QPointF coordinate, int indexWP) -{ - if (uas) - { - const QVector &waypoints = uas->getWaypointManager()->getWaypointList(); - if (waypoints.size() > 0) - { - Waypoint *temp = waypoints.at(indexWP); +//void WaypointList::waypointGlobalChanged(QPointF coordinate, int indexWP) +//{ +// if (uas) +// { +// const QVector &waypoints = uas->getWaypointManager()->getWaypointList(); +// if (waypoints.size() > 0) +// { +// Waypoint *temp = waypoints.at(indexWP); - temp->setX(coordinate.x()); - temp->setY(coordinate.y()); +// temp->setX(coordinate.x()); +// temp->setY(coordinate.y()); - //WaypointGlobalView* widget = wpGlobalViews.find(waypoints[indexWP]).value(); - //widget->updateValues(); - } - } +// //WaypointGlobalView* widget = wpGlobalViews.find(waypoints[indexWP]).value(); +// //widget->updateValues(); +// } +// } -} +//} ///** @brief The MapWidget informs that a waypoint global was changed on the map */ @@ -493,17 +576,16 @@ void WaypointList::clearWPWidget() widget->remove(); } } - //emit changePositionWPBySpinBox(wp->getId(), wp->getY(), wp->getX()); } -void WaypointList::setIsLoadFileWP() -{ - loadFileGlobalWP = true; -} +//void WaypointList::setIsLoadFileWP() +//{ +// loadFileGlobalWP = true; +//} -void WaypointList::setIsReadGlobalWP(bool value) -{ - // FIXME James Check this - Q_UNUSED(value); - // readGlobalWP = value; -} +//void WaypointList::setIsReadGlobalWP(bool value) +//{ +// // FIXME James Check this +// Q_UNUSED(value); +// // readGlobalWP = value; +//} diff --git a/src/ui/WaypointList.h b/src/ui/WaypointList.h index cb36e8bd5f2f589ae9e580d9b44bbc9708505bc9..e07c701c2e7ebc5085a8a8e2036cd5bddebda6bf 100644 --- a/src/ui/WaypointList.h +++ b/src/ui/WaypointList.h @@ -87,8 +87,8 @@ public slots: /** @brief The waypoint manager informs that the waypoint list was changed */ void waypointListChanged(void); - /** @brief The MapWidget informs that a waypoint global was changed on the map */ - void waypointGlobalChanged(const QPointF coordinate, const int indexWP); +// /** @brief The MapWidget informs that a waypoint global was changed on the map */ +// void waypointGlobalChanged(const QPointF coordinate, const int indexWP); void clearWPWidget(); @@ -99,8 +99,8 @@ public slots: void moveDown(Waypoint* wp); void removeWaypoint(Waypoint* wp); - void setIsLoadFileWP(); - void setIsReadGlobalWP(bool value); +// void setIsLoadFileWP(); +// void setIsReadGlobalWP(bool value); diff --git a/src/ui/WaypointView.cc b/src/ui/WaypointView.cc index 13851500c1bdfd0f1818bb563a196f3dcd472031..ef651594923f7acf553053b3cf503bb89b013ab7 100644 --- a/src/ui/WaypointView.cc +++ b/src/ui/WaypointView.cc @@ -14,50 +14,68 @@ #include #include -#include //M_PI -#ifndef M_PI -#define M_PI 3.14159265358979323846 -#endif +#include +#include #include "WaypointView.h" #include "ui_WaypointView.h" +#include "ui_QGCCustomWaypointAction.h" WaypointView::WaypointView(Waypoint* wp, QWidget* parent) : QWidget(parent), + customCommand(new Ui_QGCCustomWaypointAction), + viewMode(QGC_WAYPOINTVIEW_MODE_NAV), m_ui(new Ui::WaypointView) { m_ui->setupUi(this); this->wp = wp; + connect(wp, SIGNAL(destroyed(QObject*)), this, SLOT(deleted(QObject*))); + + // CUSTOM COMMAND WIDGET + customCommand->setupUi(m_ui->customActionWidget); // add actions - m_ui->comboBox_action->addItem("Navigate",MAV_ACTION_NAVIGATE); - m_ui->comboBox_action->addItem("TakeOff",MAV_ACTION_TAKEOFF); - m_ui->comboBox_action->addItem("Land",MAV_ACTION_LAND); - m_ui->comboBox_action->addItem("Loiter",MAV_ACTION_LOITER); + m_ui->comboBox_action->addItem(tr("Navigate"),MAV_CMD_NAV_WAYPOINT); + m_ui->comboBox_action->addItem(tr("TakeOff"),MAV_CMD_NAV_TAKEOFF); + m_ui->comboBox_action->addItem(tr("Loiter Unlim."),MAV_CMD_NAV_LOITER_UNLIM); + m_ui->comboBox_action->addItem(tr("Loiter Time"),MAV_CMD_NAV_LOITER_TIME); + m_ui->comboBox_action->addItem(tr("Loiter Turns"),MAV_CMD_NAV_LOITER_TURNS); + m_ui->comboBox_action->addItem(tr("Ret. to Launch"),MAV_CMD_NAV_RETURN_TO_LAUNCH); + m_ui->comboBox_action->addItem(tr("Land"),MAV_CMD_NAV_LAND); + m_ui->comboBox_action->addItem(tr("Other"), MAV_CMD_ENUM_END); + // m_ui->comboBox_action->addItem(tr("Delay"), MAV_ACTION_DELAY_BEFORE_COMMAND); + // m_ui->comboBox_action->addItem(tr("Ascend/Descent"), MAV_ACTION_ASCEND_AT_RATE); + // m_ui->comboBox_action->addItem(tr("Change Mode"), MAV_ACTION_CHANGE_MODE); + // m_ui->comboBox_action->addItem(tr("Relay ON"), MAV_ACTION_RELAY_ON); + // m_ui->comboBox_action->addItem(tr("Relay OFF"), MAV_ACTION_RELAY_OFF); // add frames m_ui->comboBox_frame->addItem("Global",MAV_FRAME_GLOBAL); m_ui->comboBox_frame->addItem("Local",MAV_FRAME_LOCAL); + m_ui->comboBox_frame->addItem("Mission",MAV_FRAME_MISSION); + + // Initialize view correctly + updateActionView(wp->getAction()); + updateFrameView(wp->getFrame()); // Read values and set user interface updateValues(); - // defaults - //changedAction(wp->getAction()); - //changedFrame(wp->getFrame()); + // Check for mission frame + if (wp->getFrame() == MAV_FRAME_MISSION) + { + m_ui->comboBox_action->setCurrentIndex(m_ui->comboBox_action->count()-1); + } connect(m_ui->posNSpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setX(double))); connect(m_ui->posESpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setY(double))); connect(m_ui->posDSpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setZ(double))); - connect(m_ui->lonSpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setX(double))); - connect(m_ui->latSpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setY(double))); + connect(m_ui->latSpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setX(double))); + connect(m_ui->lonSpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setY(double))); connect(m_ui->altSpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setZ(double))); - - //hidden degree to radian conversion of the yaw angle - connect(m_ui->yawSpinBox, SIGNAL(valueChanged(int)), this, SLOT(setYaw(int))); - connect(this, SIGNAL(setYaw(double)), wp, SLOT(setYaw(double))); + connect(m_ui->yawSpinBox, SIGNAL(valueChanged(int)), wp, SLOT(setYaw(int))); connect(m_ui->upButton, SIGNAL(clicked()), this, SLOT(moveUp())); connect(m_ui->downButton, SIGNAL(clicked()), this, SLOT(moveDown())); @@ -68,13 +86,21 @@ WaypointView::WaypointView(Waypoint* wp, QWidget* parent) : connect(m_ui->comboBox_action, SIGNAL(activated(int)), this, SLOT(changedAction(int))); connect(m_ui->comboBox_frame, SIGNAL(activated(int)), this, SLOT(changedFrame(int))); - connect(m_ui->orbitSpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setOrbit(double))); - connect(m_ui->holdTimeSpinBox, SIGNAL(valueChanged(int)), wp, SLOT(setHoldTime(int))); -} - -void WaypointView::setYaw(int yawDegree) -{ - emit setYaw((double)yawDegree*M_PI/180.); + connect(m_ui->orbitSpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setLoiterOrbit(double))); + connect(m_ui->acceptanceSpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setAcceptanceRadius(double))); + connect(m_ui->holdTimeSpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setHoldTime(double))); + connect(m_ui->turnsSpinBox, SIGNAL(valueChanged(int)), wp, SLOT(setTurns(int))); + connect(m_ui->takeOffAngleSpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setParam1(double))); + + // Connect actions + connect(customCommand->commandSpinBox, SIGNAL(valueChanged(int)), wp, SLOT(setAction(int))); + connect(customCommand->param1SpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setParam1(double))); + connect(customCommand->param2SpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setParam2(double))); + connect(customCommand->param3SpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setParam3(double))); + connect(customCommand->param4SpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setParam4(double))); + connect(customCommand->param5SpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setParam5(double))); + connect(customCommand->param6SpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setParam6(double))); + connect(customCommand->param7SpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setParam7(double))); } void WaypointView::moveUp() @@ -90,7 +116,7 @@ void WaypointView::moveDown() void WaypointView::remove() { emit removeWaypoint(wp); - delete this; + deleteLater(); } void WaypointView::changedAutoContinue(int state) @@ -101,48 +127,169 @@ void WaypointView::changedAutoContinue(int state) wp->setAutocontinue(true); } -void WaypointView::changedAction(int index) +void WaypointView::updateActionView(int action) { - // set action for waypoint - - - // hide everything to start - m_ui->orbitSpinBox->hide(); - m_ui->takeOffAngleSpinBox->hide(); - m_ui->autoContinue->hide(); - m_ui->holdTimeSpinBox->hide(); - - // set waypoint action - MAV_ACTION action = (MAV_ACTION) m_ui->comboBox_action->itemData(index).toUInt(); - wp->setAction(action); - // expose ui based on action + switch(action) { - case MAV_ACTION_TAKEOFF: + case MAV_CMD_NAV_TAKEOFF: + m_ui->orbitSpinBox->hide(); + m_ui->yawSpinBox->hide(); + m_ui->turnsSpinBox->hide(); + m_ui->autoContinue->hide(); + m_ui->holdTimeSpinBox->hide(); + m_ui->acceptanceSpinBox->hide(); + m_ui->customActionWidget->hide(); m_ui->takeOffAngleSpinBox->show(); break; - case MAV_ACTION_LAND: + case MAV_CMD_NAV_LAND: + m_ui->orbitSpinBox->hide(); + m_ui->takeOffAngleSpinBox->hide(); + m_ui->yawSpinBox->hide(); + m_ui->turnsSpinBox->hide(); + m_ui->autoContinue->hide(); + m_ui->holdTimeSpinBox->hide(); + m_ui->acceptanceSpinBox->hide(); + m_ui->customActionWidget->hide(); + break; + case MAV_CMD_NAV_RETURN_TO_LAUNCH: + m_ui->orbitSpinBox->hide(); + m_ui->takeOffAngleSpinBox->hide(); + m_ui->yawSpinBox->hide(); + m_ui->turnsSpinBox->hide(); + m_ui->autoContinue->hide(); + m_ui->holdTimeSpinBox->hide(); + m_ui->acceptanceSpinBox->hide(); + m_ui->customActionWidget->hide(); break; - case MAV_ACTION_NAVIGATE: + case MAV_CMD_NAV_WAYPOINT: + m_ui->orbitSpinBox->hide(); + m_ui->takeOffAngleSpinBox->hide(); + m_ui->turnsSpinBox->hide(); + m_ui->holdTimeSpinBox->show(); + m_ui->customActionWidget->hide(); + m_ui->autoContinue->show(); + m_ui->acceptanceSpinBox->show(); + m_ui->yawSpinBox->show(); + break; + case MAV_CMD_NAV_LOITER_UNLIM: + m_ui->takeOffAngleSpinBox->hide(); + m_ui->yawSpinBox->hide(); + m_ui->turnsSpinBox->hide(); + m_ui->autoContinue->hide(); + m_ui->holdTimeSpinBox->hide(); + m_ui->acceptanceSpinBox->hide(); + m_ui->customActionWidget->hide(); + m_ui->orbitSpinBox->show(); + break; + case MAV_CMD_NAV_LOITER_TURNS: + m_ui->takeOffAngleSpinBox->hide(); + m_ui->yawSpinBox->hide(); + m_ui->autoContinue->hide(); + m_ui->holdTimeSpinBox->hide(); + m_ui->acceptanceSpinBox->hide(); + m_ui->customActionWidget->hide(); m_ui->orbitSpinBox->show(); + m_ui->turnsSpinBox->show(); break; - case MAV_ACTION_LOITER: + case MAV_CMD_NAV_LOITER_TIME: + m_ui->takeOffAngleSpinBox->hide(); + m_ui->yawSpinBox->hide(); + m_ui->turnsSpinBox->hide(); + m_ui->autoContinue->hide(); + m_ui->acceptanceSpinBox->hide(); + m_ui->customActionWidget->hide(); m_ui->orbitSpinBox->show(); m_ui->holdTimeSpinBox->show(); break; default: - std::cerr << "unknown action" << std::endl; + break; } } -void WaypointView::changedFrame(int index) +/** + * @param index The index of the combo box of the action entry, NOT the action ID + */ +void WaypointView::changedAction(int index) { // set waypoint action - MAV_FRAME frame = (MAV_FRAME)m_ui->comboBox_frame->itemData(index).toUInt(); - wp->setFrame(frame); + int actionIndex = m_ui->comboBox_action->itemData(index).toUInt(); + if (actionIndex < MAV_CMD_ENUM_END && actionIndex >= 0) + { + MAV_CMD action = (MAV_CMD) actionIndex; + wp->setAction(action); + } + + // Expose ui based on action + // Change to mission frame + // if action is unknown + + switch(actionIndex) + { + case MAV_CMD_NAV_TAKEOFF: + case MAV_CMD_NAV_LAND: + case MAV_CMD_NAV_RETURN_TO_LAUNCH: + case MAV_CMD_NAV_WAYPOINT: + case MAV_CMD_NAV_LOITER_UNLIM: + case MAV_CMD_NAV_LOITER_TURNS: + case MAV_CMD_NAV_LOITER_TIME: + changeViewMode(QGC_WAYPOINTVIEW_MODE_NAV); + // Update frame view + updateFrameView(m_ui->comboBox_frame->currentIndex()); + // Update view + updateActionView(actionIndex); + break; + case MAV_CMD_ENUM_END: + default: + // Switch to mission frame + changeViewMode(QGC_WAYPOINTVIEW_MODE_DIRECT_EDITING); + break; + } +} + +void WaypointView::changeViewMode(QGC_WAYPOINTVIEW_MODE mode) +{ + switch (mode) + { + case QGC_WAYPOINTVIEW_MODE_NAV: + case QGC_WAYPOINTVIEW_MODE_CONDITION: + // Hide everything, show condition widget + // TODO + case QGC_WAYPOINTVIEW_MODE_DO: + break; + case QGC_WAYPOINTVIEW_MODE_DIRECT_EDITING: + // Hide almost everything + m_ui->orbitSpinBox->hide(); + m_ui->takeOffAngleSpinBox->hide(); + m_ui->yawSpinBox->hide(); + m_ui->turnsSpinBox->hide(); + m_ui->holdTimeSpinBox->hide(); + m_ui->acceptanceSpinBox->hide(); + m_ui->posDSpinBox->hide(); + m_ui->posESpinBox->hide(); + m_ui->posNSpinBox->hide(); + m_ui->latSpinBox->hide(); + m_ui->lonSpinBox->hide(); + m_ui->altSpinBox->hide(); + + // Show action widget + if (!m_ui->customActionWidget->isVisible()) + { + m_ui->customActionWidget->show(); + } + if (!m_ui->autoContinue->isVisible()) + { + m_ui->autoContinue->show(); + } + break; + } +} + +void WaypointView::updateFrameView(int frame) +{ switch(frame) { case MAV_FRAME_GLOBAL: @@ -152,6 +299,9 @@ void WaypointView::changedFrame(int index) m_ui->lonSpinBox->show(); m_ui->latSpinBox->show(); m_ui->altSpinBox->show(); + // Coordinate frame + m_ui->comboBox_frame->show(); + m_ui->customActionWidget->hide(); break; case MAV_FRAME_LOCAL: m_ui->lonSpinBox->hide(); @@ -160,12 +310,33 @@ void WaypointView::changedFrame(int index) m_ui->posNSpinBox->show(); m_ui->posESpinBox->show(); m_ui->posDSpinBox->show(); + // Coordinate frame + m_ui->comboBox_frame->show(); + m_ui->customActionWidget->hide(); break; default: std::cerr << "unknown frame" << std::endl; } } +void WaypointView::deleted(QObject* waypoint) +{ + Q_UNUSED(waypoint); +// if (waypoint == this->wp) +// { +// deleteLater(); +// } +} + +void WaypointView::changedFrame(int index) +{ + // set waypoint action + MAV_FRAME frame = (MAV_FRAME)m_ui->comboBox_frame->itemData(index).toUInt(); + wp->setFrame(frame); + + updateFrameView(frame); +} + void WaypointView::changedCurrent(int state) { if (state == 0) @@ -183,6 +354,13 @@ void WaypointView::changedCurrent(int state) void WaypointView::updateValues() { + // Check if we just lost the wp, delete the widget + // accordingly + if (!wp) + { + deleteLater(); + return; + } // Deactivate signals from the WP wp->blockSignals(true); // update frame @@ -191,6 +369,7 @@ void WaypointView::updateValues() if (m_ui->comboBox_frame->currentIndex() != frame_index) { m_ui->comboBox_frame->setCurrentIndex(frame_index); + updateFrameView(frame); } switch(frame) { @@ -212,13 +391,13 @@ void WaypointView::updateValues() break; case(MAV_FRAME_GLOBAL): { - if (m_ui->lonSpinBox->value() != wp->getX()) + if (m_ui->latSpinBox->value() != wp->getX()) { - m_ui->lonSpinBox->setValue(wp->getX()); + m_ui->latSpinBox->setValue(wp->getX()); } - if (m_ui->latSpinBox->value() != wp->getY()) + if (m_ui->lonSpinBox->value() != wp->getY()) { - m_ui->latSpinBox->setValue(wp->getY()); + m_ui->lonSpinBox->setValue(wp->getY()); } if (m_ui->altSpinBox->value() != wp->getZ()) { @@ -226,34 +405,51 @@ void WaypointView::updateValues() } } break; + default: + // Do nothing + break; } - changedFrame(frame_index); - // update action - MAV_ACTION action = wp->getAction(); + // Update action + MAV_CMD action = wp->getAction(); int action_index = m_ui->comboBox_action->findData(action); + // Set to "Other" action if it was -1 + if (action_index == -1) + { + action_index = m_ui->comboBox_action->findData(MAV_CMD_ENUM_END); + } + // Only update if changed if (m_ui->comboBox_action->currentIndex() != action_index) { - m_ui->comboBox_action->setCurrentIndex(action_index); + // If action is unknown, set direct editing mode + if (wp->getAction() < 0 || wp->getAction() > MAV_CMD_NAV_TAKEOFF) + { + changeViewMode(QGC_WAYPOINTVIEW_MODE_DIRECT_EDITING); + } + else + { + // Action ID known, update + m_ui->comboBox_action->setCurrentIndex(action_index); + updateActionView(action); + } } switch(action) { - case MAV_ACTION_TAKEOFF: + case MAV_CMD_NAV_TAKEOFF: break; - case MAV_ACTION_LAND: + case MAV_CMD_NAV_LAND: break; - case MAV_ACTION_NAVIGATE: + case MAV_CMD_NAV_WAYPOINT: break; - case MAV_ACTION_LOITER: + case MAV_CMD_NAV_LOITER_UNLIM: break; default: std::cerr << "unknown action" << std::endl; } - changedAction(action_index); - if (m_ui->yawSpinBox->value() != wp->getYaw()/M_PI*180.) + if (m_ui->yawSpinBox->value() != wp->getYaw()) { - m_ui->yawSpinBox->setValue(wp->getYaw()/M_PI*180.); + m_ui->yawSpinBox->setValue(wp->getYaw()); } if (m_ui->selectedBox->isChecked() != wp->getCurrent()) { @@ -264,27 +460,78 @@ void WaypointView::updateValues() m_ui->autoContinue->setChecked(wp->getAutoContinue()); } m_ui->idLabel->setText(QString("%1").arg(wp->getId())); - if (m_ui->orbitSpinBox->value() != wp->getOrbit()) + if (m_ui->orbitSpinBox->value() != wp->getLoiterOrbit()) { - m_ui->orbitSpinBox->setValue(wp->getOrbit()); + m_ui->orbitSpinBox->setValue(wp->getLoiterOrbit()); + } + if (m_ui->acceptanceSpinBox->value() != wp->getAcceptanceRadius()) + { + m_ui->acceptanceSpinBox->setValue(wp->getAcceptanceRadius()); } if (m_ui->holdTimeSpinBox->value() != wp->getHoldTime()) { m_ui->holdTimeSpinBox->setValue(wp->getHoldTime()); } - wp->blockSignals(false); -} + if (m_ui->turnsSpinBox->value() != wp->getTurns()) + { + m_ui->turnsSpinBox->setValue(wp->getTurns()); + } + if (m_ui->takeOffAngleSpinBox->value() != wp->getParam1()) + { + m_ui->takeOffAngleSpinBox->setValue(wp->getParam1()); + } -void WaypointView::setCurrent(bool state) -{ - if (state) + // UPDATE CUSTOM ACTION WIDGET + + if (customCommand->commandSpinBox->value() != wp->getAction()) { - m_ui->selectedBox->setCheckState(Qt::Checked); + customCommand->commandSpinBox->setValue(wp->getAction()); + qDebug() << "Changed action"; } - else + // Param 1 + if (customCommand->param1SpinBox->value() != wp->getParam1()) + { + customCommand->param1SpinBox->setValue(wp->getParam1()); + } + // Param 2 + if (customCommand->param2SpinBox->value() != wp->getParam2()) { - m_ui->selectedBox->setCheckState(Qt::Unchecked); + customCommand->param2SpinBox->setValue(wp->getParam2()); } + // Param 3 + if (customCommand->param3SpinBox->value() != wp->getParam3()) + { + customCommand->param3SpinBox->setValue(wp->getParam3()); + } + // Param 4 + if (customCommand->param4SpinBox->value() != wp->getParam4()) + { + customCommand->param4SpinBox->setValue(wp->getParam4()); + } + // Param 5 + if (customCommand->param5SpinBox->value() != wp->getParam5()) + { + customCommand->param5SpinBox->setValue(wp->getParam5()); + } + // Param 6 + if (customCommand->param6SpinBox->value() != wp->getParam6()) + { + customCommand->param6SpinBox->setValue(wp->getParam6()); + } + // Param 7 + if (customCommand->param7SpinBox->value() != wp->getParam7()) + { + customCommand->param7SpinBox->setValue(wp->getParam7()); + } + + wp->blockSignals(false); +} + +void WaypointView::setCurrent(bool state) +{ + m_ui->selectedBox->blockSignals(true); + m_ui->selectedBox->setChecked(state); + m_ui->selectedBox->blockSignals(false); } WaypointView::~WaypointView() diff --git a/src/ui/WaypointView.h b/src/ui/WaypointView.h index 8a1058bde33105b5ba2d31c027bd3a3a849386cc..a59f0df2298598b4424f17dc57ec31bd0a2ff06b 100644 --- a/src/ui/WaypointView.h +++ b/src/ui/WaypointView.h @@ -1,5 +1,4 @@ -/*===================================================================== - +/*=================================================================== QGroundControl Open Source Ground Control Station (c) 2009, 2010 QGROUNDCONTROL PROJECT @@ -10,15 +9,15 @@ This file is part of the QGROUNDCONTROL project it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. - + QGROUNDCONTROL is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. - + You should have received a copy of the GNU General Public License along with QGROUNDCONTROL. If not, see . - + ======================================================================*/ /** @@ -38,39 +37,56 @@ This file is part of the QGROUNDCONTROL project #include "Waypoint.h" #include +enum QGC_WAYPOINTVIEW_MODE { + QGC_WAYPOINTVIEW_MODE_NAV, + QGC_WAYPOINTVIEW_MODE_CONDITION, + QGC_WAYPOINTVIEW_MODE_DO, + QGC_WAYPOINTVIEW_MODE_DIRECT_EDITING +}; + namespace Ui { class WaypointView; } +class Ui_QGCCustomWaypointAction; class WaypointView : public QWidget { Q_OBJECT Q_DISABLE_COPY(WaypointView) - public: - explicit WaypointView(Waypoint* wp, QWidget* parent); +public: + explicit WaypointView(Waypoint* wp, QWidget* parent); virtual ~WaypointView(); - + public: void setCurrent(bool state); - + public slots: void moveUp(); void moveDown(); void remove(); + /** @brief Waypoint matching this widget has been deleted */ + void deleted(QObject* waypoint); void changedAutoContinue(int); + void updateFrameView(int frame); void changedFrame(int state); + void updateActionView(int action); void changedAction(int state); void changedCurrent(int); void updateValues(void); - - void setYaw(int); //hidden degree to radian conversion + +protected slots: + void changeViewMode(QGC_WAYPOINTVIEW_MODE mode); protected: virtual void changeEvent(QEvent *e); Waypoint* wp; - + // Special widgets extendending the + // waypoint view to mission capabilities + Ui_QGCCustomWaypointAction* customCommand; + QGC_WAYPOINTVIEW_MODE viewMode; + private: Ui::WaypointView *m_ui; - + signals: void moveUpWaypoint(Waypoint*); void moveDownWaypoint(Waypoint*); diff --git a/src/ui/WaypointView.ui b/src/ui/WaypointView.ui index 4615d6b48fd6e125a3ef6e668541d412f75a162c..8644d9af0d7de6b6223a83c51eb2a73b590ab621 100644 --- a/src/ui/WaypointView.ui +++ b/src/ui/WaypointView.ui @@ -6,7 +6,7 @@ 0 0 - 1389 + 2208 39 @@ -161,7 +161,7 @@ QProgressBar::chunk#thrustBar { - + 2 @@ -176,6 +176,9 @@ QProgressBar::chunk#thrustBar { Currently selected waypoint + + Currently selected waypoint + @@ -199,7 +202,7 @@ QProgressBar::chunk#thrustBar { Waypoint Sequence Number - TextLabel + ID Qt::AlignCenter @@ -214,6 +217,15 @@ QProgressBar::chunk#thrustBar { 0 + + Action at Waypoint + + + Action at Waypoint + + + QComboBox::AdjustToContentsOnFirstShow + @@ -224,6 +236,15 @@ QProgressBar::chunk#thrustBar { 0 + + Coordinate frame + + + Coordinate frame + + + QComboBox::AdjustToContentsOnFirstShow + @@ -240,6 +261,9 @@ QProgressBar::chunk#thrustBar { Position X coordinate + + Position X corrdinate + false @@ -278,7 +302,10 @@ QProgressBar::chunk#thrustBar { Qt::WheelFocus - Position Y coordinate + Position Y/Longitude coordinate + + + Position Y/Longitude coordinate E @@ -312,7 +339,10 @@ QProgressBar::chunk#thrustBar { Qt::WheelFocus - Position Z coordinate (negative) + Position Z coordinate (local frame, negative) + + + D @@ -342,6 +372,12 @@ QProgressBar::chunk#thrustBar { Qt::WheelFocus + + Latitude in degrees + + + Latitude in degrees + lat @@ -373,6 +409,12 @@ QProgressBar::chunk#thrustBar { Qt::WheelFocus + + Longitude in degrees + + + Longitude in degrees + lon @@ -401,6 +443,12 @@ QProgressBar::chunk#thrustBar { 0 + + Altitude in meters + + + Altitude in meters + alt @@ -430,7 +478,10 @@ QProgressBar::chunk#thrustBar { Qt::StrongFocus - Yaw angle + Rotary wing only: Desired yaw angle at waypoint + + + Rotary wing only: Desired yaw angle at waypoint true @@ -461,7 +512,10 @@ QProgressBar::chunk#thrustBar { Qt::StrongFocus - Orbit radius + Loiter radius + + + Loiter radius m @@ -481,33 +535,49 @@ QProgressBar::chunk#thrustBar { - - - - 0 - 0 - + + + Uncertainty radius in meters +where to accept this waypoint as reached - - Qt::StrongFocus + + Uncertainty radius in meters where to accept this waypoint as reached + + + m + + 0.200000000000000 + + + + + - Time in milliseconds that the MAV has to stay inside the orbit before advancing + Rotaty wing and ground vehicles only: +Time to stay at this position before advancing - + Rotaty wing and ground vehicles only: Time to stay at this position before advancing - ms + s - 60000 + 9999.989999999999782 - - 500 + + + + + + Number of turns to loiter - - 0 + + Number of turns to loiter + + + turns @@ -525,12 +595,28 @@ QProgressBar::chunk#thrustBar { Take off angle + + Take off angle + ° + + 0 + + + + + + + + 0 + 0 + + @@ -538,6 +624,9 @@ QProgressBar::chunk#thrustBar { Automatically continue after this waypoint + + Automatically continue after this waypoint + @@ -555,7 +644,10 @@ QProgressBar::chunk#thrustBar { Qt::NoFocus - Move Up + Move Up in List + + + Move Up in List @@ -578,7 +670,10 @@ QProgressBar::chunk#thrustBar { Qt::NoFocus - Move Down + Move Down in List + + + Move Down in List diff --git a/src/ui/designer/QGCToolWidget.cc b/src/ui/designer/QGCToolWidget.cc index dad5c8a3fbbeb3953b66b90f62bc96e00f5a2362..f25445f4165bcac38f212a8f157c212fa4873f78 100644 --- a/src/ui/designer/QGCToolWidget.cc +++ b/src/ui/designer/QGCToolWidget.cc @@ -32,7 +32,7 @@ QGCToolWidget::QGCToolWidget(const QString& title, QWidget *parent) : } // Try with parent - dock = dynamic_cast(this->parentWidget()); + dock = dynamic_cast(parent); if (dock) { dock->setWindowTitle(title); diff --git a/src/ui/linechart/LinechartPlot.cc b/src/ui/linechart/LinechartPlot.cc index 85af32ffe6d14cba76bfb6499ebc5d4d69a0ced8..cbce1e45bd3470be022c55e94474b320aa5db4ed 100644 --- a/src/ui/linechart/LinechartPlot.cc +++ b/src/ui/linechart/LinechartPlot.cc @@ -789,13 +789,13 @@ void TimeSeriesData::append(quint64 ms, double value) this->value[count] = value; this->lastValue = value; this->mean = 0; - QList medianList = QList(); + //QList medianList = QList(); for (unsigned int i = 0; (i < averageWindow) && (((int)count - (int)i) >= 0); ++i) { this->mean += this->value[count-i]; - medianList.append(this->value[count-i]); + //medianList.append(this->value[count-i]); } - mean = mean / static_cast(qMin(averageWindow,static_cast(count))); + this->mean = mean / static_cast(qMin(averageWindow,static_cast(count))); this->variance = 0; for (unsigned int i = 0; (i < averageWindow) && (((int)count - (int)i) >= 0); ++i) @@ -804,19 +804,19 @@ void TimeSeriesData::append(quint64 ms, double value) } this->variance = this->variance / static_cast(qMin(averageWindow,static_cast(count))); - qSort(medianList); - - if (medianList.count() > 2) - { - if (medianList.count() % 2 == 0) - { - median = (medianList.at(medianList.count()/2) + medianList.at(medianList.count()/2+1)) / 2.0; - } - else - { - median = medianList.at(medianList.count()/2+1); - } - } +// qSort(medianList); + +// if (medianList.count() > 2) +// { +// if (medianList.count() % 2 == 0) +// { +// median = (medianList.at(medianList.count()/2) + medianList.at(medianList.count()/2+1)) / 2.0; +// } +// else +// { +// median = medianList.at(medianList.count()/2+1); +// } +// } // Update statistical values if(ms < startTime) startTime = ms; diff --git a/src/ui/linechart/LinechartWidget.cc b/src/ui/linechart/LinechartWidget.cc index 21ac666aa44bcfef1840b95abd698fc1ea9bacf4..644c05d781323a6dfd4a7ca2eb338a0a5e445f72 100644 --- a/src/ui/linechart/LinechartWidget.cc +++ b/src/ui/linechart/LinechartWidget.cc @@ -107,7 +107,9 @@ updateTimer(new QTimer()) int labelRow = curvesWidgetLayout->rowCount(); - curvesWidgetLayout->addWidget(new QLabel(tr("On")), labelRow, 0, 1, 2); + selectAllCheckBox = new QCheckBox("", this); + connect(selectAllCheckBox, SIGNAL(clicked(bool)), this, SLOT(selectAllCurves(bool))); + curvesWidgetLayout->addWidget(selectAllCheckBox, labelRow, 0, 1, 2); label = new QLabel(this); label->setText("Name"); @@ -153,6 +155,15 @@ LinechartWidget::~LinechartWidget() listedCurves = NULL; } +void LinechartWidget::selectAllCurves(bool all) +{ + QMap::iterator i; + for (i = curveLabels->begin(); i != curveLabels->end(); ++i) + { + activePlot->setVisible(i.key(), all); + } +} + void LinechartWidget::writeSettings() { QSettings settings; @@ -299,7 +310,7 @@ void LinechartWidget::appendData(int uasId, QString curve, double value, quint64 // Log data if (logging) { - if (activePlot->isVisible(curve)) + if (activePlot->isVisible(curve+unit)) { if (logStartTime == 0) logStartTime = usec; qint64 time = usec - logStartTime; @@ -365,7 +376,7 @@ void LinechartWidget::appendData(int uasId, const QString& curve, const QString& // Log data if (logging) { - if (activePlot->isVisible(curve)) + if (activePlot->isVisible(curve+unit)) { if (logStartTime == 0) logStartTime = usec; qint64 time = usec - logStartTime; @@ -416,7 +427,7 @@ void LinechartWidget::refresh() QMap::iterator j; for (j = curveMeans->begin(); j != curveMeans->end(); ++j) { - double val = activePlot->getCurrentValue(j.key()); + double val = activePlot->getMean(j.key()); int intval = static_cast(val); if (intval >= 100000 || intval <= -100000) { @@ -660,6 +671,7 @@ void LinechartWidget::addCurve(const QString& curve, const QString& unit) // TODO // Connect actions + connect(selectAllCheckBox, SIGNAL(clicked(bool)), checkBox, SLOT(setChecked(bool))); QObject::connect(checkBox, SIGNAL(clicked(bool)), this, SLOT(takeButtonClick(bool))); QObject::connect(this, SIGNAL(curveVisible(QString, bool)), plot, SLOT(setVisible(QString, bool))); diff --git a/src/ui/linechart/LinechartWidget.h b/src/ui/linechart/LinechartWidget.h index d654a12c0952af285463915f8d975c3580ba2ac4..0ba2f0495c34665374a4e4ac7e917bc16f729067 100644 --- a/src/ui/linechart/LinechartWidget.h +++ b/src/ui/linechart/LinechartWidget.h @@ -99,6 +99,8 @@ public slots: void writeSettings(); /** @brief Read the current configuration from disk */ void readSettings(); + /** @brief Select all curves */ + void selectAllCurves(bool all); protected: void addCurveToList(QString curve); @@ -145,6 +147,7 @@ protected: quint64 logStartTime; QTimer* updateTimer; LogCompressor* compressor; + QCheckBox* selectAllCheckBox; static const int updateInterval = 400; ///< Time between number updates, in milliseconds static const int MAX_CURVE_MENUITEM_NUMBER = 8; diff --git a/src/ui/linechart/Linecharts.cc b/src/ui/linechart/Linecharts.cc index 6fc366537a0e6501937a9393ed2bb498435fcca2..0ffea43d91e68041b33af138b92ca8fa4895932e 100644 --- a/src/ui/linechart/Linecharts.cc +++ b/src/ui/linechart/Linecharts.cc @@ -101,7 +101,7 @@ void Linecharts::addSystem(UASInterface* uas) // Values with unit as double connect(uas, SIGNAL(valueChanged(int,QString,QString,double,quint64)), widget, SLOT(appendData(int,QString,QString,double,quint64))); // Values with unit as integer - connect(uas, SIGNAL(valueChanged(int,QString,QString,double,quint64)), widget, SLOT(appendData(int,QString,QString,double,quint64))); + connect(uas, SIGNAL(valueChanged(int,QString,QString,int,quint64)), widget, SLOT(appendData(int,QString,QString,int,quint64))); #endif connect(widget, SIGNAL(logfileWritten(QString)), this, SIGNAL(logfileWritten(QString))); // Set system active if this is the only system diff --git a/src/ui/map/MAV2DIcon.cc b/src/ui/map/MAV2DIcon.cc index 18440d69719dbaec57658f31f865d430760d3365..628c1425543dff8e0d7ccfe79d9c074b1914ca18 100644 --- a/src/ui/map/MAV2DIcon.cc +++ b/src/ui/map/MAV2DIcon.cc @@ -3,6 +3,8 @@ #include +#include "QGC.h" + MAV2DIcon::MAV2DIcon(UASInterface* uas, int radius, int type, const QColor& color, QString name, Alignment alignment, QPen* pen) : Point(uas->getLatitude(), uas->getLongitude(), name, alignment), yaw(0.0f), @@ -57,12 +59,12 @@ void MAV2DIcon::setYaw(float yaw) { //qDebug() << "MAV2Icon" << yaw; float diff = fabs(yaw - this->yaw); - while (diff > M_PI) + while (diff > (float)M_PI) { - diff -= M_PI; + diff -= (float)M_PI; } - if (diff > 0.1) + if (diff > 0.1f) { this->yaw = yaw; drawIcon(mypen); @@ -99,7 +101,11 @@ void MAV2DIcon::drawIcon(QPen* pen) //qDebug() << "Painting ellipse" << radius/2-width << width; //selPen->deleteLater(); } + drawAirframePolygon(airframe, painter, radius, iconColor, yaw); +} +void MAV2DIcon::drawAirframePolygon(int airframe, QPainter& painter, int radius, QColor& iconColor, float yaw) +{ switch (airframe) { case UASInterface::QGC_AIRFRAME_PREDATOR: @@ -108,7 +114,10 @@ void MAV2DIcon::drawIcon(QPen* pen) // DRAW AIRPLANE // Rotate 180 degs more since the icon is oriented downwards - float yawRotate = (yaw/(float)M_PI)*180.0f + 180.0f+180.0f; + //float yawRotate = (yaw/(float)M_PI)*180.0f + 180.0f+180.0f; + + const float yawDeg = ((yaw/M_PI)*180.0f)+180.0f+180.0f; + int yawRotate = static_cast(yawDeg) % 360; painter.rotate(yawRotate); @@ -147,6 +156,7 @@ void MAV2DIcon::drawIcon(QPen* pen) painter.setPen(iconPen); painter.drawPolygon(poly); + painter.rotate(-yawRotate); } break; case UASInterface::QGC_AIRFRAME_MIKROKOPTER: @@ -154,7 +164,8 @@ void MAV2DIcon::drawIcon(QPen* pen) { // QUADROTOR float iconSize = radius*0.9f; - float yawRotate = (yaw/(float)M_PI)*180.0f + 180.0f; + const float yawDeg = ((yaw/M_PI)*180.0f)+180.0f; + int yawRotate = static_cast(yawDeg) % 360; painter.rotate(yawRotate); @@ -186,6 +197,7 @@ void MAV2DIcon::drawIcon(QPen* pen) painter.setBrush(Qt::red); painter.drawEllipse(front, radius/4/2, radius/4/2); + painter.rotate(-yawRotate); } break; case UASInterface::QGC_AIRFRAME_EASYSTAR: @@ -195,8 +207,8 @@ void MAV2DIcon::drawIcon(QPen* pen) { // DRAW AIRPLANE - float yawRotate = (yaw/(float)M_PI)*180.0f + 180.0f; - + const float yawDeg = ((yaw/M_PI)*180.0f)+180.0f; + int yawRotate = static_cast(yawDeg) % 360; painter.rotate(yawRotate); //qDebug() << "ICON SIZE:" << radius; @@ -243,6 +255,7 @@ void MAV2DIcon::drawIcon(QPen* pen) painter.setPen(iconPen); painter.drawPolygon(poly); + painter.rotate(-yawRotate); } break; case UASInterface::QGC_AIRFRAME_GENERIC: @@ -268,6 +281,7 @@ void MAV2DIcon::drawIcon(QPen* pen) painter.setPen(iconPen); painter.drawPolygon(poly); + painter.rotate(-yawRotate); } break; } diff --git a/src/ui/map/MAV2DIcon.h b/src/ui/map/MAV2DIcon.h index 071d71bf5adba745bd37a45ca4ee4376d4981111..6a447ffe381839276d578885b7cc18c89efcda34 100644 --- a/src/ui/map/MAV2DIcon.h +++ b/src/ui/map/MAV2DIcon.h @@ -61,6 +61,7 @@ public: void drawIcon(QPen* pen); void drawIcon() { drawIcon(mypen); } + static void drawAirframePolygon(int airframe, QPainter& painter, int radius, QColor& iconColor, float yaw); protected: float yaw; ///< Yaw angle of the MAV diff --git a/src/ui/map3D/QGCGoogleEarthView.cc b/src/ui/map3D/QGCGoogleEarthView.cc index 52cbb30f0b825769eb144c9eeaba930756e57b27..c4813948321dd83c26cbc5d72ac5a1e8ddd137b7 100644 --- a/src/ui/map3D/QGCGoogleEarthView.cc +++ b/src/ui/map3D/QGCGoogleEarthView.cc @@ -11,6 +11,7 @@ #ifdef Q_OS_MAC #include #include +#include #include "QGCWebPage.h" #endif @@ -23,35 +24,33 @@ #include "QGC.h" #include "ui_QGCGoogleEarthView.h" #include "QGCGoogleEarthView.h" +#include "UASWaypointManager.h" #define QGCGOOGLEEARTHVIEWSETTINGS QString("GoogleEarthViewSettings_") QGCGoogleEarthView::QGCGoogleEarthView(QWidget *parent) : QWidget(parent), updateTimer(new QTimer(this)), - refreshRateMs(40), + refreshRateMs(100), mav(NULL), followCamera(true), trailEnabled(true), webViewInitialized(false), jScriptInitialized(false), gEarthInitialized(false), + currentViewMode(QGCGoogleEarthView::VIEW_MODE_SIDE), #if (defined Q_OS_MAC) webViewMac(new QWebView(this)), #endif #ifdef _MSC_VER webViewWin(new QGCWebAxWidget(this)), + documentWin(NULL), #endif -#if (defined _MSC_VER) ui(new Ui::QGCGoogleEarthView) -#else - ui(new Ui::QGCGoogleEarthView) -#endif { #ifdef _MSC_VER // Create layout and attach webViewWin - QFile file("doc.html"); if (!file.open(QIODevice::WriteOnly | QIODevice::Text)) qDebug() << __FILE__ << __LINE__ << "Could not open log file"; @@ -82,6 +81,11 @@ QGCGoogleEarthView::QGCGoogleEarthView(QWidget *parent) : #endif connect(updateTimer, SIGNAL(timeout()), this, SLOT(updateState())); + connect(ui->resetButton, SIGNAL(clicked()), this, SLOT(reloadHTML())); + connect(ui->changeViewButton, SIGNAL(clicked()), this, SLOT(toggleViewMode())); + connect(ui->clearTrailsButton, SIGNAL(clicked()), this, SLOT(clearTrails())); + connect(ui->atmosphereCheckBox, SIGNAL(clicked(bool)), this, SLOT(enableAtmosphere(bool))); + connect(ui->daylightCheckBox, SIGNAL(clicked(bool)), this, SLOT(enableDaylight(bool))); } QGCGoogleEarthView::~QGCGoogleEarthView() @@ -107,6 +111,30 @@ void QGCGoogleEarthView::setViewRangeScaledInt(int range) setViewRange(range/100.0f); } +void QGCGoogleEarthView::reloadHTML() +{ + hide(); + webViewInitialized = false; + jScriptInitialized = false; + gEarthInitialized = false; + show(); +} + +void QGCGoogleEarthView::enableEditMode(bool mode) +{ + javaScript(QString("setDraggingAllowed(%1);").arg(mode)); +} + +void QGCGoogleEarthView::enableDaylight(bool enable) +{ + javaScript(QString("enableDaylight(%1);").arg(enable)); +} + +void QGCGoogleEarthView::enableAtmosphere(bool enable) +{ + javaScript(QString("enableAtmosphere(%1);").arg(enable)); +} + /** * @param range in meters (SI-units) */ @@ -115,6 +143,51 @@ void QGCGoogleEarthView::setViewRange(float range) javaScript(QString("setViewRange(%1);").arg(range, 0, 'f', 5)); } +void QGCGoogleEarthView::setDistanceMode(int mode) +{ + javaScript(QString("setDistanceMode(%1);").arg(mode)); +} + +void QGCGoogleEarthView::toggleViewMode() +{ + switch (currentViewMode) + { + case VIEW_MODE_MAP: + setViewMode(VIEW_MODE_SIDE); + break; + case VIEW_MODE_SIDE: + setViewMode(VIEW_MODE_MAP); + break; + case VIEW_MODE_CHASE_LOCKED: + setViewMode(VIEW_MODE_CHASE_FREE); + break; + case VIEW_MODE_CHASE_FREE: + setViewMode(VIEW_MODE_CHASE_LOCKED); + break; + } +} + +void QGCGoogleEarthView::setViewMode(QGCGoogleEarthView::VIEW_MODE mode) +{ + switch (mode) + { + case VIEW_MODE_MAP: + ui->changeViewButton->setText("Free View"); + break; + case VIEW_MODE_SIDE: + ui->changeViewButton->setText("Map View"); + break; + case VIEW_MODE_CHASE_LOCKED: + ui->changeViewButton->setText("Free Chase"); + break; + case VIEW_MODE_CHASE_FREE: + ui->changeViewButton->setText("Fixed Chase"); + break; + } + currentViewMode = mode; + javaScript(QString("setViewMode(%1);").arg(mode)); +} + void QGCGoogleEarthView::addUAS(UASInterface* uas) { // uasid, type, color (in #rrbbgg format) @@ -129,9 +202,14 @@ void QGCGoogleEarthView::addUAS(UASInterface* uas) if (trailEnabled) javaScript(QString("showTrail(%1);").arg(uas->getUASID())); - //javaScript(QString("createAircraft(%1, %2, %3);").arg(uas->getUASID()).arg(uas->getSystemType()).arg("0")); // Automatically receive further position updates connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64))); + // Receive waypoint updates + // Connect the waypoint manager / data storage to the UI + connect(uas->getWaypointManager(), SIGNAL(waypointListChanged(int)), this, SLOT(updateWaypointList(int))); + connect(uas->getWaypointManager(), SIGNAL(waypointChanged(int, Waypoint*)), this, SLOT(updateWaypoint(int,Waypoint*))); + //connect(this, SIGNAL(waypointCreated(Waypoint*)), uas->getWaypointManager(), SLOT(addWaypoint(Waypoint*))); + // TODO Update waypoint list on UI changes here } void QGCGoogleEarthView::setActiveUAS(UASInterface* uas) @@ -140,17 +218,82 @@ void QGCGoogleEarthView::setActiveUAS(UASInterface* uas) { mav = uas; javaScript(QString("setCurrAircraft(%1);").arg(uas->getUASID())); + updateWaypointList(uas->getUASID()); + } +} + +/** + * This function is called if a a single waypoint is updated and + * also if the whole list changes. + */ +void QGCGoogleEarthView::updateWaypoint(int uas, Waypoint* wp) +{ + // Only accept waypoints in global coordinate frame + if (wp->getFrame() == MAV_FRAME_GLOBAL) + { + // We're good, this is a global waypoint + + // Get the index of this waypoint + // note the call to getGlobalFrameIndexOf() + // as we're only handling global waypoints + int wpindex = UASManager::instance()->getUASForId(uas)->getWaypointManager()->getGlobalFrameIndexOf(wp); + // If not found, return (this should never happen, but helps safety) + if (wpindex == -1) + { + return; + } + else + { + javaScript(QString("updateWaypoint(%1,%2,%3,%4,%5,%6);").arg(uas).arg(wpindex).arg(wp->getLatitude(), 0, 'f', 18).arg(wp->getLongitude(), 0, 'f', 18).arg(wp->getAltitude(), 0, 'f', 18).arg(wp->getAction())); + //qDebug() << QString("updateWaypoint(%1,%2,%3,%4,%5,%6);").arg(uas).arg(wpindex).arg(wp->getLatitude(), 0, 'f', 18).arg(wp->getLongitude(), 0, 'f', 18).arg(wp->getAltitude(), 0, 'f', 18).arg(wp->getAction()); + } + } +} + +/** + * Update the whole list of waypoints. This is e.g. necessary if the list order changed. + * The UAS manager will emit the appropriate signal whenever updating the list + * is necessary. + */ +void QGCGoogleEarthView::updateWaypointList(int uas) +{ + // Get already existing waypoints + UASInterface* uasInstance = UASManager::instance()->getUASForId(uas); + if (uasInstance) + { + // Get all waypoints, including non-global waypoints + QVector wpList = uasInstance->getWaypointManager()->getGlobalFrameWaypointList(); + + // Trim internal list to number of global waypoints in the waypoint manager list + javaScript(QString("updateWaypointListLength(%1,%2);").arg(uas).arg(wpList.count())); + + qDebug() << QString("updateWaypointListLength(%1,%2);").arg(uas).arg(wpList.count()); + + // Load all existing waypoints into map view + foreach (Waypoint* wp, wpList) + { + updateWaypoint(uas, wp); + } } } -void QGCGoogleEarthView::updateGlobalPosition(UASInterface* uas, double lon, double lat, double alt, quint64 usec) +void QGCGoogleEarthView::updateGlobalPosition(UASInterface* uas, double lat, double lon, double alt, quint64 usec) { Q_UNUSED(usec); - javaScript(QString("addTrailPosition(%1, %2, %3, %4);").arg(uas->getUASID()).arg(lat, 0, 'f', 15).arg(lon, 0, 'f', 15).arg(alt, 0, 'f', 15)); + javaScript(QString("addTrailPosition(%1, %2, %3, %4);").arg(uas->getUASID()).arg(lat, 0, 'f', 18).arg(lon, 0, 'f', 18).arg(alt, 0, 'f', 15)); //qDebug() << QString("addTrailPosition(%1, %2, %3, %4);").arg(uas->getUASID()).arg(lat, 0, 'f', 15).arg(lon, 0, 'f', 15).arg(alt, 0, 'f', 15); } +void QGCGoogleEarthView::clearTrails() +{ + QList mavs = UASManager::instance()->getUASList(); + foreach (UASInterface* currMav, mavs) + { + javaScript(QString("clearTrail(%1);").arg(currMav->getUASID())); + } +} + void QGCGoogleEarthView::showTrail(bool state) { // Check if the current trail has to be hidden @@ -184,8 +327,19 @@ void QGCGoogleEarthView::showWaypoints(bool state) void QGCGoogleEarthView::follow(bool follow) { ui->followAirplaneCheckbox->setChecked(follow); + if (follow != followCamera) + { + if (follow) + { + setViewMode(VIEW_MODE_CHASE_LOCKED); + } + else + { + setViewMode(VIEW_MODE_SIDE); + } + } followCamera = follow; - if (gEarthInitialized) javaScript(QString("setFollowEnabled(%1)").arg(follow)); + javaScript(QString("setFollowEnabled(%1)").arg(follow)); } void QGCGoogleEarthView::goHome() @@ -248,7 +402,14 @@ void QGCGoogleEarthView::printWinException(int no, QString str1, QString str2, Q QVariant QGCGoogleEarthView::javaScript(QString javaScript) { #ifdef Q_OS_MAC - return webViewMac->page()->currentFrame()->evaluateJavaScript(javaScript); + if (!gEarthInitialized) + { + return QVariant(false); + } + else + { + return webViewMac->page()->currentFrame()->evaluateJavaScript(javaScript); + } #endif #ifdef _MSC_VER if(!jScriptInitialized) @@ -261,7 +422,43 @@ QVariant QGCGoogleEarthView::javaScript(QString javaScript) QVariantList params; params.append(javaScript); params.append("JScript"); - return jScriptWin->dynamicCall("execScript(QString, QString)", params); + QVariant result = jScriptWin->dynamicCall("execScript(QString, QString)", params); + return result; + } +#endif +} + +QVariant QGCGoogleEarthView::documentElement(QString name) +{ +#ifdef Q_OS_MAC + QString javaScript("getGlobal(%1)"); + QVariant result = webViewMac->page()->currentFrame()->evaluateJavaScript(javaScript.arg(name)); + return result; +#endif +#ifdef _MSC_VER + if(!jScriptInitialized) + { + qDebug() << "TOO EARLY JAVASCRIPT CALL, ABORTING"; + return QVariant(false); + } + else + { + QVariantList params; + QString javaScript("getGlobal(%1)"); + params.append(javaScript.arg(name)); + params.append("JScript"); + QVariant result = jScriptWin->dynamicCall("execScript(QString, QString)", params); + qDebug() << "JScript result: " << result << result.toDouble(); +// if (documentWin) +// { +// // Get HTMLElement object +// QVariantList params; +// params.append(name); +// //QAxObject* elementWin = documentWin->dynamicCall("getElementById(QString)", params); +// QVariant result =documentWin->dynamicCall("toString()"); +// qDebug() << "GOT RESULT" << result; +// return QVariant(0);//QVariant(result); +// } } #endif } @@ -275,8 +472,9 @@ void QGCGoogleEarthView::initializeGoogleEarth() #endif #ifdef _MSC_VER QAxObject* doc = webViewWin->querySubObject("Document()"); - IDispatch* Disp; + //IDispatch* Disp; IDispatch* winDoc = NULL; + IHTMLDocument2* document = NULL; //332C4425-26CB-11D0-B483-00C04FD90119 IHTMLDocument2 //25336920-03F9-11CF-8FD0-00AA00686F13 HTMLDocument @@ -287,11 +485,11 @@ void QGCGoogleEarthView::initializeGoogleEarth() // CoInternetSetFeatureEnabled // (FEATURE_LOCALMACHINE_LOCKDOWN, SET_FEATURE_ON_PROCESS, TRUE); // - IHTMLDocument2* document = NULL; + document = NULL; winDoc->QueryInterface( IID_IHTMLDocument2, (void**)&document ); IHTMLWindow2 *window = NULL; document->get_parentWindow( &window ); - + documentWin = new QAxObject(document, webViewWin); jScriptWin = new QAxObject(window, webViewWin); connect(jScriptWin, SIGNAL(exception(int,QString,QString,QString)), this, SLOT(printWinException(int,QString,QString,QString))); jScriptInitialized = true; @@ -314,6 +512,8 @@ void QGCGoogleEarthView::initializeGoogleEarth() } else { + gEarthInitialized = true; + // Set home location setHome(47.3769, 8.549444, 500); @@ -331,32 +531,44 @@ void QGCGoogleEarthView::initializeGoogleEarth() setActiveUAS(UASManager::instance()->getActiveUAS()); // Add any further MAV automatically - connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)), this, SLOT(addUAS(UASInterface*))); - connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*))); + connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)), this, SLOT(addUAS(UASInterface*)), Qt::UniqueConnection); + connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*)), Qt::UniqueConnection); // Connect UI signals/slots // Follow checkbox ui->followAirplaneCheckbox->setChecked(followCamera); - connect(ui->followAirplaneCheckbox, SIGNAL(toggled(bool)), this, SLOT(follow(bool))); + connect(ui->followAirplaneCheckbox, SIGNAL(toggled(bool)), this, SLOT(follow(bool)), Qt::UniqueConnection); // Trail checkbox ui->trailCheckbox->setChecked(trailEnabled); - connect(ui->trailCheckbox, SIGNAL(toggled(bool)), this, SLOT(showTrail(bool))); + connect(ui->trailCheckbox, SIGNAL(toggled(bool)), this, SLOT(showTrail(bool)), Qt::UniqueConnection); // Go home connect(ui->goHomeButton, SIGNAL(clicked()), this, SLOT(goHome())); // Cam distance slider - connect(ui->camDistanceSlider, SIGNAL(valueChanged(int)), this, SLOT(setViewRangeScaledInt(int))); + connect(ui->camDistanceSlider, SIGNAL(valueChanged(int)), this, SLOT(setViewRangeScaledInt(int)), Qt::UniqueConnection); setViewRangeScaledInt(ui->camDistanceSlider->value()); + // Distance combo box + connect(ui->camDistanceComboBox, SIGNAL(currentIndexChanged(int)), this, SLOT(setDistanceMode(int)), Qt::UniqueConnection); + // Edit mode button + connect(ui->editButton, SIGNAL(clicked(bool)), this, SLOT(enableEditMode(bool)), Qt::UniqueConnection); + + // Update waypoint list + if (mav) updateWaypointList(mav->getUASID()); + // Start update timer updateTimer->start(refreshRateMs); + // Set current view mode + setViewMode(currentViewMode); + setDistanceMode(ui->camDistanceComboBox->currentIndex()); + enableEditMode(ui->editButton->isChecked()); + enableAtmosphere(ui->atmosphereCheckBox->isChecked()); + enableDaylight(ui->daylightCheckBox->isChecked()); follow(this->followCamera); - - gEarthInitialized = true; } } } @@ -389,6 +601,8 @@ void QGCGoogleEarthView::updateState() pitch = currMav->getPitch(); yaw = currMav->getYaw(); + //qDebug() << "SETTING POSITION FOR" << uasId << lat << lon << alt << roll << pitch << yaw; + javaScript(QString("setAircraftPositionAttitude(%1, %2, %3, %4, %6, %7, %8);") .arg(uasId) .arg(lat, 0, 'f', 15) @@ -398,6 +612,93 @@ void QGCGoogleEarthView::updateState() .arg(pitch, 0, 'f', 9) .arg(yaw, 0, 'f', 9)); } + + + // Read out new waypoint positions and waypoint create events + // this is polling (bad) but forced because of the crappy + // Microsoft API available in Qt - improvements wanted + + // First check if a new WP should be created +// bool newWaypointPending = .to + bool newWaypointPending = documentElement("newWaypointPending").toBool(); + if (newWaypointPending) + { + bool coordsOk = true; + bool ok; + double latitude = documentElement("newWaypointLatitude").toDouble(&ok); + coordsOk &= ok; + double longitude = documentElement("newWaypointLongitude").toDouble(&ok); + coordsOk &= ok; + double altitude = documentElement("newWaypointAltitude").toDouble(&ok); + coordsOk &= ok; + if (coordsOk) + { + // Add new waypoint + if (mav) + { + int nextIndex = mav->getWaypointManager()->getWaypointList().count(); + Waypoint* wp = new Waypoint(nextIndex, latitude, longitude, altitude, true); + wp->setFrame(MAV_FRAME_GLOBAL); +// wp.setLatitude(latitude); +// wp.setLongitude(longitude); +// wp.setAltitude(altitude); + mav->getWaypointManager()->addWaypoint(wp); + } + } + javaScript("setNewWaypointPending(false);"); + } + + // Check if a waypoint should be moved + bool dragWaypointPending = documentElement("dragWaypointPending").toBool(); + + if (dragWaypointPending) + { + bool coordsOk = true; + bool ok; + double latitude = documentElement("dragWaypointLatitude").toDouble(&ok); + coordsOk &= ok; + double longitude = documentElement("dragWaypointLongitude").toDouble(&ok); + coordsOk &= ok; + double altitude = documentElement("dragWaypointAltitude").toDouble(&ok); + coordsOk &= ok; + + // UPDATE WAYPOINTS, HOME LOCATION AND OTHER LOCATIONS + if (coordsOk) + { + QString idText = documentElement("dragWaypointIndex").toString(); + if (idText == "HOME") + { + setHome(latitude, longitude, altitude); + } + else + { + // Update waypoint or symbol + if (mav) + { + QVector wps = mav->getWaypointManager()->getGlobalFrameWaypointList(); + + bool ok; + int index = idText.toInt(&ok); + + if (ok && index >= 0 && index < wps.count()) + { + Waypoint* wp = wps.at(index); + wp->setLatitude(latitude); + wp->setLongitude(longitude); + wp->setAltitude(altitude); + mav->getWaypointManager()->notifyOfChange(wp); + } + } + } + } + else + { + // If coords were not ok, move the view in google earth back + // to last acceptable location + updateWaypointList(mav->getUASID()); + } + javaScript("setDragWaypointPending(false);"); + } } } diff --git a/src/ui/map3D/QGCGoogleEarthView.h b/src/ui/map3D/QGCGoogleEarthView.h index 80131dc7ed1079d3a23d133a797a2a4169c0450f..f4eb4c3ac4cb615f7ca75a3ee962bd5cb99d9395 100644 --- a/src/ui/map3D/QGCGoogleEarthView.h +++ b/src/ui/map3D/QGCGoogleEarthView.h @@ -58,13 +58,11 @@ protected: #endif namespace Ui { -#ifdef _MSC_VER - class QGCGoogleEarthView; -#else class QGCGoogleEarthView; -#endif } +class Waypoint; + class QGCGoogleEarthView : public QWidget { Q_OBJECT @@ -73,6 +71,14 @@ public: explicit QGCGoogleEarthView(QWidget *parent = 0); ~QGCGoogleEarthView(); + enum VIEW_MODE + { + VIEW_MODE_SIDE = 0, ///< View from above, orientation is free + VIEW_MODE_MAP, ///< View from above, orientation is north (map view) + VIEW_MODE_CHASE_LOCKED, ///< Locked chase camera + VIEW_MODE_CHASE_FREE, ///< Position is chasing object, but view can rotate around object + }; + public slots: /** @brief Update the internal state. Does not trigger a redraw */ void updateState(); @@ -81,9 +87,15 @@ public slots: /** @brief Set the currently selected UAS */ void setActiveUAS(UASInterface* uas); /** @brief Update the global position */ - void updateGlobalPosition(UASInterface* uas, double lon, double lat, double alt, quint64 usec); + void updateGlobalPosition(UASInterface* uas, double lat, double lon, double alt, quint64 usec); + /** @brief Update a single waypoint */ + void updateWaypoint(int uas, Waypoint* wp); + /** @brief Update the waypoint list */ + void updateWaypointList(int uas); /** @brief Show the vehicle trail */ void showTrail(bool state); + /** @brief Clear the current vehicle trails and start with new ones */ + void clearTrails(); /** @brief Show the waypoints */ void showWaypoints(bool state); /** @brief Follow the aircraft during flight */ @@ -92,10 +104,24 @@ public slots: void goHome(); /** @brief Set the home location */ void setHome(double lat, double lon, double alt); + /** @brief Allow waypoint editing */ + void enableEditMode(bool mode); + /** @brief Enable daylight/night */ + void enableDaylight(bool enable); + /** @brief Enable atmosphere */ + void enableAtmosphere(bool enable); /** @brief Set camera view range to aircraft in meters */ void setViewRange(float range); + /** @brief Set the distance mode - either to ground or to MAV */ + void setDistanceMode(int mode); + /** @brief Set the view mode */ + void setViewMode(VIEW_MODE mode); + /** @brief Toggle through the different view modes */ + void toggleViewMode(); /** @brief Set camera view range to aircraft in centimeters */ void setViewRangeScaledInt(int range); + /** @brief Reset Google Earth View */ + void reloadHTML(); /** @brief Initialize Google Earth */ void initializeGoogleEarth(); @@ -105,6 +131,8 @@ public slots: public: /** @brief Execute java script inside the Google Earth window */ QVariant javaScript(QString javascript); + /** @brief Get a document element */ + QVariant documentElement(QString name); protected: void changeEvent(QEvent *e); @@ -117,9 +145,11 @@ protected: bool webViewInitialized; bool jScriptInitialized; bool gEarthInitialized; + VIEW_MODE currentViewMode; #ifdef _MSC_VER QGCWebAxWidget* webViewWin; QAxObject* jScriptWin; + QAxObject* documentWin; #endif #if (defined Q_OS_MAC) QWebView* webViewMac; diff --git a/src/ui/map3D/QGCGoogleEarthView.ui b/src/ui/map3D/QGCGoogleEarthView.ui index 9fd726a4a071f113cdac3920bc17baffa3dd6acd..cfe038858f141833c7194a349234cd661e46f6e3 100644 --- a/src/ui/map3D/QGCGoogleEarthView.ui +++ b/src/ui/map3D/QGCGoogleEarthView.ui @@ -6,14 +6,14 @@ 0 0 - 828 - 300 + 1089 + 302 Form - + 8 @@ -23,7 +23,7 @@ 2 - + @@ -58,20 +58,23 @@ - + Qt::Horizontal + + QSizePolicy::Expanding + - 177 + 5 20 - + Distance of the chase camera to the MAV @@ -97,86 +100,127 @@ - + - Distance of the chase camera to the MAV + Chase the MAV - Distance of the chase camera to the MAV + Chase the MAV - Distance of the chase camera to the MAV + Chase the MAV - Dist + Follow - - + + - Show waypoints + Select the MAV to chase - Show waypoints + Select the MAV to chase - Show waypoints + Select the MAV to chase + + + MAV Distance + + + + + Ground Distance + + + + + + + + Qt::Vertical + + + + + - WPs + Reset - - + + + + Overhead + + + + + - Chase the MAV + Delete all waypoints - Chase the MAV + Delete all waypoints - Chase the MAV + Delete all waypoints - Follow + Clear Trails - - + + + + Qt::Vertical + + + + + - Select the MAV to chase + Enable waypoint and home location edit mode - Select the MAV to chase + Enable waypoint and home location edit mode - - Select the MAV to chase + + Edit + + + true - - + + - Delete all waypoints + Enable the atmosphere visualization. Not recommended for mission execution, only for visualization - Delete all waypoints - - - Delete all waypoints + Enable the atmosphere visualization. Not recommended for mission execution, only for visualization - Clear WPs + Sky - - - - Qt::Vertical + + + + Enable day/night light. Not recommended for mission execution, only for visualization. + + + Enable day/night light. Not recommended for mission execution, only for visualization. + + + Day/Night diff --git a/src/ui/map3D/QGCGoogleEarthViewWin.ui b/src/ui/map3D/QGCGoogleEarthViewWin.ui deleted file mode 100644 index 6191eaebb2358c8b8934b41f4dd4fc5307842172..0000000000000000000000000000000000000000 --- a/src/ui/map3D/QGCGoogleEarthViewWin.ui +++ /dev/null @@ -1,19 +0,0 @@ - - - QGCGoogleEarthViewWin - - - - 0 - 0 - 400 - 300 - - - - Form - - - - - diff --git a/src/ui/map3D/WaypointGroupNode.cc b/src/ui/map3D/WaypointGroupNode.cc index deb08f643c229d1d8312c649a10c78effa37b2c0..3ca8834a4d82ae16bab0b17742776e04690d765e 100644 --- a/src/ui/map3D/WaypointGroupNode.cc +++ b/src/ui/map3D/WaypointGroupNode.cc @@ -90,7 +90,7 @@ WaypointGroupNode::update(MAV_FRAME frame, UASInterface *uas) osg::ref_ptr sd = new osg::ShapeDrawable; osg::ref_ptr cylinder = new osg::Cylinder(osg::Vec3d(0.0, 0.0, -wpZ / 2.0), - wp->getOrbit(), + wp->getAcceptanceRadius(), fabs(wpZ)); sd->setShape(cylinder); diff --git a/src/ui/mission/QGCCustomWaypointAction.ui b/src/ui/mission/QGCCustomWaypointAction.ui new file mode 100644 index 0000000000000000000000000000000000000000..5c98f8980a88bec21661814b2867855a6fe4ca3c --- /dev/null +++ b/src/ui/mission/QGCCustomWaypointAction.ui @@ -0,0 +1,134 @@ + + + QGCCustomWaypointAction + + + + 0 + 0 + 1105 + 25 + + + + Form + + + + 5 + + + 0 + + + + + CMD + + + 0 + + + 255 + + + 0 + + + + + + + P1 + + + -2147483647.000000000000000 + + + 2147483647.000000000000000 + + + + + + + P2 + + + -2147483647.000000000000000 + + + 2147483647.000000000000000 + + + + + + + P3 + + + -2147483647.000000000000000 + + + 2147483647.000000000000000 + + + + + + + P4 + + + -2147483647.000000000000000 + + + 2147483647.000000000000000 + + + + + + + P5 + + + -2147483647.000000000000000 + + + 2147483647.000000000000000 + + + + + + + P6 + + + -2147483647.000000000000000 + + + 2147483647.000000000000000 + + + + + + + P7 + + + -2147483647.000000000000000 + + + 2147483647.000000000000000 + + + + + + + + diff --git a/src/ui/uas/UASView.cc b/src/ui/uas/UASView.cc index 59de83d6a739b1289fbfc4d1a295a4c933e0d399..bb30a918d0143fd0f07979f4ffd22e5b76a807e0 100644 --- a/src/ui/uas/UASView.cc +++ b/src/ui/uas/UASView.cc @@ -66,8 +66,9 @@ UASView::UASView(UASInterface* uas, QWidget *parent) : localFrame(false), removeAction(new QAction("Delete this system", this)), renameAction(new QAction("Rename..", this)), - selectAction(new QAction("Select this system", this )), - selectAirframeAction(new QAction("Select Airframe", this)), + selectAction(new QAction("Control this system", this )), + selectAirframeAction(new QAction("Choose Airframe", this)), + setBatterySpecsAction(new QAction("Set Battery Options", this)), m_ui(new Ui::UASView) { m_ui->setupUi(this); @@ -88,7 +89,9 @@ UASView::UASView(UASInterface* uas, QWidget *parent) : connect(uas->getWaypointManager(), SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointUpdated(quint16))); connect(uas, SIGNAL(systemTypeSet(UASInterface*,uint)), this, SLOT(setSystemType(UASInterface*,uint))); connect(UASManager::instance(), SIGNAL(activeUASStatusChanged(UASInterface*,bool)), this, SLOT(updateActiveUAS(UASInterface*,bool))); - + connect(uas, SIGNAL(textMessageReceived(int,int,int,QString)), this, SLOT(showStatusText(int, int, int, QString))); + connect(uas, SIGNAL(navModeChanged(int, int, QString)), this, SLOT(updateNavMode(int, int, QString))); + // Setup UAS selection connect(m_ui->uasViewFrame, SIGNAL(clicked(bool)), this, SLOT(setUASasActive(bool))); @@ -106,6 +109,7 @@ UASView::UASView(UASInterface* uas, QWidget *parent) : connect(renameAction, SIGNAL(triggered()), this, SLOT(rename())); connect(selectAction, SIGNAL(triggered()), uas, SLOT(setSelected())); connect(selectAirframeAction, SIGNAL(triggered()), this, SLOT(selectAirframe())); + connect(setBatterySpecsAction, SIGNAL(triggered()), this, SLOT(setBatterySpecs())); connect(uas, SIGNAL(systemRemoved()), this, SLOT(deleteLater())); // Name changes @@ -128,6 +132,7 @@ UASView::UASView(UASInterface* uas, QWidget *parent) : // Heartbeat fade refreshTimer = new QTimer(this); connect(refreshTimer, SIGNAL(timeout()), this, SLOT(refresh())); + refreshTimer->start(updateInterval); // Hide kill and shutdown buttons per default m_ui->killButton->hide(); @@ -149,6 +154,22 @@ void UASView::heartbeatTimeout() timeout = true; } +void UASView::updateNavMode(int uasid, int mode, const QString& text) +{ + Q_UNUSED(uasid); + Q_UNUSED(mode); + m_ui->navLabel->setText(text); +} + +void UASView::showStatusText(int uasid, int componentid, int severity, QString text) +{ + Q_UNUSED(uasid); + Q_UNUSED(componentid); + Q_UNUSED(severity); + //m_ui->statusTextLabel->setText(text); + stateDesc = text; +} + /** * Set the background color based on the MAV color. If the MAV is selected as the * currently actively controlled system, the frame color is highlighted @@ -250,7 +271,7 @@ void UASView::receiveHeartbeat(UASInterface* uas) { Q_UNUSED(uas); heartbeatColor = QColor(20, 200, 20); - QString colorstyle("QGroupBox { border-radius: 5px; padding: 2px; margin: 2px; border: 0px; background-color: %1; }"); + QString colorstyle("QGroupBox { border-radius: 5px; padding: 2px; margin: 0px; border: 0px; background-color: %1; }"); m_ui->heartbeatIcon->setStyleSheet(colorstyle.arg(heartbeatColor.name())); if (timeout) setBackgroundColor(); timeout = false; @@ -345,7 +366,7 @@ void UASView::updateGlobalPosition(UASInterface* uas, double lon, double lat, do void UASView::updateSpeed(UASInterface*, double x, double y, double z, quint64 usec) { Q_UNUSED(usec); - totalSpeed = sqrt((pow(x, 2) + pow(y, 2) + pow(z, 2))); + totalSpeed = sqrt(x*x + y*y + z*z); } void UASView::currentWaypointUpdated(quint16 waypoint) @@ -415,16 +436,31 @@ void UASView::updateLoad(UASInterface* uas, double load) void UASView::contextMenuEvent (QContextMenuEvent* event) { QMenu menu(this); + menu.addAction(selectAction); + menu.addSeparator(); menu.addAction(renameAction); if (timeout) { menu.addAction(removeAction); } - menu.addAction(selectAction); menu.addAction(selectAirframeAction); + menu.addAction(setBatterySpecsAction); menu.exec(event->globalPos()); } +void UASView::setBatterySpecs() +{ + if (uas) + { + bool ok; + QString newName = QInputDialog::getText(this, tr("Set Battery Specifications for %1").arg(uas->getUASName()), + tr("Specs: (empty,warn,full), e.g. (9V,9.5V,12.6V) or just warn level in percent (e.g. 15%) to use estimate from MAV"), QLineEdit::Normal, + uas->getBatterySpecs(), &ok); + + if (ok && !newName.isEmpty()) uas->setBatterySpecs(newName); + } +} + void UASView::rename() { if (uas) @@ -569,7 +605,7 @@ void UASView::refresh() } generalUpdateCount++; - QString colorstyle("QGroupBox { border-radius: 5px; padding: 2px; margin: 2px; border: 0px; background-color: %1; }"); + QString colorstyle("QGroupBox { border-radius: 5px; padding: 2px; margin: 0px; border: 0px; background-color: %1; }"); if (timeout) { diff --git a/src/ui/uas/UASView.h b/src/ui/uas/UASView.h index 1654452da510a3ff1724c58881be1bf7b891f288..8a4f7764bbbd6cb3305d217c05f63dddca3e3af1 100644 --- a/src/ui/uas/UASView.h +++ b/src/ui/uas/UASView.h @@ -84,6 +84,12 @@ public slots: void rename(); /** @brief Select airframe for this vehicle */ void selectAirframe(); + /** @brief Select the battery type */ + void setBatterySpecs(); + /** @brief Show a status text message */ + void showStatusText(int uasid, int componentid, int severity, QString text); + /** @brief Update the navigation mode state */ + void updateNavMode(int uasid, int mode, const QString& text); protected: void changeEvent(QEvent *e); @@ -114,6 +120,7 @@ protected: QAction* renameAction; QAction* selectAction; QAction* selectAirframeAction; + QAction* setBatterySpecsAction; static const int updateInterval = 300;