diff --git a/.gitignore b/.gitignore index 0ac71d585d050baa02b4c4fbd405dd0caf2d9d8a..24e18cdeeb47d927491ab890fb030e8669b0afce 100644 --- a/.gitignore +++ b/.gitignore @@ -8,6 +8,7 @@ Info.plist obj *.log *~ +*~.skp bin/*.exe bin/*.txt bin/mac diff --git a/README b/README index 7c70df8ff4faab842a312b7368c2c0ef101e7bae..c80836be273f77930dd41df7ad2f8cd09765c7b7 100644 --- a/README +++ b/README @@ -78,11 +78,39 @@ Done. Windows ======= +DETAILED STEPS BELOW THE VISUAL STUDIO 2010 NOTES. +GNU GCC / MINGW IS UNTESTED, COULD WORK +VISUAL STUDIO 2008 / 2010 EXPRESS EDITION IS FREE! + +------------------------------------------------------------------------------------- +VISUAL STUDIO 2010 NOTES (VS 2008 runs out-of-the-box, just follow the steps below): + +For use of Qt 4x with Visual Studio 2010 Add-in. + +Visual studio adds automatically certain defines that are wrong and cause errors. +To resolve this, execute these steps: + +In the projects properties -> C/C++ ->preprocessor change: + +in DEBUG: + delete QT_NO_DEBUG + +in both (DEBUG / RELEASE): + delete QT_NO_DYNAMIC_CAST +------------------------------------------------------------------------------------- + + + + +Steps for Visual Studio 2008 / 2010. (VS 2008 is easier, VS 2010 only recommended for +expert developers) + + Windows XP/7: -1) Download and install the QT SDK for Windows from http://qt.nokia.com/downloads/ (Visual Studio 2008 version) +1) Download and install the Qt SDK for Windows from http://qt.nokia.com/downloads/ (Visual Studio 2008 version) OR download Qt source and compile with VS 2010 -2) Download and install Visual Studio 2008 Express Edition (free) +2) Download and install Visual Studio 2008 Express Edition (free) OR VS 2010 Express Edition 3) Go to the QGroundControl folder and then to thirdParty -> libxbee diff --git a/bin/SDL.dll b/bin/SDL.dll deleted file mode 100644 index 49f8aa008fbfe6722d8a9bf0824c72c6ed074197..0000000000000000000000000000000000000000 Binary files a/bin/SDL.dll and /dev/null differ diff --git a/flightgear/connector_flightgear_uav.xml b/flightgear/connector_flightgear_uav.xml new file mode 100644 index 0000000000000000000000000000000000000000..0513e1cd96dd4a9954d0821d1d5d459abd337910 --- /dev/null +++ b/flightgear/connector_flightgear_uav.xml @@ -0,0 +1,161 @@ + + + + newline + , + + + lat + float + %+1.8f + /position/latitude-deg + + + + lon + float + %+1.8f + /position/longitude-deg + + + + alt + float + %+1.4f + /position/altitude-ft + + + + + speed + float + %2.3f + /velocities/groundspeed-kt + + + + airspeed + float + %2.3f + /velocities/airspeed-kt + + + + + + pitch + float + %+1.3f + /orientation/pitch-deg + + + + roll + float + %+1.3f + /orientation/roll-deg + + + + heading + float + %+1.3f + /orientation/heading-deg + + + + v_n + float + %2.3f + /velocities/speed-north-fps + + + + v_e + float + %2.3f + /velocities/speed-east-fps + + + + v_d + float + %2.3f + /velocities/speed-down-fps + + + + + rate_phi + float + %2.3f + /orientation/roll-rate-degps + + + + rate_theta + float + %2.3f + /orientation/pitch-rate-degps + + + + rate_psi + float + %2.3f + /orientation/yaw-rate-degps + + + + + + + newline + , + + + pitch + float + %+1.3f + /autopilot/settings/target-pitch-deg + + + + roll + float + %+1.3f + /autopilot/settings/target-roll-deg + + + + throttle + float + %+1.3f + /controls/engines/engine/throttle + + + + lat + float + %+1.8f + /sim/view/target/latitude-deg + + + + lon + float + %+1.8f + /sim/view/target/longitude-deg + + + + alt + float + %+1.4f + /sim/view/target/alt + + + + + + diff --git a/images/earth.html b/images/earth.html index 260f8eb7b872eee770b920ac4b3aabbbbe7f13aa..363e97d22dd3c8c6b256d944d019a602a27bd757 100644 --- a/images/earth.html +++ b/images/earth.html @@ -1,4 +1,4 @@ - + @@ -16,6 +16,7 @@ var ge = null; var initialized = false; var currAircraft = 0; var followEnabled = false; +var lineAltitudeOffset = 0.5; ///< 0.5 m higher than waypoint, prevents the line from entering the ground var lastLat = 0; var lastLon = 0; @@ -211,23 +212,24 @@ google.earth.addEventListener(ge.getGlobe(), 'mousemove', function(event) { if (draggingAllowed && (clickMode == 0)) { - if (dragInfo) { - event.preventDefault(); - var point = dragInfo.placemark.getGeometry(); - point.setLatitude(event.getLatitude()); - point.setLongitude(event.getLongitude()); - dragInfo.dragged = true; - dragWaypointIndex = dragInfo.placemark.getDescription(); - document.getElementById('JScript_dragWaypointIndex').setAttribute('value',dragWaypointIndex); - dragWaypointLatitude = event.getLatitude(); - dragWaypointLongitude = event.getLongitude(); - dragWaypointAltitude = point.getAltitude(); - dragWaypointPending = true; - document.getElementById('JScript_dragWaypointLatitude').setAttribute('value',dragWaypointLatitude); - document.getElementById('JScript_dragWaypointLongitude').setAttribute('value',dragWaypointLongitude); - document.getElementById('JScript_dragWaypointAltitude').setAttribute('value',dragWaypointAltitude); - document.getElementById('JScript_dragWaypointPending').setAttribute('value',true); - } + if (dragInfo) + { + event.preventDefault(); + var point = dragInfo.placemark.getGeometry(); + point.setLatitude(event.getLatitude()); + point.setLongitude(event.getLongitude()); + dragInfo.dragged = true; + dragWaypointIndex = dragInfo.placemark.getDescription(); + document.getElementById('JScript_dragWaypointIndex').setAttribute('value',dragWaypointIndex); + dragWaypointLatitude = event.getLatitude(); + dragWaypointLongitude = event.getLongitude(); + dragWaypointAltitude = point.getAltitude(); + dragWaypointPending = true; + document.getElementById('JScript_dragWaypointLatitude').setAttribute('value',dragWaypointLatitude); + document.getElementById('JScript_dragWaypointLongitude').setAttribute('value',dragWaypointLongitude); + document.getElementById('JScript_dragWaypointAltitude').setAttribute('value',dragWaypointAltitude); + document.getElementById('JScript_dragWaypointPending').setAttribute('value',true); + } } }); @@ -298,7 +300,7 @@ function setGCSHome(lat, lon, alt) if (homePlacemark == null) { - var placemark = ge.createPlacemark(''); + var placemark = ge.createPlacemark(''); var icon = ge.createIcon(''); icon.setHref('http://google-maps-icons.googlecode.com/files/blackH.png'); var style = ge.createStyle(''); @@ -349,6 +351,8 @@ function updateWaypointListLength(id, len) { var placemark = waypoints.pop(); ge.getFeatures().removeChild(placemark); + var line = waypointLines[id].pop(); + ge.getFeatures().removeChild(line); } } } @@ -366,6 +370,9 @@ function updateWaypoint(id, index, lat, lon, alt, action) location.setAltitude(alt); waypoints[index].setGeometry(location); waypoints[index].setDescription(index+""); + + // Set the WP line location + waypointLines[id].getCoordinates().setLatLngAlt(index, lat, lon, alt); } else { @@ -386,122 +393,108 @@ function updateWaypoint(id, index, lat, lon, alt, action) var location = ge.createPoint(''); location.setLatitude(lat); location.setLongitude(lon); - location.setAltitude(alt); + location.setAltitude(alt+lineAltitudeOffset); placemark.setGeometry(location); // Add the placemark to Earth. ge.getFeatures().appendChild(placemark); waypoints[index] = placemark; - } - - // Add waypoint line - waypointLines[id].setExtrude(false); - waypointLines[id].setAltitudeMode(ge.ALTITUDE_ABSOLUTE); - + // Add LineString points waypointLines[id].getCoordinates().pushLatLngAlt(lat, lon, alt); - - // Create a style and set width and color of line - waypointLinePlacemarks[id].setStyleSelector(ge.createStyle('')); - lineStyle = waypointLinePlacemarks[id].getStyleSelector().getLineStyle(); - lineStyle.setWidth(5); - lineStyle.getColor().set(waypointLineColors[id]);  // aabbggrr format - //lineStyle.getColor().set(color);  // aabbggrr format - - // Add the feature to Earth - //if (waypointLinesVisible[id] == true) - //ge.getFeatures().replaceChild(waypointLinePlacemarks[id], waypointLinePlacemarks[id]); // does not work - - // Add connecting line - + } } function createAircraft(id, type, color) { - planePlacemark = ge.createPlacemark(''); - planePlacemark.setName('aircraft'); - planeModel = ge.createModel(''); - ge.getFeatures().appendChild(planePlacemark); - planeLoc = ge.createLocation(''); - planeModel.setLocation(planeLoc); - planeLink = ge.createLink(''); - planeOrient = ge.createOrientation(''); - planeModel.setOrientation(planeOrient); - - planeLink.setHref('http://qgroundcontrol.org/_media/users/models/multiplex-twinstar.dae'); - planeModel.setLink(planeLink); - planeModel.setAltitudeMode (ge.ALTITUDE_ABSOLUTE); - - planeLoc.setLatitude(currLat); - planeLoc.setLongitude(currLon); - planeLoc.setAltitude(currAlt); - - planePlacemark.setGeometry(planeModel); + planePlacemark = ge.createPlacemark(''); + planePlacemark.setName('aircraft'); + planeModel = ge.createModel(''); + ge.getFeatures().appendChild(planePlacemark); + planeLoc = ge.createLocation(''); + planeModel.setLocation(planeLoc); + planeLink = ge.createLink(''); + planeOrient = ge.createOrientation(''); + planeModel.setOrientation(planeOrient); + + planeLink.setHref('http://qgroundcontrol.org/_media/users/models/ascent-park-glider.dae'); + planeModel.setLink(planeLink); + planeModel.setAltitudeMode (ge.ALTITUDE_ABSOLUTE); + + planeLoc.setLatitude(currLat); + planeLoc.setLongitude(currLon); + planeLoc.setAltitude(currAlt); + + planePlacemark.setGeometry(planeModel); - // Write into global structure - aircraft[id] = planePlacemark; - attitudes[id] = planeOrient; - aircraftLocations[id] = planeLoc; - aircraftLastLocations[id] = ge.createLocation(''); - //planeColor = color; + // Write into global structure + aircraft[id] = planePlacemark; + attitudes[id] = planeOrient; + aircraftLocations[id] = planeLoc; + aircraftLastLocations[id] = ge.createLocation(''); - createTrail(id, color); - createWaypointLine(id, color); - //console.log(color); + createTrail(id, color); + createWaypointLine(id, color); } function createTrail(id, color) { - trailPlacemarks[id] = ge.createPlacemark(''); - // Create the placemark -// Create the LineString; set it to extend down to the ground -// and set the altitude mode -trails[id] = ge.createLineString(''); -trailPlacemarks[id].setGeometry(trails[id]); -trails[id].setExtrude(false); -trails[id].setAltitudeMode(ge.ALTITUDE_ABSOLUTE); - -// Add LineString points -//lineString.getCoordinates().pushLatLngAlt(48.754, -121.835, 700); - -// Create a style and set width and color of line -trailPlacemarks[id].setStyleSelector(ge.createStyle('')); -lineStyle = trailPlacemarks[id].getStyleSelector().getLineStyle(); -lineStyle.setWidth(5); -trailColors[id] = color; -lineStyle.getColor().set('00000000');  // aabbggrr format - -trailsVisible[id] = false; + trailPlacemarks[id] = ge.createPlacemark(''); + // Create the placemark + // Create the LineString; set it to extend down to the ground + // and set the altitude mode + trails[id] = ge.createLineString(''); + trailPlacemarks[id].setGeometry(trails[id]); + trails[id].setExtrude(false); + trails[id].setAltitudeMode(ge.ALTITUDE_ABSOLUTE); -// Add the feature to Earth -//ge.getFeatures().appendChild(trailPlacemarks[id]); + // Create a style and set width and color of line + trailPlacemarks[id].setStyleSelector(ge.createStyle('')); + lineStyle = trailPlacemarks[id].getStyleSelector().getLineStyle(); + lineStyle.setWidth(5); + trailColors[id] = color; + lineStyle.getColor().set('00000000');  // aabbggrr format + trailsVisible[id] = false; } function createWaypointLine(id, color) { - waypointLinePlacemarks[id] = ge.createPlacemark(''); - // Create the placemark -// Create the LineString; set it to extend down to the ground -// and set the altitude mode -waypointLines[id] = ge.createLineString(''); -waypointLinePlacemarks[id].setGeometry(waypointLines[id]); -waypointLines[id].setExtrude(false); -waypointLines[id].setAltitudeMode(ge.ALTITUDE_ABSOLUTE); - -// Add LineString points -//lineString.getCoordinates().pushLatLngAlt(48.754, -121.835, 700); + // Create the placemark + waypointLinePlacemarks[id] = ge.createPlacemark(''); + // Create the LineString; set it to extend down to the ground + // and set the altitude mode + waypointLines[id] = ge.createLineString(''); + waypointLinePlacemarks[id].setGeometry(waypointLines[id]); + waypointLines[id].setExtrude(false); + waypointLines[id].setAltitudeMode(ge.ALTITUDE_ABSOLUTE); + + // Add LineString points + //lineString.getCoordinates().pushLatLngAlt(48.754, -121.835, 700); + + // Create a style and set width and color of line + waypointLinePlacemarks[id].setStyleSelector(ge.createStyle('')); + lineStyle = waypointLinePlacemarks[id].getStyleSelector().getLineStyle(); + lineStyle.setWidth(15); + waypointLineColors[id] = color; + lineStyle.getColor().set(color);  // aabbggrr format + + + // Add waypoint line + //waypointLines[id].setExtrude(false); + //waypointLines[id].setAltitudeMode(ge.ALTITUDE_ABSOLUTE); -// Create a style and set width and color of line -waypointLinePlacemarks[id].setStyleSelector(ge.createStyle('')); -lineStyle = waypointLinePlacemarks[id].getStyleSelector().getLineStyle(); -lineStyle.setWidth(10); -waypointLineColors[id] = color; -lineStyle.getColor().set('00000000');  // aabbggrr format + -// Add the feature to Earth -//ge.getFeatures().appendChild(trailPlacemarks[id]); + // Create a style and set width and color of line + //waypointLinePlacemarks[id].setStyleSelector(ge.createStyle('')); + //lineStyle = waypointLinePlacemarks[id].getStyleSelector().getLineStyle(); + //lineStyle.setWidth(15); + //lineStyle.getColor().set(waypointLineColors[id]);  // aabbggrr format + //lineStyle.getColor().set(color);  // aabbggrr format + // Add the feature to Earth + ge.getFeatures().appendChild(waypointLinePlacemarks[id]); } function clearTrail(id) @@ -535,8 +528,8 @@ function setViewRange(dist) function addTrailPosition(id, lat, lon, alt) { - trails[id].setExtrude(false); - trails[id].setAltitudeMode(ge.ALTITUDE_ABSOLUTE); + //trails[id].setExtrude(false); + //trails[id].setAltitudeMode(ge.ALTITUDE_ABSOLUTE); // Add LineString points trails[id].getCoordinates().pushLatLngAlt(lat, lon, alt); @@ -546,10 +539,6 @@ function addTrailPosition(id, lat, lon, alt) lineStyle = trailPlacemarks[id].getStyleSelector().getLineStyle(); lineStyle.setWidth(5); lineStyle.getColor().set(trailColors[id]);  // aabbggrr format - //lineStyle.getColor().set(color);  // aabbggrr format - - // Add the feature to Earth - if (trailsVisible[id] == true) ge.getFeatures().replaceChild(trailPlacemarks[id], trailPlacemarks[id]); } function initCallback(object) @@ -581,6 +570,7 @@ function setAircraftPositionAttitude(id, lat, lon, alt, roll, pitch, yaw) lastLat = currLat; lastLon = currLon; } + currLat = lat; currLon = lon; var trueGroundAlt = ge.getGlobe().getGroundAltitude(lat, lon); @@ -599,19 +589,19 @@ function setAircraftPositionAttitude(id, lat, lon, alt, roll, pitch, yaw) //currFollowHeading = ((yaw/M_PI)+1.0)*360.0; - // FIXME Currently invalid conversion from right-handed z-down to z-up frame - planeOrient.setRoll(((roll/M_PI))*180.0+180.0); - planeOrient.setTilt(((pitch/M_PI))*180.0+180.0); - planeOrient.setHeading(((yaw/M_PI))*180.0-90.0); + // FIXME Currently invalid conversion from right-handed z-down to z-up frame + planeOrient.setRoll(((roll/M_PI))*180.0+180.0); + planeOrient.setTilt(((pitch/M_PI))*180.0+180.0); + planeOrient.setHeading(((yaw/M_PI))*180.0-90.0); - currFollowHeading = ((yaw/M_PI))*180.0; + currFollowHeading = ((yaw/M_PI))*180.0; - planeLoc.setLatitude(lastLat); - planeLoc.setLongitude(lastLon); - planeLoc.setAltitude(lastAlt); - planeModel.setLocation(planeLoc); + planeLoc.setLatitude(lastLat); + planeLoc.setLongitude(lastLon); + planeLoc.setAltitude(lastAlt); + planeModel.setLocation(planeLoc); - if (followEnabled) updateFollowAircraft(); + if (followEnabled) updateFollowAircraft(); } } @@ -661,6 +651,7 @@ function setViewMode(mode) currView.setTilt(lastTilt); currView.setHeading(lastHeading); } + if (mode == 1 && viewMode != mode) { lastTilt = currView.getTilt(); diff --git a/models/ascent-park-glider.skp b/models/ascent-park-glider.skp index 1a791d7591c89f6c536f49bdfd651a859faf9f0f..f18003c4b6a02498f09bee0dd7c4bf360487693d 100644 Binary files a/models/ascent-park-glider.skp and b/models/ascent-park-glider.skp differ diff --git a/models/multiplex-twinstar.skp b/models/multiplex-twinstar.skp index 30fd6f91775931cc2fc867dad1c416765adfdc94..245300e960fc7f48e85f8a1b0aaab4c2cfe3ca97 100644 Binary files a/models/multiplex-twinstar.skp and b/models/multiplex-twinstar.skp differ diff --git a/price.txt b/price.txt deleted file mode 100644 index 3e4e1661ed67758d52eb04409464b355cd2aec58..0000000000000000000000000000000000000000 --- a/price.txt +++ /dev/null @@ -1,12 +0,0 @@ -price quantity -210 81 -250 73 -280 64 -300 61 -320 50 -340 46 -360 45 -380 44 -400 43 -420 39 -440 36 diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 65779bc2d9762ddb68e93817f3798b9e1f5b3ff5..55733b960d32aeaca1d90430f1385c69858b755d 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -246,6 +246,7 @@ HEADERS += src/MG.h \ src/comm/ProtocolInterface.h \ src/comm/MAVLinkProtocol.h \ src/comm/AS4Protocol.h \ + src/comm/QGCFlightGearLink.h \ src/ui/CommConfigurationWindow.h \ src/ui/SerialConfigurationWindow.h \ src/ui/MainWindow.h \ @@ -376,6 +377,7 @@ SOURCES += src/main.cc \ src/comm/SerialSimulationLink.cc \ src/comm/MAVLinkProtocol.cc \ src/comm/AS4Protocol.cc \ + src/comm/QGCFlightGearLink.cc \ src/ui/CommConfigurationWindow.cc \ src/ui/SerialConfigurationWindow.cc \ src/ui/MainWindow.cc \ diff --git a/src/apps/mavlinkgen/generator/MAVLinkXMLParser.cc b/src/apps/mavlinkgen/generator/MAVLinkXMLParser.cc index 34cc9ec9bc067a77e76d3099a8e3ba57e0e8fff5..66769ae15839a0a919bced5a4fe2cc5aafcf4376 100644 --- a/src/apps/mavlinkgen/generator/MAVLinkXMLParser.cc +++ b/src/apps/mavlinkgen/generator/MAVLinkXMLParser.cc @@ -1,4 +1,24 @@ /*===================================================================== + +QGroundControl Open Source Ground Control Station + +(c) 2009 - 2011 QGROUNDCONTROL PROJECT + +This file is part of the QGROUNDCONTROL 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 . + ======================================================================*/ /** @@ -29,7 +49,8 @@ MAVLinkXMLParser::MAVLinkXMLParser(QString document, QString outputDirectory, QO { doc = new QDomDocument(); QFile file(document); - if (file.open(QIODevice::ReadOnly | QIODevice::Text)) { + if (file.open(QIODevice::ReadOnly | QIODevice::Text)) + { const QString instanceText(QString::fromUtf8(file.readAll())); doc->setContent(instanceText); } @@ -50,7 +71,8 @@ bool MAVLinkXMLParser::generate() bool success = true; // Only generate if output dir is correctly set - if (outputDirName == "") { + if (outputDirName == "") + { emit parseState(tr("ERROR: No output directory given.\nAbort.")); return false; } @@ -96,7 +118,7 @@ bool MAVLinkXMLParser::generate() static int highest_message_id; static int recursion_level; - if (recursion_level == 0) { + if (recursion_level == 0){ highest_message_id = 0; memset(message_lengths, 0, sizeof(message_lengths)); } @@ -113,18 +135,24 @@ bool MAVLinkXMLParser::generate() // Run through root children - while(!n.isNull()) { + while(!n.isNull()) + { // Each child is a message QDomElement e = n.toElement(); // try to convert the node to an element. - if(!e.isNull()) { - if (e.tagName() == "mavlink") { + if(!e.isNull()) + { + if (e.tagName() == "mavlink") + { p = n; n = n.firstChild(); - while (!n.isNull()) { + while (!n.isNull()) + { e = n.toElement(); - if (!e.isNull()) { + if (!e.isNull()) + { // Handle all include tags - if (e.tagName() == "include") { + if (e.tagName() == "include") + { QString incFileName = e.text(); // Load file //QDomDocument includeDoc = QDomDocument(); @@ -134,14 +162,16 @@ bool MAVLinkXMLParser::generate() QFileInfo fInfo(incFileName); QString incFilePath; - if (fInfo.isRelative()) { + if (fInfo.isRelative()) + { QFileInfo rInfo(this->fileName); incFilePath = rInfo.absoluteDir().canonicalPath() + "/" + incFileName; pureIncludeFileName = fInfo.baseName().split(".", QString::SkipEmptyParts).first(); } QFile file(incFilePath); - if (file.open(QIODevice::ReadOnly | QIODevice::Text)) { + if (file.open(QIODevice::ReadOnly | QIODevice::Text)) + { emit parseState(QString("Included messages from file: %1").arg(incFileName)); // NEW MODE: CREATE INDIVIDUAL FOLDERS // Create new output directory, parse included XML and generate C-code @@ -175,7 +205,9 @@ bool MAVLinkXMLParser::generate() // } emit parseState(QString("End of inclusion from file: %1").arg(incFileName)); - } else { + } + else + { // Include file could not be opened emit parseState(QString("ERROR: Failed including file: %1, file is not readable. Wrong/misspelled filename?\nAbort.").arg(fileName)); return false; @@ -183,33 +215,40 @@ bool MAVLinkXMLParser::generate() } // Handle all enum tags - else if (e.tagName() == "version") { + else if (e.tagName() == "version") + { //QString fieldType = e.attribute("type", ""); //QString fieldName = e.attribute("name", ""); QString fieldText = e.text(); // Check if version has been previously set - if (mavlinkVersion != 0) { + if (mavlinkVersion != 0) + { emit parseState(QString("ERROR: Protocol version tag set twice, please use it only once. First version was %1, second version is %2.\nAbort.").arg(mavlinkVersion).arg(fieldText)); return false; } bool ok; int version = fieldText.toInt(&ok); - if (ok && (version > 0) && (version < 256)) { + if (ok && (version > 0) && (version < 256)) + { // Set MAVLink version mavlinkVersion = version; - } else { + } + else + { emit parseState(QString("ERROR: Reading version string failed: %1, string is not an integer number between 1 and 255.\nAbort.").arg(fieldText)); return false; } } // Handle all enum tags - else if (e.tagName() == "enums") { + else if (e.tagName() == "enums") + { // One down into the enums list p = n; n = n.firstChild(); - while (!n.isNull()) { + while (!n.isNull()) + { e = n.toElement(); QString currEnum; @@ -217,18 +256,25 @@ bool MAVLinkXMLParser::generate() // Comment QString comment; - if(!e.isNull() && e.tagName() == "enum") { + if(!e.isNull() && e.tagName() == "enum") + { // Get enum name QString enumName = e.attribute("name", "").toLower(); - if (enumName.size() == 0) { + if (enumName.size() == 0) + { emit parseState(tr("ERROR: Missing required name=\"\" attribute for tag %2 near line %1\nAbort.").arg(QString::number(e.lineNumber()), e.tagName())); return false; - } else { + } + else + { // Sanity check: Accept only enum names not used previously - if (usedEnumNames->contains(enumName)) { + if (usedEnumNames->contains(enumName)) + { emit parseState(tr("ERROR: Enum name %1 used twice, second occurence near line %2 of file %3\nAbort.").arg(enumName, QString::number(e.lineNumber()), fileName)); return false; - } else { + } + else + { usedEnumNames->insert(enumName, QString::number(e.lineNumber())); } @@ -240,22 +286,28 @@ bool MAVLinkXMLParser::generate() // Get the enum fields QDomNode f = e.firstChild(); - while (!f.isNull()) { + while (!f.isNull()) + { QDomElement e2 = f.toElement(); - if (!e2.isNull() && e2.tagName() == "entry") { + if (!e2.isNull() && e2.tagName() == "entry") + { QString fieldValue = e2.attribute("value", ""); // If value was given, use it, if not, use the enum iterator // value. The iterator value gets reset by manual values QString fieldName = e2.attribute("name", ""); - if (fieldValue.length() == 0) { + if (fieldValue.length() == 0) + { fieldValue = QString::number(nextEnumValue); nextEnumValue++; - } else { + } + else + { bool ok; nextEnumValue = fieldValue.toInt(&ok) + 1; - if (!ok) { + if (!ok) + { emit parseState(tr("ERROR: Enum entry %1 has not a valid number (%2) in the value field.\nAbort.").arg(fieldName, fieldValue)); return false; } @@ -267,7 +319,8 @@ bool MAVLinkXMLParser::generate() { QString sep(" | "); QDomNode pp = e2.firstChild(); - while (!pp.isNull()) { + while (!pp.isNull()) + { QDomElement pp2 = pp.toElement(); if (pp2.isText() || pp2.isCDATASection()) { @@ -306,37 +359,49 @@ bool MAVLinkXMLParser::generate() } // Handle all message tags - else if (e.tagName() == "messages") { + else if (e.tagName() == "messages") + { p = n; n = n.firstChild(); - while (!n.isNull()) { + while (!n.isNull()) + { e = n.toElement(); - if(!e.isNull()) { + if(!e.isNull()) + { //if (e.isNull()) continue; // Get message name QString messageName = e.attribute("name", "").toLower(); - if (messageName.size() == 0) { + if (messageName.size() == 0) + { emit parseState(tr("ERROR: Missing required name=\"\" attribute for tag %2 near line %1\nAbort.").arg(QString::number(e.lineNumber()), e.tagName())); return false; - } else { + } + else + { // Get message id bool ok; int messageId = e.attribute("id", "-1").toInt(&ok, 10); emit parseState(tr("Compiling message %1 \t(#%3) \tnear line %2").arg(messageName, QString::number(n.lineNumber()), QString::number(messageId))); // Sanity check: Accept only message IDs not used previously - if (usedMessageIDs->contains(messageId)) { + if (usedMessageIDs->contains(messageId)) + { emit parseState(tr("ERROR: Message ID %1 used twice, second occurence near line %2 of file %3\nAbort.").arg(QString::number(messageId), QString::number(e.lineNumber()), fileName)); return false; - } else { + } + else + { usedMessageIDs->append(messageId); } // Sanity check: Accept only message names not used previously - if (usedMessageNames->contains(messageName)) { + if (usedMessageNames->contains(messageName)) + { emit parseState(tr("ERROR: Message name %1 used twice, second occurence near line %2 of file %3\nAbort.").arg(messageName, QString::number(e.lineNumber()), fileName)); return false; - } else { + } + else + { usedMessageNames->insert(messageName, QString::number(e.lineNumber())); } @@ -375,9 +440,11 @@ bool MAVLinkXMLParser::generate() // Get the message fields QDomNode f = e.firstChild(); - while (!f.isNull()) { + while (!f.isNull()) + { QDomElement e2 = f.toElement(); - if (!e2.isNull() && e2.tagName() == "field") { + if (!e2.isNull() && e2.tagName() == "field") + { QString fieldType = e2.attribute("type", ""); QString fieldName = e2.attribute("name", ""); QString fieldText = e2.text(); @@ -386,7 +453,8 @@ bool MAVLinkXMLParser::generate() QString unpackingComment = QString("/**\n * @brief Get field %1 from %2 message\n *\n * @return %3\n */\n").arg(fieldName, messageName, fieldText); // Send arguments do not work for the version field - if (!fieldType.contains("uint8_t_mavlink_version")) { + if (!fieldType.contains("uint8_t_mavlink_version")) + { // Send arguments are the same for integral types and arrays sendArguments += ", " + fieldName; commentLines += commentEntry.arg(fieldName, fieldText.replace("\n", " ")); @@ -394,7 +462,8 @@ bool MAVLinkXMLParser::generate() // MAVLink version field // this is a special field always containing the version define - if (fieldType.contains("uint8_t_mavlink_version")) { + if (fieldType.contains("uint8_t_mavlink_version")) + { // Add field to C structure cStructLines += QString("\t%1 %2; ///< %3\n").arg("uint8_t", fieldName, fieldText); // Add pack line to message_xx_pack function @@ -404,7 +473,8 @@ bool MAVLinkXMLParser::generate() } // Array handling is different from simple types - else if (fieldType.startsWith("array")) { + else if (fieldType.startsWith("array")) + { int arrayLength = QString(fieldType.split("[").at(1).split("]").first()).toInt(); QString arrayType = fieldType.split("[").first(); packParameters += QString(", const ") + QString("int8_t*") + " " + fieldName; @@ -417,7 +487,9 @@ bool MAVLinkXMLParser::generate() // Add decode function for this type decodeLines += QString("\tmavlink_msg_%1_get_%2(msg, %1->%2);\n").arg(messageName, fieldName); arrayDefines += QString("#define MAVLINK_MSG_%1_FIELD_%2_LEN %3\n").arg(messageName.toUpper(), fieldName.toUpper(), QString::number(arrayLength)); - } else if (fieldType.startsWith("string")) { + } + else if (fieldType.startsWith("string")) + { int arrayLength = QString(fieldType.split("[").at(1).split("]").first()).toInt(); QString arrayType = fieldType.split("[").first(); packParameters += QString(", const ") + QString("char*") + " " + fieldName; @@ -432,7 +504,8 @@ bool MAVLinkXMLParser::generate() arrayDefines += QString("#define MAVLINK_MSG_%1_FIELD_%2_LEN %3\n").arg(messageName.toUpper(), fieldName.toUpper(), QString::number(arrayLength)); } // Expand array handling to all valid mavlink data types - else if(fieldType.contains('[') && fieldType.contains(']')) { + else if(fieldType.contains('[') && fieldType.contains(']')) + { int arrayLength = QString(fieldType.split("[").at(1).split("]").first()).toInt(); QString arrayType = fieldType.split("[").first(); packParameters += QString(", const ") + arrayType + "* " + fieldName; @@ -452,7 +525,8 @@ bool MAVLinkXMLParser::generate() // decodeLines += ""; prepends += QString("+sizeof(%1)*%2").arg(arrayType, QString::number(arrayLength)); - } else + } + else // Handle simple types like integers and floats { packParameters += ", " + fieldType + " " + fieldName; @@ -470,7 +544,8 @@ bool MAVLinkXMLParser::generate() // message length calculation unsigned element_multiplier = 1; unsigned element_length = 0; - const struct { + const struct + { const char *prefix; unsigned length; } length_map[] = { @@ -487,10 +562,12 @@ bool MAVLinkXMLParser::generate() { "float", 4 }, { "double", 8 }, }; - if (fieldType.contains("[")) { + if (fieldType.contains("[")) + { element_multiplier = fieldType.split("[").at(1).split("]").first().toInt(); } - for (unsigned i=0; ipayload%2)[0];").arg("uint8_t", prepends); - } else if (fieldType == "uint8_t" || fieldType == "int8_t") { + } + else if (fieldType == "uint8_t" || fieldType == "int8_t") + { unpackingCode = QString("\treturn (%1)(msg->payload%2)[0];").arg(fieldType, prepends); - } else if (fieldType == "uint16_t" || fieldType == "int16_t") { + } + else if (fieldType == "uint16_t" || fieldType == "int16_t") + { unpackingCode = QString("\tgeneric_16bit r;\n\tr.b[1] = (msg->payload%1)[0];\n\tr.b[0] = (msg->payload%1)[1];\n\treturn (%2)r.s;").arg(prepends).arg(fieldType); - } else if (fieldType == "uint32_t" || fieldType == "int32_t") { + } + else if (fieldType == "uint32_t" || fieldType == "int32_t") + { unpackingCode = QString("\tgeneric_32bit r;\n\tr.b[3] = (msg->payload%1)[0];\n\tr.b[2] = (msg->payload%1)[1];\n\tr.b[1] = (msg->payload%1)[2];\n\tr.b[0] = (msg->payload%1)[3];\n\treturn (%2)r.i;").arg(prepends).arg(fieldType); - } else if (fieldType == "float") { + } + else if (fieldType == "float") + { unpackingCode = QString("\tgeneric_32bit r;\n\tr.b[3] = (msg->payload%1)[0];\n\tr.b[2] = (msg->payload%1)[1];\n\tr.b[1] = (msg->payload%1)[2];\n\tr.b[0] = (msg->payload%1)[3];\n\treturn (%2)r.f;").arg(prepends).arg(fieldType); - } else if (fieldType == "uint64_t" || fieldType == "int64_t") { + } + else if (fieldType == "uint64_t" || fieldType == "int64_t") + { unpackingCode = QString("\tgeneric_64bit r;\n\tr.b[7] = (msg->payload%1)[0];\n\tr.b[6] = (msg->payload%1)[1];\n\tr.b[5] = (msg->payload%1)[2];\n\tr.b[4] = (msg->payload%1)[3];\n\tr.b[3] = (msg->payload%1)[4];\n\tr.b[2] = (msg->payload%1)[5];\n\tr.b[1] = (msg->payload%1)[6];\n\tr.b[0] = (msg->payload%1)[7];\n\treturn (%2)r.ll;").arg(prepends).arg(fieldType); - } else if (fieldType.startsWith("array")) { + } + else if (fieldType.startsWith("array")) + { // fieldtype formatis string[n] where n is the number of bytes, extract n from field type string unpackingCode = QString("\n\tmemcpy(r_data, msg->payload%1, %2);\n\treturn %2;").arg(prepends, fieldType.split("[").at(1).split("]").first()); - } else if (fieldType.startsWith("string")) { + } + else if (fieldType.startsWith("string")) + { // fieldtype formatis string[n] where n is the number of bytes, extract n from field type string unpackingCode = QString("\n\tstrcpy(r_data, msg->payload%1, %2);\n\treturn %2;").arg(prepends, fieldType.split("[").at(1).split("]").first()); } // Generate the message decoding function - if (fieldType.contains("uint8_t_mavlink_version")) { + if (fieldType.contains("uint8_t_mavlink_version")) + { unpacking += unpackingComment + QString("static inline %1 mavlink_msg_%2_get_%3(const mavlink_message_t* msg)\n{\n%4\n}\n\n").arg("uint8_t", messageName, fieldName, unpackingCode); decodeLines += ""; prepends += "+sizeof(uint8_t)"; } // Array handling is different from simple types - else if (fieldType.startsWith("array")) { + else if (fieldType.startsWith("array")) + { unpacking += unpackingComment + QString("static inline uint16_t mavlink_msg_%1_get_%2(const mavlink_message_t* msg, int8_t* r_data)\n{\n%4\n}\n\n").arg(messageName, fieldName, unpackingCode); decodeLines += ""; QString arrayLength = QString(fieldType.split("[").at(1).split("]").first()); prepends += "+" + arrayLength; - } else if (fieldType.startsWith("string")) { + } + else if (fieldType.startsWith("string")) + { unpacking += unpackingComment + QString("static inline uint16_t mavlink_msg_%1_get_%2(const mavlink_message_t* msg, char* r_data)\n{\n%4\n}\n\n").arg(messageName, fieldName, unpackingCode); decodeLines += ""; QString arrayLength = QString(fieldType.split("[").at(1).split("]").first()); prepends += "+" + arrayLength; - } else if(fieldType.contains('[') && fieldType.contains(']')) { + } + else if(fieldType.contains('[') && fieldType.contains(']')) + { // prevent this case from being caught in the following else - } else { + } + else + { unpacking += unpackingComment + QString("static inline %1 mavlink_msg_%2_get_%3(const mavlink_message_t* msg)\n{\n%4\n}\n\n").arg(fieldType, messageName, fieldName, unpackingCode); decodeLines += ""; prepends += "+sizeof(" + e2.attribute("type", "void") + ")"; @@ -553,7 +653,8 @@ bool MAVLinkXMLParser::generate() f = f.nextSibling(); } - if (messageId > highest_message_id) { + if (messageId > highest_message_id) + { highest_message_id = messageId; } message_lengths[messageId] = message_length; @@ -600,7 +701,8 @@ bool MAVLinkXMLParser::generate() mainHeader += "// MESSAGE DEFINITIONS\n\n"; // Create directory if it doesn't exist, report result in success if (!dir.exists()) success = success && dir.mkpath(outputDirName + "/" + messagesDirName); - for (int i = 0; i < cFiles.size(); i++) { + for (int i = 0; i < cFiles.size(); i++) + { QFile rawFile(dir.filePath(cFiles.at(i).first)); bool ok = rawFile.open(QIODevice::WriteOnly | QIODevice::Text); success = success && ok; @@ -612,7 +714,8 @@ bool MAVLinkXMLParser::generate() mainHeader += "\n\n// MESSAGE LENGTHS\n\n"; mainHeader += "#undef MAVLINK_MESSAGE_LENGTHS\n"; mainHeader += "#define MAVLINK_MESSAGE_LENGTHS { "; - for (int i=0; i + +This file is part of the QGROUNDCONTROL 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 . + +======================================================================*/ + +/** + * @file + * @brief Implementation of class MAVLinkXMLParserV10 + * @author Lorenz Meier + */ + +#include +#include +#include +#include +#include +#include +#include +#include "MAVLinkXMLParserV10.h" + +#include + +MAVLinkXMLParserV10::MAVLinkXMLParserV10(QDomDocument* document, QString outputDirectory, QObject* parent) : QObject(parent), +doc(document), +outputDirName(outputDirectory), +fileName("") +{ +} + +MAVLinkXMLParserV10::MAVLinkXMLParserV10(QString document, QString outputDirectory, QObject* parent) : QObject(parent) +{ + doc = new QDomDocument(); + QFile file(document); + if (file.open(QIODevice::ReadOnly | QIODevice::Text)) + { + const QString instanceText(QString::fromUtf8(file.readAll())); + doc->setContent(instanceText); + } + fileName = document; + outputDirName = outputDirectory; +} + +MAVLinkXMLParserV10::~MAVLinkXMLParserV10() +{ +} + +/** + * Generate C-code (C-89 compliant) out of the XML protocol specs. + */ +bool MAVLinkXMLParserV10::generate() +{ + // Process result + bool success = true; + + // Only generate if output dir is correctly set + if (outputDirName == "") + { + emit parseState(tr("ERROR: No output directory given.\nAbort.")); + return false; + } + + QString topLevelOutputDirName = outputDirName; + + // print out the element names of all elements that are direct children + // of the outermost element. + QDomElement docElem = doc->documentElement(); + QDomNode n = docElem;//.firstChild(); + QDomNode p = docElem; + + // Sanity check variables + QList* usedMessageIDs = new QList(); + QMap* usedMessageNames = new QMap(); + QMap* usedEnumNames = new QMap(); + + QList< QPair > cFiles; + QString lcmStructDefs = ""; + + QString pureFileName; + QString pureIncludeFileName; + + QFileInfo fInfo(this->fileName); + pureFileName = fInfo.baseName().split(".", QString::SkipEmptyParts).first(); + + // XML parsed and converted to C code. Now generating the files + outputDirName += QDir::separator() + pureFileName; + QDateTime now = QDateTime::currentDateTime().toUTC(); + QLocale loc(QLocale::English); + QString dateFormat = "dddd, MMMM d yyyy, hh:mm UTC"; + QString date = loc.toString(now, dateFormat); + QString includeLine = "#include \"%1\"\n"; + QString mainHeaderName = pureFileName + ".h"; + QString messagesDirName = ".";//"generated"; + QDir dir(outputDirName + "/" + messagesDirName); + + int mavlinkVersion = 0; + + + + // Start main header + QString mainHeader = QString("/** @file\n *\t@brief MAVLink comm protocol.\n *\t@see http://qgroundcontrol.org/mavlink/\n *\t Generated on %1\n */\n#ifndef " + pureFileName.toUpper() + "_H\n#define " + pureFileName.toUpper() + "_H\n\n").arg(date); // The main header includes all messages + // Mark all code as C code + mainHeader += "#ifdef __cplusplus\nextern \"C\" {\n#endif\n\n"; + mainHeader += "\n#include \"../protocol.h\"\n"; + mainHeader += "\n#define MAVLINK_ENABLED_" + pureFileName.toUpper() + "\n\n"; + + QString enums; + + + // Run through root children + while(!n.isNull()) + { + // Each child is a message + QDomElement e = n.toElement(); // try to convert the node to an element. + if(!e.isNull()) + { + if (e.tagName() == "mavlink") + { + p = n; + n = n.firstChild(); + while (!n.isNull()) + { + e = n.toElement(); + if (!e.isNull()) + { + // Handle all include tags + if (e.tagName() == "include") + { + QString incFileName = e.text(); + // Load file + //QDomDocument includeDoc = QDomDocument(); + + // Prepend file path if it is a relative path and + // make it relative to opened file + QFileInfo fInfo(incFileName); + + QString incFilePath; + if (fInfo.isRelative()) + { + QFileInfo rInfo(this->fileName); + incFilePath = rInfo.absoluteDir().canonicalPath() + "/" + incFileName; + pureIncludeFileName = fInfo.baseName().split(".", QString::SkipEmptyParts).first(); + } + + QFile file(incFilePath); + if (file.open(QIODevice::ReadOnly | QIODevice::Text)) + { + emit parseState(QString("Included messages from file: %1").arg(incFileName)); + // NEW MODE: CREATE INDIVIDUAL FOLDERS + // Create new output directory, parse included XML and generate C-code + MAVLinkXMLParserV10 includeParser(incFilePath, topLevelOutputDirName, this); + connect(&includeParser, SIGNAL(parseState(QString)), this, SIGNAL(parseState(QString))); + // Generate and write + includeParser.generate(); + mainHeader += "\n#include \"../" + pureIncludeFileName + "/" + pureIncludeFileName + ".h\"\n"; + + + // OLD MODE: MERGE BOTH FILES + // const QString instanceText(QString::fromUtf8(file.readAll())); + // includeDoc.setContent(instanceText); + // // Get all messages + // QDomNode in = includeDoc.documentElement().firstChild(); + // QDomElement ie = in.toElement(); + // if (!ie.isNull()) + // { + // if (ie.tagName() == "messages" || ie.tagName() == "include") + // { + // QDomNode ref = n.parentNode().insertAfter(in, n); + // if (ref.isNull()) + // { + // emit parseState(QString("ERROR: Inclusion failed: XML syntax error in file %1. Wrong/misspelled XML?\nAbort.").arg(fileName)); + // return false; + // } + // } + // } + + emit parseState(QString("End of inclusion from file: %1").arg(incFileName)); + } + else + { + // Include file could not be opened + emit parseState(QString("ERROR: Failed including file: %1, file is not readable. Wrong/misspelled filename?\nAbort.").arg(fileName)); + return false; + } + + } + // Handle all enum tags + else if (e.tagName() == "version") + { + //QString fieldType = e.attribute("type", ""); + //QString fieldName = e.attribute("name", ""); + QString fieldText = e.text(); + + // Check if version has been previously set + if (mavlinkVersion != 0) + { + emit parseState(QString("ERROR: Protocol version tag set twice, please use it only once. First version was %1, second version is %2.\nAbort.").arg(mavlinkVersion).arg(fieldText)); + return false; + } + + bool ok; + int version = fieldText.toInt(&ok); + if (ok && (version > 0) && (version < 256)) + { + // Set MAVLink version + mavlinkVersion = version; + } + else + { + emit parseState(QString("ERROR: Reading version string failed: %1, string is not an integer number between 1 and 255.\nAbort.").arg(fieldText)); + return false; + } + } + // Handle all enum tags + else if (e.tagName() == "enums") + { + // One down into the enums list + p = n; + n = n.firstChild(); + while (!n.isNull()) + { + e = n.toElement(); + + QString currEnum; + QString currEnumEnd; + // Comment + QString comment; + + if(!e.isNull() && e.tagName() == "enum") + { + // Get enum name + QString enumName = e.attribute("name", "").toLower(); + if (enumName.size() == 0) + { + emit parseState(tr("ERROR: Missing required name=\"\" attribute for tag %2 near line %1\nAbort.").arg(QString::number(e.lineNumber()), e.tagName())); + return false; + } + else + { + // Sanity check: Accept only enum names not used previously + if (usedEnumNames->contains(enumName)) + { + emit parseState(tr("ERROR: Enum name %1 used twice, second occurence near line %2 of file %3\nAbort.").arg(enumName, QString::number(e.lineNumber()), fileName)); + return false; + } + else + { + usedEnumNames->insert(enumName, QString::number(e.lineNumber())); + } + + // Everything sane, starting with enum content + currEnum = "enum " + enumName.toUpper() + "\n{\n"; + currEnumEnd = QString("\t%1_ENUM_END\n};\n\n").arg(enumName.toUpper()); + + int nextEnumValue = 0; + + // Get the enum fields + QDomNode f = e.firstChild(); + while (!f.isNull()) + { + QDomElement e2 = f.toElement(); + if (!e2.isNull() && e2.tagName() == "entry") + { + QString fieldValue = e2.attribute("value", ""); + + // If value was given, use it, if not, use the enum iterator + // value. The iterator value gets reset by manual values + + QString fieldName = e2.attribute("name", ""); + if (fieldValue.length() == 0) + { + fieldValue = QString::number(nextEnumValue); + nextEnumValue++; + } + else + { + bool ok; + nextEnumValue = fieldValue.toInt(&ok) + 1; + if (!ok) + { + emit parseState(tr("ERROR: Enum entry %1 has not a valid number (%2) in the value field.\nAbort.").arg(fieldName, fieldValue)); + return false; + } + } + + // Add comment of field if there is one + QString fieldComment; + if (e2.text().length() > 0) + { + QString sep(" | "); + QDomNode pp = e2.firstChild(); + while (!pp.isNull()) + { + QDomElement pp2 = pp.toElement(); + if (pp2.isText() || pp2.isCDATASection()) + { + fieldComment += pp2.nodeValue() + sep; + } + else if (pp2.isElement()) + { + fieldComment += pp2.text() + sep; + } + pp = pp.nextSibling(); + } + fieldComment = fieldComment.replace("\n", " "); + fieldComment = " /* " + fieldComment.simplified() + " */"; + } + currEnum += "\t" + fieldName.toUpper() + "=" + fieldValue + "," + fieldComment + "\n"; + } + else if(!e2.isNull() && e2.tagName() == "description") + { + comment = " " + e2.text().replace("\n", " ") + comment; + } + f = f.nextSibling(); + } + } + // Add the last parsed enum + // Remove the last comma, as the last value has none + // ENUM END MARKER IS LAST ENTRY, COMMA REMOVAL NOT NEEDED + //int commaPosition = currEnum.lastIndexOf(","); + //currEnum.remove(commaPosition, 1); + + enums += "/** @brief " + comment.simplified() + " */\n" + currEnum + currEnumEnd; + } // Element is non-zero and element name is + n = n.nextSibling(); + } // While through + // One up, back into the structure + n = p; + } + + // Handle all message tags + else if (e.tagName() == "messages") + { + p = n; + n = n.firstChild(); + while (!n.isNull()) + { + e = n.toElement(); + if(!e.isNull()) + { + //if (e.isNull()) continue; + // Get message name + QString messageName = e.attribute("name", "").toLower(); + if (messageName.size() == 0) + { + emit parseState(tr("ERROR: Missing required name=\"\" attribute for tag %2 near line %1\nAbort.").arg(QString::number(e.lineNumber()), e.tagName())); + return false; + } + else + { + // Get message id + bool ok; + int messageId = e.attribute("id", "-1").toInt(&ok, 10); + emit parseState(tr("Compiling message %1 \t(#%3) \tnear line %2").arg(messageName, QString::number(n.lineNumber()), QString::number(messageId))); + + // Sanity check: Accept only message IDs not used previously + if (usedMessageIDs->contains(messageId)) + { + emit parseState(tr("ERROR: Message ID %1 used twice, second occurence near line %2 of file %3\nAbort.").arg(QString::number(messageId), QString::number(e.lineNumber()), fileName)); + return false; + } + else + { + usedMessageIDs->append(messageId); + } + + // Sanity check: Accept only message names not used previously + if (usedMessageNames->contains(messageName)) + { + emit parseState(tr("ERROR: Message name %1 used twice, second occurence near line %2 of file %3\nAbort.").arg(messageName, QString::number(e.lineNumber()), fileName)); + return false; + } + else + { + usedMessageNames->insert(messageName, QString::number(e.lineNumber())); + } + + QString channelType("mavlink_channel_t"); + QString messageType("mavlink_message_t"); + QString headerType("mavlink_header_t"); + + // Build up function call + QString commentContainer("/**\n * @brief Pack a %1 message\n * @param system_id ID of this system\n * @param component_id ID of this component (e.g. 200 for IMU)\n * @param msg The MAVLink message to compress the data into\n *\n%2 * @return length of the message in bytes (excluding serial stream start sign)\n */\n"); + QString commentPackChanContainer("/**\n * @brief Pack a %1 message\n * @param system_id ID of this system\n * @param component_id ID of this component (e.g. 200 for IMU)\n * @param chan The MAVLink channel this message was sent over\n * @param msg The MAVLink message to compress the data into\n%2 * @return length of the message in bytes (excluding serial stream start sign)\n */\n"); + QString commentSendContainer("/**\n * @brief Send a %1 message\n * @param chan MAVLink channel to send the message\n *\n%2 */\n"); + QString commentEncodeContainer("/**\n * @brief Encode a %1 struct into a message\n *\n * @param system_id ID of this system\n * @param component_id ID of this component (e.g. 200 for IMU)\n * @param msg The MAVLink message to compress the data into\n * @param %1 C-struct to read the message contents from\n */\n"); + QString commentDecodeContainer("/**\n * @brief Decode a %1 message into a struct\n *\n * @param msg The message to decode\n * @param %1 C-struct to decode the message contents into\n */\n"); + QString commentEntry(" * @param %1 %2\n"); + QString idDefine = QString("#define MAVLINK_MSG_ID_%1 %2\n#define MAVLINK_MSG_ID_%1_LEN %3\n#define MAVLINK_MSG_%2_LEN %3"); + QString arrayDefines; + QString cStructName = QString("mavlink_%1_t").arg(messageName); + QString cStruct("typedef struct __%1 \n{\n%2\n} %1;"); + QString cStructLines; + QString encode("static inline uint16_t mavlink_msg_%1_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const %2* %1)\n{\n\treturn mavlink_msg_%1_pack(%3);\n}\n"); + + // QString decode("static inline void mavlink_msg_%1_decode(const mavlink_message_t* msg, %2* %1)\n{\n%3}\n"); + QString decode("static inline void mavlink_msg_%1_decode(const mavlink_message_t* msg, %2* %1)\n{\n\tmemcpy( %1, msg->payload, sizeof(%2));\n}\n"); + // QString pack("static inline uint16_t mavlink_msg_%1_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg%2)\n{\n\tuint16_t i = 0;\n\tmsg->msgid = MAVLINK_MSG_ID_%3;\n\n%4\n\treturn mavlink_finalize_message(msg, system_id, component_id, i);\n}\n\n"); + QString pack("static inline uint16_t mavlink_msg_%1_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg%2)\n{\n\tmavlink_%1_t *p = (mavlink_%1_t *)&msg->payload[0];\n\tmsg->msgid = MAVLINK_MSG_ID_%3;\n\n%4\n\treturn mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_%3_LEN);\n}\n\n"); + // QString packChan("static inline uint16_t mavlink_msg_%1_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg%2)\n{\n\tuint16_t i = 0;\n\tmsg->msgid = MAVLINK_MSG_ID_%3;\n\n%4\n\treturn mavlink_finalize_message_chan(msg, system_id, component_id, chan, i);\n}\n\n"); + QString packChan("static inline uint16_t mavlink_msg_%1_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg%2)\n{\n\tmavlink_%1_t *p = (mavlink_%1_t *)&msg->payload[0];\n\tmsg->msgid = MAVLINK_MSG_ID_%3;\n\n%4\n\treturn mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_%3_LEN);\n}\n\n"); + //QString compactStructSend = "#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS\n\nstatic inline void mavlink_msg_%3_struct_send(%1 chan%5)\n{\n\t%2 msg;\n\tmavlink_msg_%3_encode(mavlink_system.sysid, mavlink_system.compid, &msg%4);\n\tmavlink_send_uart(chan, &msg);\n}\n\n#endif"; + QString unpacking; + QString prepends; + QString packParameters; + QString packArguments("system_id, component_id, msg"); + QString packLines; + QString decodeLines; + QString sendArguments; + QString commentLines; + int calculatedLength = 0; + + + // Get the message fields + QDomNode f = e.firstChild(); + while (!f.isNull()) + { + QDomElement e2 = f.toElement(); + if (!e2.isNull() && e2.tagName() == "field") + { + QString fieldType = e2.attribute("type", ""); + QString fieldName = e2.attribute("name", ""); + QString fieldOffset = e2.attribute("offset", ""); + QString fieldText = e2.text(); + + QString unpackingCode; + QString unpackingComment = QString("/**\n * @brief Get field %1 from %2 message\n *\n * @return %3\n */\n").arg(fieldName, messageName, fieldText); + + // Send arguments do not work for the version field + if (!fieldType.contains("uint8_t_mavlink_version")) + { + // Send arguments are the same for integral types and arrays + sendArguments += ", " + fieldName; + commentLines += commentEntry.arg(fieldName, fieldText.replace("\n", " ")); + } + + // MAVLink version field + // this is a special field always containing the version define + if (fieldType.contains("uint8_t_mavlink_version")) + { + // Add field to C structure + cStructLines += QString("\t%1 %2; ///< %3\n").arg("uint8_t", fieldName, fieldText); + calculatedLength += 1; + // Add pack line to message_xx_pack function + // packLines += QString("\ti += put_uint8_t_by_index(%1, i, msg->payload); // %2\n").arg(mavlinkVersion).arg(fieldText); + packLines += QString("\n\tp->%2 = MAVLINK_VERSION; // %1:%3").arg(fieldType, fieldName, e2.text()); + // Add decode function for this type + decodeLines += QString("\n\t%1->%2 = mavlink_msg_%1_get_%2(msg);").arg(messageName, fieldName); + + if (fieldOffset != "") { // does not use the number - always moves up one slot + QStringList itemList; + // Swap field in C structure + itemList = cStructLines.split("\n", QString::SkipEmptyParts); + if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); else ; + cStructLines = itemList.join("\n") + "\n"; + + // Swap line in message_xx_pack function + itemList = packLines.split("\n", QString::SkipEmptyParts); + if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); else ; + packLines = itemList.join("\n") + "\n"; + + // Swap line in decode function for this type + itemList = decodeLines.split("\n", QString::SkipEmptyParts); + if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); else ; + decodeLines = itemList.join("\n") + "\n"; + } + } + + // Array handling is different from simple types + else if (fieldType.startsWith("array")) + { + int arrayLength = QString(fieldType.split("[").at(1).split("]").first()).toInt(); + QString arrayType = fieldType.split("[").first(); + if (arrayType.contains("array")) calculatedLength += arrayLength; else + if (arrayType.contains("char")) calculatedLength += arrayLength; else + if (arrayType.contains("int8")) calculatedLength += arrayLength; else + if (arrayType.contains("int16")) calculatedLength += arrayLength*2; else + if (arrayType.contains("int32")) calculatedLength += arrayLength*4; else + if (arrayType.contains("int64")) calculatedLength += arrayLength*8; else + if (arrayType == "float") calculatedLength += arrayLength*4; else { + emit parseState(tr("ERROR: In message %1 inavlid array type %4 used near line %2 of file %3\nAbort.").arg(messageName, QString::number(e.lineNumber()), fileName, arrayType)); + return false; + } + packParameters += QString(", const ") + QString("int8_t*") + " " + fieldName; + packArguments += ", " + messageName + "->" + fieldName; + + // Add field to C structure + cStructLines += QString("\t%1 %2[%3]; ///< %4\n").arg("int8_t", fieldName, QString::number(arrayLength), fieldText); + // Add pack line to message_xx_pack function + // packLines += QString("\ti += put_%1_by_index(%2, %3, i, msg->payload); // %4\n").arg(arrayType, fieldName, QString::number(arrayLength), fieldText); + packLines += QString("\tmemcpy( p->%2, %2, sizeof(p->%2)); // %1[%3]:%4\n").arg(arrayType, fieldName, QString::number(arrayLength), fieldText); + // Add decode function for this type + decodeLines += QString("\n\tmavlink_msg_%1_get_%2(msg, %1->%2);").arg(messageName, fieldName); + + if (fieldOffset != "") { // does not use the number - always moves up one slot + QStringList itemList; + // Swap field in C structure + itemList = cStructLines.split("\n", QString::SkipEmptyParts); + if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); else ; + cStructLines = itemList.join("\n") + "\n"; + + // Swap line in message_xx_pack function + itemList = packLines.split("\n", QString::SkipEmptyParts); + if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); else ; + packLines = itemList.join("\n") + "\n"; + + // Swap line in decode function for this type + itemList = decodeLines.split("\n", QString::SkipEmptyParts); + if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); else ; + decodeLines = itemList.join("\n") + "\n"; + } + + arrayDefines += QString("#define MAVLINK_MSG_%1_FIELD_%2_LEN %3\n").arg(messageName.toUpper(), fieldName.toUpper(), QString::number(arrayLength)); + } + else if (fieldType.startsWith("string")) + { + int arrayLength = QString(fieldType.split("[").at(1).split("]").first()).toInt(); + calculatedLength += arrayLength; + QString arrayType = fieldType.split("[").first(); + if (arrayType.contains("string")) calculatedLength += arrayLength; else + if (arrayType.contains("array")) calculatedLength += arrayLength; else + if (arrayType.contains("char")) calculatedLength += arrayLength; else + if (arrayType.contains("int8")) calculatedLength += arrayLength; else + if (arrayType.contains("int16")) calculatedLength += arrayLength*2; else + if (arrayType.contains("int32")) calculatedLength += arrayLength*4; else + if (arrayType.contains("int64")) calculatedLength += arrayLength*8; else + if (arrayType == "float") calculatedLength += arrayLength*4; else { + emit parseState(tr("ERROR: In message %1 inavlid array type %4 used near line %2 of file %3\nAbort.").arg(messageName, QString::number(e.lineNumber()), fileName, arrayType)); + return false; + } + packParameters += QString(", const ") + QString("char*") + " " + fieldName; + packArguments += ", " + messageName + "->" + fieldName; + + // Add field to C structure + cStructLines += QString("\t%1 %2[%3]; ///< %4\n").arg("char", fieldName, QString::number(arrayLength), fieldText); + // Add pack line to message_xx_pack function + // packLines += QString("\ti += put_%1_by_index(%2, %3, i, msg->payload); // %4\n").arg(arrayType, fieldName, QString::number(arrayLength), e2.text()); + packLines += QString("\tstrncpy( p->%2, %2, sizeof(p->%2)); // %1[%3]:%4\n").arg("char", fieldName, QString::number(arrayLength), fieldText); + // Add decode function for this type + decodeLines += QString("\tmavlink_msg_%1_get_%2(msg, %1->%2);\n").arg(messageName, fieldName); + arrayDefines += QString("#define MAVLINK_MSG_%1_FIELD_%2_LEN %3\n").arg(messageName.toUpper(), fieldName.toUpper(), QString::number(arrayLength)); + + if (fieldOffset != "") { // does not use the number - always moves up one slot + QStringList itemList; + // Swap field in C structure + itemList = cStructLines.split("\n", QString::SkipEmptyParts); + if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); else ; + cStructLines = itemList.join("\n") + "\n"; + + // Swap line in message_xx_pack function + itemList = packLines.split("\n", QString::SkipEmptyParts); + if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); else ; + packLines = itemList.join("\n") + "\n"; + + // Swap line in decode function for this type + itemList = decodeLines.split("\n", QString::SkipEmptyParts); + if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); else ; + decodeLines = itemList.join("\n") + "\n"; + } + } + // Expand array handling to all valid mavlink data types + else if(fieldType.contains('[') && fieldType.contains(']')) + { + int arrayLength = QString(fieldType.split("[").at(1).split("]").first()).toInt(); + QString arrayType = fieldType.split("[").first(); + if (arrayType.contains("array")) calculatedLength += arrayLength; else + if (arrayType.contains("char")) calculatedLength += arrayLength; else + if (arrayType.contains("int8")) calculatedLength += arrayLength; else + if (arrayType.contains("int16")) calculatedLength += arrayLength*2; else + if (arrayType.contains("int32")) calculatedLength += arrayLength*4; else + if (arrayType.contains("int64")) calculatedLength += arrayLength*8; else + if (arrayType == "float") calculatedLength += arrayLength*4; else { + emit parseState(tr("ERROR: In message %1 inavlid array type %4 used near line %2 of file %3\nAbort.").arg(messageName, QString::number(e.lineNumber()), fileName, arrayType)); + return false; + } + packParameters += QString(", const ") + arrayType + "* " + fieldName; + packArguments += ", " + messageName + "->" + fieldName; + + // Add field to C structure + cStructLines += QString("\t%1 %2[%3]; ///< %4\n").arg(arrayType, fieldName, QString::number(arrayLength), fieldText); + // Add pack line to message_xx_pack function + // packLines += QString("\ti += put_array_by_index((const int8_t*)%1, sizeof(%2)*%3, i, msg->payload); // %4\n").arg(fieldName, arrayType, QString::number(arrayLength), fieldText); + packLines += QString("\tmemcpy(p->%2, %2, sizeof(p->%2)); // %1[%3]:%4\n").arg(arrayType, fieldName, QString::number(arrayLength), fieldText); + // Add decode function for this type + decodeLines += QString("\n\tmavlink_msg_%1_get_%2(msg, %1->%2);").arg(messageName, fieldName); + + if (fieldOffset != "") { // does not use the number - always moves up one slot + QStringList itemList; + // Swap field in C structure + itemList = cStructLines.split("\n", QString::SkipEmptyParts); + if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); else ; + cStructLines = itemList.join("\n") + "\n"; + + // Swap line in message_xx_pack function + itemList = packLines.split("\n", QString::SkipEmptyParts); + if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); else ; + packLines = itemList.join("\n") + "\n"; + + // Swap line in decode function for this type + itemList = decodeLines.split("\n", QString::SkipEmptyParts); + if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); else ; + decodeLines = itemList.join("\n") + "\n"; + } + + arrayDefines += QString("#define MAVLINK_MSG_%1_FIELD_%2_LEN %3\n").arg(messageName.toUpper(), fieldName.toUpper(), QString::number(arrayLength)); + + // unpackingCode = QString("\n\tmemcpy(r_data, msg->payload%1, sizeof(%2)*%3);\n\treturn sizeof(%2)*%3;").arg(prepends, arrayType, QString::number(arrayLength)); + unpackingCode = QString("\n\tmemcpy(%1, p->%1, sizeof(p->%1));\n\treturn sizeof(p->%1);").arg(fieldName); + + // unpacking += unpackingComment + QString("static inline uint16_t mavlink_msg_%1_get_%2(const mavlink_message_t* msg, %3* r_data)\n{\n%4\n}\n\n").arg(messageName, fieldName, arrayType, unpackingCode); + unpacking += unpackingComment + QString("static inline uint16_t mavlink_msg_%1_get_%2(const mavlink_message_t* msg, %3* %2)\n{\n\tmavlink_%1_t *p = (mavlink_%1_t *)&msg->payload[0];\n%4\n}\n\n").arg(messageName, fieldName, arrayType, unpackingCode); + // decodeLines += ""; + prepends += QString("+sizeof(%1)*%2").arg(arrayType, QString::number(arrayLength)); + + } + else + // Handle simple types like integers and floats + { + packParameters += ", " + fieldType + " " + fieldName; + packArguments += ", " + messageName + "->" + fieldName; + + // Add field to C structure + cStructLines += QString("\t%1 %2; ///< %3\n").arg(fieldType, fieldName, fieldText); + // Add pack line to message_xx_pack function + // packLines += QString("\ti += put_%1_by_index(%2, i, msg->payload); // %3\n").arg(fieldType, fieldName, e2.text()); + packLines += QString("\tp->%2 = %2; // %1:%3\n").arg(fieldType, fieldName, e2.text()); + // Add decode function for this type + // decodeLines += QString("\t%1->%2 = mavlink_msg_%1_get_%2(msg);\n").arg(messageName, fieldName); + decodeLines += QString("\n\t%1 = p->%1;").arg(fieldName); + + if (fieldOffset != "") { // does not use the number - always moves up one slot + QStringList itemList; + // Swap field in C structure + itemList = cStructLines.split("\n", QString::SkipEmptyParts); + if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); else ; + cStructLines = itemList.join("\n") + "\n"; + + // Swap line in message_xx_pack function + itemList = packLines.split("\n", QString::SkipEmptyParts); + if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); else ; + packLines = itemList.join("\n") + "\n"; + + // Swap line in decode function for this type + itemList = decodeLines.split("\n", QString::SkipEmptyParts); + if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); else ; + decodeLines = itemList.join("\n") + "\n"; + } + + if (fieldType.contains("array")) calculatedLength += 1; else + if (fieldType.contains("char")) calculatedLength += 1; else + if (fieldType.contains("int8")) calculatedLength += 1; else + if (fieldType.contains("int16")) calculatedLength += 2; else + if (fieldType.contains("int32")) calculatedLength += 4; else + if (fieldType.contains("int64")) calculatedLength += 8; else + if (fieldType == "float") calculatedLength += 4; else { + emit parseState(tr("ERROR: In message %1 inavlid type %4 used near line %2 of file %3\nAbort.").arg(messageName, QString::number(e.lineNumber()), fileName, fieldType)); + return false; + } + } + + + // + // QString unpackingCode; + + if (fieldType == "uint8_t_mavlink_version") + { + // unpackingCode = QString("\treturn (%1)(msg->payload%2)[0];").arg("uint8_t", prepends); + unpackingCode = QString("\treturn (%1)(p->%2);").arg("uint8_t", fieldName); + } + else if (fieldType == "uint8_t" || fieldType == "int8_t") + { + // unpackingCode = QString("\treturn (%1)(msg->payload%2)[0];").arg(fieldType, prepends); + unpackingCode = QString("\treturn (%1)(p->%2);").arg(fieldType, fieldName); + } + else if (fieldType == "uint16_t" || fieldType == "int16_t") + { + // unpackingCode = QString("\tgeneric_16bit r;\n\tr.b[1] = (msg->payload%1)[0];\n\tr.b[0] = (msg->payload%1)[1];\n\treturn (%2)r.s;").arg(prepends).arg(fieldType); + unpackingCode = QString("\treturn (%1)(p->%2);").arg(fieldType, fieldName); + } + else if (fieldType == "uint32_t" || fieldType == "int32_t") + { + // unpackingCode = QString("\tgeneric_32bit r;\n\tr.b[3] = (msg->payload%1)[0];\n\tr.b[2] = (msg->payload%1)[1];\n\tr.b[1] = (msg->payload%1)[2];\n\tr.b[0] = (msg->payload%1)[3];\n\treturn (%2)r.i;").arg(prepends).arg(fieldType); + unpackingCode = QString("\treturn (%1)(p->%2);").arg(fieldType, fieldName); + } + else if (fieldType == "float") + { + // unpackingCode = QString("\tgeneric_32bit r;\n\tr.b[3] = (msg->payload%1)[0];\n\tr.b[2] = (msg->payload%1)[1];\n\tr.b[1] = (msg->payload%1)[2];\n\tr.b[0] = (msg->payload%1)[3];\n\treturn (%2)r.f;").arg(prepends).arg(fieldType); + unpackingCode = QString("\treturn (%1)(p->%2);").arg(fieldType, fieldName); + } + else if (fieldType == "uint64_t" || fieldType == "int64_t") + { + // unpackingCode = QString("\tgeneric_64bit r;\n\tr.b[7] = (msg->payload%1)[0];\n\tr.b[6] = (msg->payload%1)[1];\n\tr.b[5] = (msg->payload%1)[2];\n\tr.b[4] = (msg->payload%1)[3];\n\tr.b[3] = (msg->payload%1)[4];\n\tr.b[2] = (msg->payload%1)[5];\n\tr.b[1] = (msg->payload%1)[6];\n\tr.b[0] = (msg->payload%1)[7];\n\treturn (%2)r.ll;").arg(prepends).arg(fieldType); + unpackingCode = QString("\treturn (%1)(p->%2);").arg(fieldType, fieldName); + } + else if (fieldType.startsWith("array")) + { // fieldtype formatis string[n] where n is the number of bytes, extract n from field type string + // unpackingCode = QString("\n\tmemcpy(r_data, msg->payload%1, %2);\n\treturn %2;").arg(filedName, prepends, fieldType.split("[").at(1).split("]").first()); + unpackingCode = QString("\n\tmemcpy(%1, p->%1, sizeof(p->%1));\n\treturn sizeof(p->%1);").arg(fieldName); + } + else if (fieldType.startsWith("string")) + { // fieldtype formatis string[n] where n is the number of bytes, extract n from field type string + // unpackingCode = QString("\n\tstrcpy(r_data, msg->payload%1, %2);\n\treturn %2;").arg(prepends, fieldType.split("[").at(1).split("]").first()); + unpackingCode = QString("\n\tstrncpy(%1, p->%1, sizeof(p->%1));\n\treturn sizeof(p->%1);").arg(fieldName); + } + + + // Generate the message decoding function + if (fieldType.contains("uint8_t_mavlink_version")) + { + // unpacking += unpackingComment + QString("static inline %1 mavlink_msg_%2_get_%3(const mavlink_message_t* msg)\n{\n%4\n}\n\n").arg("uint8_t", messageName, fieldName, unpackingCode); + unpacking += unpackingComment + QString("static inline %1 mavlink_msg_%2_get_%3(const mavlink_message_t* msg)\n{\n\tmavlink_%2_t *p = (mavlink_%2_t *)&msg->payload[0];\n%4\n}\n\n").arg("uint8_t", messageName, fieldName, unpackingCode); + decodeLines += ""; + prepends += "+sizeof(uint8_t)"; + } + // Array handling is different from simple types + else if (fieldType.startsWith("array")) + { + // unpacking += unpackingComment + QString("static inline uint16_t mavlink_msg_%1_get_%2(const mavlink_message_t* msg, int8_t* r_data)\n{\n%4\n}\n\n").arg(messageName, fieldName, unpackingCode); + unpacking += unpackingComment + QString("static inline uint16_t mavlink_msg_%1_get_%2(const mavlink_message_t* msg, int8_t* %2)\n{\n\tmavlink_%1_t *p = (mavlink_%1_t *)&msg->payload[0];\n%3\n}\n\n").arg(messageName, fieldName, unpackingCode); + decodeLines += ""; + QString arrayLength = QString(fieldType.split("[").at(1).split("]").first()); + prepends += "+" + arrayLength; + } + else if (fieldType.startsWith("string")) + { + // unpacking += unpackingComment + QString("static inline uint16_t mavlink_msg_%1_get_%2(const mavlink_message_t* msg, char* r_data)\n{\n%4\n}\n\n").arg(messageName, fieldName, unpackingCode); + unpacking += unpackingComment + QString("static inline uint16_t mavlink_msg_%1_get_%2(const mavlink_message_t* msg, char* %2)\n{\n\tmavlink_%1 *p = (mavlink_%1 *)&msg->payload[0];\n%3\n}\n\n").arg(messageName, fieldName, unpackingCode); + decodeLines += ""; + QString arrayLength = QString(fieldType.split("[").at(1).split("]").first()); + prepends += "+" + arrayLength; + } + else if(fieldType.contains('[') && fieldType.contains(']')) + { + // prevent this case from being caught in the following else + } + else + { + // unpacking += unpackingComment + QString("static inline %1 mavlink_msg_%2_get_%3(const mavlink_message_t* msg)\n{\n%4\n}\n\n").arg(fieldType, messageName, fieldName, unpackingCode); + unpacking += unpackingComment + QString("static inline %1 mavlink_msg_%2_get_%3(const mavlink_message_t* msg)\n{\n\tmavlink_%2_t *p = (mavlink_%2_t *)&msg->payload[0];\n%4\n}\n\n").arg(fieldType, messageName, fieldName, unpackingCode); + decodeLines += ""; + prepends += "+sizeof(" + e2.attribute("type", "void") + ")"; + } + } + f = f.nextSibling(); + } + + // cStruct = cStruct.arg(cStructName, cStructLines, QString::number(calculatedLength) ); + cStruct = cStruct.arg(cStructName, cStructLines ); + lcmStructDefs.append("\n").append(cStruct).append("\n"); + pack = pack.arg(messageName, packParameters, messageName.toUpper(), packLines); + packChan = packChan.arg(messageName, packParameters, messageName.toUpper(), packLines); + encode = encode.arg(messageName).arg(cStructName).arg(packArguments); + // decode = decode.arg(messageName).arg(cStructName).arg(decodeLines); + decode = decode.arg(messageName).arg(cStructName); + // QString compactSend("#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS\n\nstatic inline void mavlink_msg_%3_send(%1 chan%5)\n{\n\t%2 msg;\n\tmavlink_msg_%3_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg%4);\n\tmavlink_send_uart(chan, &msg);\n}\n\n#endif"); + QString compactSend("#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS\nstatic inline void mavlink_msg_%3_send(%1 chan%5)\n{\n\t%2 msg;\n\tuint16_t checksum;\n\tmavlink_%3_t *p = (mavlink_%3_t *)&msg.payload[0];\n\n%6\n\tmsg.STX = MAVLINK_STX;\n\tmsg.len = MAVLINK_MSG_ID_%4_LEN;\n\tmsg.msgid = MAVLINK_MSG_ID_%4;\n"); + QString compactSend2("\tmsg.sysid = mavlink_system.sysid;\n\tmsg.compid = mavlink_system.compid;\n\tmsg.seq = mavlink_get_channel_status(chan)->current_tx_seq;\n\tmavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;\n"); + QString compactSend3("\tchecksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);\n\tmsg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte\n\tmsg.ck_b = (uint8_t)(checksum >> 8); ///< High byte\n\n\tmavlink_send_msg(chan, &msg);\n}\n\n#endif"); + // compactSend = compactSend.arg(channelType, messageType, messageName, sendArguments, packParameters ); + compactSend = compactSend.arg(channelType, messageType, messageName, messageName.toUpper(), packParameters, packLines ) + compactSend2 + compactSend3; + QString compact2Send("\n\n#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL\nstatic inline void mavlink_msg_%3_send(%1 chan%5)\n{\n\t%2 hdr;\n\tmavlink_%3_t payload;\n\tuint16_t checksum;\n\tmavlink_%3_t *p = &payload;\n\n%6\n\thdr.STX = MAVLINK_STX;\n\thdr.len = MAVLINK_MSG_ID_%4_LEN;\n\thdr.msgid = MAVLINK_MSG_ID_%4;\n"); + QString compact2Send2("\thdr.sysid = mavlink_system.sysid;\n\thdr.compid = mavlink_system.compid;\n\thdr.seq = mavlink_get_channel_status(chan)->current_tx_seq;\n\tmavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1;\n\tmavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES );\n"); + QString compact2Send3("\n\tcrc_init(&checksum);\n\tchecksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN);\n\tchecksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len );\n\thdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte\n\thdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte\n\n\tmavlink_send_mem(chan, (uint8_t *)&payload, hdr.len);\n\tmavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES);\n}\n\n#endif"); + compact2Send = compact2Send.arg(channelType, headerType, messageName, messageName.toUpper(), packParameters, packLines ) + compact2Send2 + compact2Send3; + QString cFile = "// MESSAGE " + messageName.toUpper() + " PACKING\n\n" + idDefine.arg(messageName.toUpper(), QString::number(messageId), QString::number(calculatedLength)) + "\n\n" + cStruct + "\n" + arrayDefines + "\n" + commentContainer.arg(messageName.toLower(), commentLines) + pack + commentPackChanContainer.arg(messageName.toLower(), commentLines) + packChan + commentEncodeContainer.arg(messageName.toLower()) + encode + "\n" + commentSendContainer.arg(messageName.toLower(), commentLines) + compactSend + compact2Send + "\n" + "// MESSAGE " + messageName.toUpper() + " UNPACKING\n\n" + unpacking + commentDecodeContainer.arg(messageName.toLower()) + decode; + cFiles.append(qMakePair(QString("mavlink_msg_%1.h").arg(messageName), cFile)); + } // Check if tag = message + } // Check if e = NULL + n = n.nextSibling(); + } // While through + n = p; + + } // Check if tag = messages + } // Check if e = NULL + n = n.nextSibling(); + } // While through include and messages + // One up - current node = parent + n = p; + + } // Check if tag = mavlink + } // Check if e = NULL + n = n.nextSibling(); + } // While through root children + + // Add version to main header + + mainHeader += "// MAVLINK VERSION\n\n"; + mainHeader += QString("#ifndef MAVLINK_VERSION\n#define MAVLINK_VERSION %1\n#endif\n\n").arg(mavlinkVersion); + mainHeader += QString("#if (MAVLINK_VERSION == 0)\n#undef MAVLINK_VERSION\n#define MAVLINK_VERSION %1\n#endif\n\n").arg(mavlinkVersion); + + // Add enums to main header + + mainHeader += "// ENUM DEFINITIONS\n\n"; + mainHeader += enums; + mainHeader += "\n"; + + mainHeader += "// MESSAGE DEFINITIONS\n\n"; + // Create directory if it doesn't exist, report result in success + if (!dir.exists()) success = success && dir.mkpath(outputDirName + "/" + messagesDirName); + for (int i = 0; i < cFiles.size(); i++) + { + QFile rawFile(dir.filePath(cFiles.at(i).first)); + bool ok = rawFile.open(QIODevice::WriteOnly | QIODevice::Text); + success = success && ok; + rawFile.write(cFiles.at(i).second.toLatin1()); + rawFile.close(); + mainHeader += includeLine.arg(messagesDirName + "/" + cFiles.at(i).first); + } + + mainHeader += "#ifdef __cplusplus\n}\n#endif\n"; + mainHeader += "#endif"; + // Newline to make compiler happy + mainHeader += "\n"; + + // Write main header + QFile rawHeader(outputDirName + "/" + mainHeaderName); + bool ok = rawHeader.open(QIODevice::WriteOnly | QIODevice::Text); + success = success && ok; + rawHeader.write(mainHeader.toLatin1()); + rawHeader.close(); + + // Write alias mavlink header + QFile mavlinkHeader(outputDirName + "/mavlink.h"); + ok = mavlinkHeader.open(QIODevice::WriteOnly | QIODevice::Text); + success = success && ok; + QString mHeader = QString("/** @file\n *\t@brief MAVLink comm protocol.\n *\t@see http://pixhawk.ethz.ch/software/mavlink\n *\t Generated on %1\n */\n#ifndef MAVLINK_H\n#define MAVLINK_H\n\n").arg(date); // The main header includes all messages + // Mark all code as C code +// mHeader += "\n#include \"" + mainHeaderName + "\"\n\n"; + mHeader += "#pragma pack(push,1)\n#include \"" + mainHeaderName + "\"\n#ifdef MAVLINK_CHECK_LENGTH\n#include \"lengths.h\"\n#endif\n#pragma pack(pop)\n"; + mHeader += "#endif\n"; + mavlinkHeader.write(mHeader.toLatin1()); + mavlinkHeader.close(); + + // Write C structs / lcm definitions + // QFile lcmStructs(outputDirName + "/mavlink.lcm"); + // ok = lcmStructs.open(QIODevice::WriteOnly | QIODevice::Text); + // success = success && ok; + // lcmStructs.write(lcmStructDefs.toLatin1()); + + return success; +} diff --git a/src/apps/mavlinkgen/generator/MAVLinkXMLParserV10.h b/src/apps/mavlinkgen/generator/MAVLinkXMLParserV10.h new file mode 100644 index 0000000000000000000000000000000000000000..0b17e5ad834e6d486a7a1db43769c98f4797fcec --- /dev/null +++ b/src/apps/mavlinkgen/generator/MAVLinkXMLParserV10.h @@ -0,0 +1,66 @@ +/*===================================================================== + +QGroundControl Open Source Ground Control Station + +(c) 2009, 2010 QGROUNDCONTROL PROJECT + +This file is part of the QGROUNDCONTROL 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 . + +======================================================================*/ + +/** + * @file + * @brief Definition of class MAVLinkXMLParserV10 + * @author Lorenz Meier + */ + +#ifndef MAVLINKXMLPARSERV10_H +#define MAVLINKXMLPARSERV10_H + +#include +#include +#include + +/** + * @brief MAVLink micro air vehicle protocol generator + * + * MAVLink is a generic communication protocol for micro air vehicles. + * for more information, please see the official website. + * @ref http://pixhawk.ethz.ch/software/mavlink/ + **/ +class MAVLinkXMLParserV10 : public QObject +{ + Q_OBJECT +public: + MAVLinkXMLParserV10(QDomDocument* document, QString outputDirectory, QObject* parent=0); + MAVLinkXMLParserV10(QString document, QString outputDirectory, QObject* parent=0); + ~MAVLinkXMLParserV10(); + +public slots: + /** @brief Parse XML and generate C files */ + bool generate(); + +signals: + /** @brief Status message on the parsing */ + void parseState(QString message); + +protected: + QDomDocument* doc; + QString outputDirName; + QString fileName; +}; + +#endif // MAVLINKXMLPARSERV10_H diff --git a/src/apps/mavlinkgen/mavlinkgen.pri b/src/apps/mavlinkgen/mavlinkgen.pri index 8270518b47c0eca30cfe5c1fce807f747f1fa2d1..a5ef920b5520f1b382f58c55f91bdd41ea641c7f 100644 --- a/src/apps/mavlinkgen/mavlinkgen.pri +++ b/src/apps/mavlinkgen/mavlinkgen.pri @@ -29,6 +29,7 @@ FORMS += ui/XMLCommProtocolWidget.ui HEADERS += \ ui/XMLCommProtocolWidget.h \ generator/MAVLinkXMLParser.h \ + generator/MAVLinkXMLParserV10.h \ ui/DomItem.h \ ui/DomModel.h \ ui/QGCMAVLinkTextEdit.h @@ -37,6 +38,7 @@ SOURCES += \ ui/DomItem.cc \ ui/DomModel.cc \ generator/MAVLinkXMLParser.cc \ + generator/MAVLinkXMLParserV10.cc \ ui/QGCMAVLinkTextEdit.cc RESOURCES += mavlinkgen.qrc diff --git a/src/apps/mavlinkgen/ui/XMLCommProtocolWidget.cc b/src/apps/mavlinkgen/ui/XMLCommProtocolWidget.cc index 337a0195816335edf20b436a78f2d22c5fd09b47..82b37be0ed00338355affc9380bc86e9b62f068f 100644 --- a/src/apps/mavlinkgen/ui/XMLCommProtocolWidget.cc +++ b/src/apps/mavlinkgen/ui/XMLCommProtocolWidget.cc @@ -6,6 +6,7 @@ #include "XMLCommProtocolWidget.h" #include "ui_XMLCommProtocolWidget.h" #include "MAVLinkXMLParser.h" +#include "MAVLinkXMLParserV10.h" #include #include @@ -131,18 +132,37 @@ void XMLCommProtocolWidget::generate() // Syntax check already gives output return; } - - MAVLinkXMLParser* parser = new MAVLinkXMLParser(m_ui->fileNameLabel->text().trimmed(), m_ui->outputDirNameLabel->text().trimmed()); - connect(parser, SIGNAL(parseState(QString)), m_ui->compileLog, SLOT(appendHtml(QString))); - bool result = parser->generate(); - if (result) { + + MAVLinkXMLParser* parser = NULL; + MAVLinkXMLParserV10* parserV10 = NULL; + + bool result = false; + + if (m_ui->versionComboBox->currentIndex() == 0) + { + MAVLinkXMLParser* parser = new MAVLinkXMLParser(m_ui->fileNameLabel->text().trimmed(), m_ui->outputDirNameLabel->text().trimmed()); + connect(parser, SIGNAL(parseState(QString)), m_ui->compileLog, SLOT(appendHtml(QString))); + result = parser->generate(); + } + else if (m_ui->versionComboBox->currentIndex() == 1) + { + MAVLinkXMLParserV10* parserV10 = new MAVLinkXMLParserV10(m_ui->fileNameLabel->text().trimmed(), m_ui->outputDirNameLabel->text().trimmed()); + connect(parserV10, SIGNAL(parseState(QString)), m_ui->compileLog, SLOT(appendHtml(QString))); + result = parserV10->generate(); + } + + if (result) + { QMessageBox msgBox; msgBox.setText(QString("The C code / headers have been generated in folder\n%1").arg(m_ui->outputDirNameLabel->text().trimmed())); msgBox.exec(); - } else { + } + else + { QMessageBox::critical(this, tr("C code generation failed, please see the compile log for further information"), QString("The C code / headers could not be written to folder\n%1").arg(m_ui->outputDirNameLabel->text().trimmed()), QMessageBox::Ok); } - delete parser; + if (parser) delete parser; + if (parserV10) delete parserV10; } void XMLCommProtocolWidget::save() diff --git a/src/apps/mavlinkgen/ui/XMLCommProtocolWidget.ui b/src/apps/mavlinkgen/ui/XMLCommProtocolWidget.ui index 089c8afa76687e619412c067157fbfb109c74999..878d7b39854034bdd465827c254c3df7177a476c 100644 --- a/src/apps/mavlinkgen/ui/XMLCommProtocolWidget.ui +++ b/src/apps/mavlinkgen/ui/XMLCommProtocolWidget.ui @@ -13,7 +13,7 @@ Form - + 6 @@ -51,12 +51,12 @@ Select input file - + :/images/status/folder-open.svg:/images/status/folder-open.svg - + @@ -97,49 +97,70 @@ Select directory - + :/images/status/folder-open.svg:/images/status/folder-open.svg - + - + Compile Output - + - + No file loaded - + Save file - + Save and generate - + :/images/categories/applications-system.svg:/images/categories/applications-system.svg + + + + Select MAVLink Version + + + + + + + + MAVLink v0.9 (-Aug'10) + + + + + MAVLink v1.0 (Sept'10+) + + + + @@ -150,7 +171,7 @@ - + diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index 69ee72af4d437d2e56a7eee212d545e22eb9ac3b..043fcb3a6b27ef7fa9a4a80ce5d75b6448901ba4 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -17,7 +17,6 @@ #include #include -//#include "MG.h" #include "MAVLinkProtocol.h" #include "UASInterface.h" #include "UASManager.h" @@ -28,7 +27,6 @@ #include "ArduPilotMegaMAV.h" #include "configuration.h" #include "LinkManager.h" -//#include "MainWindow.h" #include "QGCMAVLink.h" #include "QGCMAVLinkUASFactory.h" #include "QGC.h" @@ -182,14 +180,14 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) unsigned int decodeState = mavlink_parse_char(link->getId(), (uint8_t)(b.at(position)), &message, &status); if (decodeState == 1) { -#ifdef MAVLINK_MESSAGE_LENGTHS - const uint8_t message_lengths[] = MAVLINK_MESSAGE_LENGTHS; - if (message.msgid >= sizeof(message_lengths) || - message.len != message_lengths[message.msgid]) { - qDebug() << "MAVLink message " << message.msgid << " length incorrect (was " << message.len << " expected " << message_lengths[message.msgid] << ")"; - continue; - } -#endif +//#ifdef MAVLINK_MESSAGE_LENGTHS +// const uint8_t message_lengths[] = MAVLINK_MESSAGE_LENGTHS; +// if (message.msgid >= sizeof(message_lengths) || +// message.len != message_lengths[message.msgid]) { +// qDebug() << "MAVLink message " << message.msgid << " length incorrect (was " << message.len << " expected " << message_lengths[message.msgid] << ")"; +// continue; +// } +//#endif // Log data if (m_loggingEnabled && m_logfile) { const int len = MAVLINK_MAX_PACKET_LEN+sizeof(quint64); diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc index 50e87e9ff790391052a43fe341b2341d39ca25df..44f338b89837e29511e889ca783d1004a39917ec 100644 --- a/src/comm/MAVLinkSimulationLink.cc +++ b/src/comm/MAVLinkSimulationLink.cc @@ -36,7 +36,7 @@ This file is part of the QGROUNDCONTROL project #include #include #include -#include "MG.h" +#include #include "LinkManager.h" #include "MAVLinkProtocol.h" #include "MAVLinkSimulationLink.h" @@ -78,7 +78,7 @@ MAVLinkSimulationLink::MAVLinkSimulationLink(QString readFile, QString writeFile simulationHeader = simulationFile->readLine(); } receiveFile = new QFile(writeFile, this); - lastSent = MG::TIME::getGroundTimeNow() * 1000; + lastSent = QGC::groundTimeMilliseconds(); if (simulationFile->exists()) { this->name = "Simulation: " + QFileInfo(simulationFile->fileName()).fileName(); @@ -118,7 +118,7 @@ void MAVLinkSimulationLink::run() static quint64 last = 0; - if (MG::TIME::getGroundTimeNow() - last >= rate) { + if (QGC::groundTimeMilliseconds() - last >= rate) { if (_isConnected) { mainloop(); @@ -132,7 +132,7 @@ void MAVLinkSimulationLink::run() readBytes(); } - last = MG::TIME::getGroundTimeNow(); + last = QGC::groundTimeMilliseconds(); } QGC::SLEEP::msleep(3); @@ -265,9 +265,6 @@ void MAVLinkSimulationLink::mainloop() double d = QString(parts.at(i)).toDouble(&res); if (!res) d = 0; - //qDebug() << "TIME" << time << "VALUE" << d; - //emit valueChanged(220, keys.at(i), d, MG::TIME::getGroundTimeNow()); - if (keys.value(i, "") == "Accel._X") { rawImuValues.xacc = d; } @@ -345,7 +342,7 @@ void MAVLinkSimulationLink::mainloop() //qDebug() << "ATTITUDE" << "BUF LEN" << bufferlength << "POINTER" << streampointer; - //qDebug() << "REALTIME" << MG::TIME::getGroundTimeNow() << "ONBOARDTIME" << attitude.msec << "ROLL" << attitude.roll; + //qDebug() << "REALTIME" << QGC::groundTimeMilliseconds() << "ONBOARDTIME" << attitude.msec << "ROLL" << attitude.roll; } @@ -667,11 +664,6 @@ qint64 MAVLinkSimulationLink::bytesAvailable() void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size) { - //qDebug() << "Simulation received " << size << " bytes from groundstation: "; - - // Increase write counter - //bitsSentTotal += size * 8; - // Parse bytes mavlink_message_t msg; mavlink_status_t comm; diff --git a/src/comm/MAVLinkSimulationMAV.cc b/src/comm/MAVLinkSimulationMAV.cc index 356104601ddf338dbe1c95ad0837dc8a654aaffc..ea03225e9271d96734957f90a77b3e6871340904 100644 --- a/src/comm/MAVLinkSimulationMAV.cc +++ b/src/comm/MAVLinkSimulationMAV.cc @@ -23,20 +23,20 @@ MAVLinkSimulationMAV::MAVLinkSimulationMAV(MAVLinkSimulationLink *parent, int sy yaw(0.0), globalNavigation(true), firstWP(false), -// previousSPX(8.548056), -// previousSPY(47.376389), -// previousSPZ(550), -// previousSPYaw(0.0), -// nextSPX(8.548056), -// nextSPY(47.376389), -// nextSPZ(550), - previousSPX(37.480391), - previousSPY(122.282883), - previousSPZ(550), - previousSPYaw(0.0), - nextSPX(37.480391), - nextSPY(122.282883), - nextSPZ(550), + // previousSPX(8.548056), + // previousSPY(47.376389), + // previousSPZ(550), + // previousSPYaw(0.0), + // nextSPX(8.548056), + // nextSPY(47.376389), + // nextSPZ(550), + previousSPX(37.480391), + previousSPY(122.282883), + previousSPZ(550), + previousSPYaw(0.0), + nextSPX(37.480391), + nextSPY(122.282883), + nextSPZ(550), nextSPYaw(0.0), sys_mode(MAV_MODE_READY), sys_state(MAV_STATE_STANDBY), diff --git a/src/comm/MAVLinkSimulationWaypointPlanner.cc b/src/comm/MAVLinkSimulationWaypointPlanner.cc index d78b4d5bfe0e96d38f1a5fd497b2f83474956eb0..55b87625582a78b0da151ae2a1832f6ea1ae7d6c 100644 --- a/src/comm/MAVLinkSimulationWaypointPlanner.cc +++ b/src/comm/MAVLinkSimulationWaypointPlanner.cc @@ -450,7 +450,7 @@ MAVLinkSimulationWaypointPlanner::MAVLinkSimulationWaypointPlanner(MAVLinkSimula timestamp_last_send_setpoint(0), systemid(sysid), compid(MAV_COMP_ID_WAYPOINTPLANNER), - setpointDelay(10), + setpointDelay(1000), yawTolerance(0.4f), verbose(true), debug(false), @@ -553,7 +553,7 @@ void MAVLinkSimulationWaypointPlanner::send_setpoint(uint16_t seq) emit messageSent(msg); } - uint64_t now = QGC::groundTimeUsecs()/1000; + uint64_t now = QGC::groundTimeMilliseconds(); timestamp_last_send_setpoint = now; } } @@ -699,7 +699,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* //qDebug() << "MAV: %d WAYPOINTPLANNER GOT MESSAGE" << systemid; - uint64_t now = QGC::groundTimeUsecs()/1000; + uint64_t now = QGC::groundTimeMilliseconds(); if (now-protocol_timestamp_lastaction > protocol_timeout && current_state != PX_WPP_IDLE) { if (verbose) qDebug() << "Last operation (state=%u) timed out, changing state to PX_WPP_IDLE" << current_state; current_state = PX_WPP_IDLE; diff --git a/src/comm/QGCFlightGearLink.cc b/src/comm/QGCFlightGearLink.cc new file mode 100644 index 0000000000000000000000000000000000000000..83e2088df1614b56cbe2d94f75035bb1424fad29 --- /dev/null +++ b/src/comm/QGCFlightGearLink.cc @@ -0,0 +1,307 @@ +/*===================================================================== + +QGroundControl Open Source Ground Control Station + +(c) 2009 - 2011 QGROUNDCONTROL PROJECT + +This file is part of the QGROUNDCONTROL 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 . + +======================================================================*/ + +/** + * @file + * @brief Definition of UDP connection (server) for unmanned vehicles + * @author Lorenz Meier + * + */ + +#include +#include +#include +#include +#include +#include "QGCFlightGearLink.h" +#include "QGC.h" +#include + +QGCFlightGearLink::QGCFlightGearLink(QString remoteHost, QHostAddress host, quint16 port) +{ + this->host = host; + this->port = port; + this->connectState = false; + this->currentPort = 49000; + + // Set unique ID and add link to the list of links + this->name = tr("FlightGear Link (port:%1)").arg(port); + setRemoteHost(remoteHost); + connect(&refreshTimer, SIGNAL(timeout()), this, SLOT(sendUAVUpdate())); + refreshTimer.start(20); // 50 Hz UAV -> Simulation update rate +} + +QGCFlightGearLink::~QGCFlightGearLink() +{ + disconnect(); +} + +/** + * @brief Runs the thread + * + **/ +void QGCFlightGearLink::run() +{ +// forever +// { +// QGC::SLEEP::msleep(5000); +// } + exec(); +} + +void QGCFlightGearLink::setPort(int port) +{ + this->port = port; + disconnectSimulation(); + connectSimulation(); +} + +/** + * @param host Hostname in standard formatting, e.g. localhost:14551 or 192.168.1.1:14551 + */ +void QGCFlightGearLink::setRemoteHost(const QString& host) +{ + //qDebug() << "UDP:" << "ADDING HOST:" << host; + if (host.contains(":")) + { + //qDebug() << "HOST: " << host.split(":").first(); + QHostInfo info = QHostInfo::fromName(host.split(":").first()); + if (info.error() == QHostInfo::NoError) + { + // Add host + QList hostAddresses = info.addresses(); + QHostAddress address; + for (int i = 0; i < hostAddresses.size(); i++) + { + // Exclude loopback IPv4 and all IPv6 addresses + if (!hostAddresses.at(i).toString().contains(":")) + { + address = hostAddresses.at(i); + } + } + currentHost = address; + //qDebug() << "Address:" << address.toString(); + // Set port according to user input + currentPort = host.split(":").last().toInt(); + } + } + else + { + QHostInfo info = QHostInfo::fromName(host); + if (info.error() == QHostInfo::NoError) + { + // Add host + currentHost = info.addresses().first(); + } + } +} + +void QGCFlightGearLink::updateGlobalPosition(quint64 time, double lat, double lon, double alt) +{ + +} + +void QGCFlightGearLink::sendUAVUpdate() +{ + // 37.613548,-122.357246,-9999.000000,0.000000,0.424000,297.899994,0.000000\n + // magnetos,aileron,elevator,rudder,throttle\n + + float magnetos = 3.0f; + float aileron = 0.0f; + float elevator = 0.0f; + float rudder = 0.0f; + float throttle = 90.0f; + + QString state("%1,%2,%3,%4,%5\n"); + state = state.arg(magnetos).arg(aileron).arg(elevator).arg(rudder).arg(throttle); + writeBytes(state.toAscii().constData(), state.length()); +} + +void QGCFlightGearLink::writeBytes(const char* data, qint64 size) +{ +//#define QGCFlightGearLink_DEBUG +#ifdef QGCFlightGearLink_DEBUG + QString bytes; + QString ascii; + for (int i=0; i 31 && data[i] < 127) + { + ascii.append(data[i]); + } + else + { + ascii.append(219); + } + } + qDebug() << "Sent" << size << "bytes to" << currentHost.toString() << ":" << currentPort << "data:"; + qDebug() << bytes; + qDebug() << "ASCII:" << ascii; +#endif + socket->writeDatagram(data, size, currentHost, currentPort); +} + +/** + * @brief Read a number of bytes from the interface. + * + * @param data Pointer to the data byte array to write the bytes to + * @param maxLength The maximum number of bytes to write + **/ +void QGCFlightGearLink::readBytes() +{ + const qint64 maxLength = 65536; + static char data[maxLength]; + QHostAddress sender; + quint16 senderPort; + + unsigned int s = socket->pendingDatagramSize(); + if (s > maxLength) std::cerr << __FILE__ << __LINE__ << " UDP datagram overflow, allowed to read less bytes than datagram size" << std::endl; + socket->readDatagram(data, maxLength, &sender, &senderPort); + + // FIXME TODO Check if this method is better than retrieving the data by individual processes + QByteArray b(data, s); + //emit bytesReceived(this, b); + + // Print string + qDebug() << "FG LINK GOT:" << QString(b); + +// // Echo data for debugging purposes +// std::cerr << __FILE__ << __LINE__ << "Received datagram:" << std::endl; +// int i; +// for (i=0; ipendingDatagramSize(); +} + +/** + * @brief Disconnect the connection. + * + * @return True if connection has been disconnected, false if connection couldn't be disconnected. + **/ +bool QGCFlightGearLink::disconnectSimulation() +{ + delete socket; + socket = NULL; + + connectState = false; + + emit flightGearDisconnected(); + emit flightGearConnected(false); + return !connectState; +} + +/** + * @brief Connect the connection. + * + * @return True if connection has been established, false if connection couldn't be established. + **/ +bool QGCFlightGearLink::connectSimulation() +{ + socket = new QUdpSocket(this); + + //Check if we are using a multicast-address +// bool multicast = false; +// if (host.isInSubnet(QHostAddress("224.0.0.0"),4)) +// { +// multicast = true; +// connectState = socket->bind(port, QUdpSocket::ShareAddress); +// } +// else +// { + connectState = socket->bind(host, port); +// } + + //Provides Multicast functionality to UdpSocket + /* not working yet + if (multicast) + { + int sendingFd = socket->socketDescriptor(); + + if (sendingFd != -1) + { + // set up destination address + struct sockaddr_in sendAddr; + memset(&sendAddr,0,sizeof(sendAddr)); + sendAddr.sin_family=AF_INET; + sendAddr.sin_addr.s_addr=inet_addr(HELLO_GROUP); + sendAddr.sin_port=htons(port); + + // set TTL + unsigned int ttl = 1; // restricted to the same subnet + if (setsockopt(sendingFd, IPPROTO_IP, IP_MULTICAST_TTL, (unsigned int*)&ttl, sizeof(ttl) ) < 0) + { + std::cout << "TTL failed\n"; + } + } + } + */ + + //QObject::connect(socket, SIGNAL(readyRead()), this, SLOT(readPendingDatagrams())); + QObject::connect(socket, SIGNAL(readyRead()), this, SLOT(readBytes())); + + emit flightGearConnected(connectState); + if (connectState) { + emit flightGearConnected(); + connectionStartTime = QGC::groundTimeUsecs()/1000; + } + + start(HighPriority); + return connectState; +} + +/** + * @brief Check if connection is active. + * + * @return True if link is connected, false otherwise. + **/ +bool QGCFlightGearLink::isConnected() +{ + return connectState; +} + +QString QGCFlightGearLink::getName() +{ + return name; +} + +void QGCFlightGearLink::setName(QString name) +{ + this->name = name; +// emit nameChanged(this->name); +} diff --git a/src/comm/QGCFlightGearLink.h b/src/comm/QGCFlightGearLink.h new file mode 100644 index 0000000000000000000000000000000000000000..8b23a31fcc0f967de345bef5eb5740b31556e0fa --- /dev/null +++ b/src/comm/QGCFlightGearLink.h @@ -0,0 +1,129 @@ +/*===================================================================== + +QGroundControl Open Source Ground Control Station + +(c) 2009 - 2011 QGROUNDCONTROL PROJECT + +This file is part of the QGROUNDCONTROL 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 . + +======================================================================*/ + +/** + * @file + * @brief UDP connection (server) for unmanned vehicles + * @author Lorenz Meier + * + */ + +#ifndef QGCFLIGHTGEARLINK_H +#define QGCFLIGHTGEARLINK_H + +#include +#include +#include +#include +#include +#include +#include +#include + +class QGCFlightGearLink : public QThread +{ + Q_OBJECT + //Q_INTERFACES(QGCFlightGearLinkInterface:LinkInterface) + +public: + QGCFlightGearLink(QString remoteHost=QString("127.0.0.1:49000"), QHostAddress host = QHostAddress::Any, quint16 port = 49005); + ~QGCFlightGearLink(); + + bool isConnected(); + qint64 bytesAvailable(); + int getPort() const { + return port; + } + + /** + * @brief The human readable port name + */ + QString getName(); + + void run(); + +public slots: +// void setAddress(QString address); + void setPort(int port); + /** @brief Add a new host to broadcast messages to */ + void setRemoteHost(const QString& host); + void updateGlobalPosition(quint64 time, double lat, double lon, double alt); + void sendUAVUpdate(); +// /** @brief Remove a host from broadcasting messages to */ +// void removeHost(const QString& host); + // void readPendingDatagrams(); + + void readBytes(); + /** + * @brief Write a number of bytes to the interface. + * + * @param data Pointer to the data byte array + * @param size The size of the bytes array + **/ + void writeBytes(const char* data, qint64 length); + bool connectSimulation(); + bool disconnectSimulation(); + +protected: + QString name; + QHostAddress host; + QHostAddress currentHost; + quint16 currentPort; + quint16 port; + int id; + QUdpSocket* socket; + bool connectState; + + quint64 bitsSentTotal; + quint64 bitsSentCurrent; + quint64 bitsSentMax; + quint64 bitsReceivedTotal; + quint64 bitsReceivedCurrent; + quint64 bitsReceivedMax; + quint64 connectionStartTime; + QMutex statisticsMutex; + QMutex dataMutex; + QTimer refreshTimer; + + void setName(QString name); + +signals: + /** + * @brief This signal is emitted instantly when the link is connected + **/ + void flightGearConnected(); + + /** + * @brief This signal is emitted instantly when the link is disconnected + **/ + void flightGearDisconnected(); + + /** + * @brief This signal is emitted instantly when the link status changes + **/ + void flightGearConnected(bool connected); + + +}; + +#endif // QGCFLIGHTGEARLINK_H diff --git a/src/comm/UDPLink.cc b/src/comm/UDPLink.cc index 5147379bd98092e9cd3ed7b42e66fe2d80f608d2..978b3946f743e2d6b83844b4ccde3159742534d3 100644 --- a/src/comm/UDPLink.cc +++ b/src/comm/UDPLink.cc @@ -86,7 +86,8 @@ void UDPLink::setPort(int port) void UDPLink::addHost(const QString& host) { //qDebug() << "UDP:" << "ADDING HOST:" << host; - if (host.contains(":")) { + if (host.contains(":")) + { //qDebug() << "HOST: " << host.split(":").first(); QHostInfo info = QHostInfo::fromName(host.split(":").first()); if (info.error() == QHostInfo::NoError) @@ -129,14 +130,18 @@ void UDPLink::removeHost(const QString& hostname) QHostInfo info = QHostInfo::fromName(host); QHostAddress address; QList hostAddresses = info.addresses(); - for (int i = 0; i < hostAddresses.size(); i++) { + for (int i = 0; i < hostAddresses.size(); i++) + { // Exclude loopback IPv4 and all IPv6 addresses - if (!hostAddresses.at(i).toString().contains(":")) { + if (!hostAddresses.at(i).toString().contains(":")) + { address = hostAddresses.at(i); } } - for (int i = 0; i < hosts.count(); ++i) { - if (hosts.at(i) == address) { + for (int i = 0; i < hosts.count(); ++i) + { + if (hosts.at(i) == address) + { hosts.removeAt(i); ports.removeAt(i); } @@ -154,7 +159,8 @@ void UDPLink::writeBytes(const char* data, qint64 size) #ifdef UDPLINK_DEBUG QString bytes; QString ascii; - for (int i=0; i 31 && data[i] < 127) @@ -183,7 +189,7 @@ void UDPLink::writeBytes(const char* data, qint64 size) void UDPLink::readBytes() { const qint64 maxLength = 65536; - char data[maxLength]; + static char data[maxLength]; QHostAddress sender; quint16 senderPort; @@ -207,11 +213,14 @@ void UDPLink::readBytes() // Add host to broadcast list if not yet present - if (!hosts.contains(sender)) { + if (!hosts.contains(sender)) + { hosts.append(sender); ports.append(senderPort); // ports->insert(sender, senderPort); - } else { + } + else + { int index = hosts.indexOf(sender); ports.replace(index, senderPort); } diff --git a/src/main.cc b/src/main.cc index e8ef93d55c9b73a58c53d5eae1ff7cfef238ffac..910c57e6bc2da580107bcba2197654989a3c815f 100644 --- a/src/main.cc +++ b/src/main.cc @@ -2,7 +2,7 @@ QGroundControl Open Source Ground Control Station -(c) 2009, 2010 QGROUNDCONTROL PROJECT +(c) 2009 - 2011 QGROUNDCONTROL PROJECT This file is part of the QGROUNDCONTROL project diff --git a/src/uas/PxQuadMAV.cc b/src/uas/PxQuadMAV.cc index 1ad0fbfe8001ef4d23d05b8946e4439d6e873dff..997541d921c9690712377e10a9b51d6cff6aafed 100644 --- a/src/uas/PxQuadMAV.cc +++ b/src/uas/PxQuadMAV.cc @@ -120,9 +120,48 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) 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); + //emit localPositionChanged(this, pos.x, pos.y, pos.z, time); } break; + case MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE: { + mavlink_vision_speed_estimate_t speed; + mavlink_msg_vision_speed_estimate_decode(&message, &speed); + quint64 time = getUnixTime(speed.usec); + emit valueChanged(uasId, "vis. speed x", "m/s", speed.x, time); + emit valueChanged(uasId, "vis. speed y", "m/s", speed.y, time); + emit valueChanged(uasId, "vis. speed z", "m/s", speed.z, time); + } + 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); + } + break; + case MAVLINK_MSG_ID_VISUAL_ODOMETRY: + { + mavlink_visual_odometry_t pos; + mavlink_msg_visual_odometry_decode(&message, &pos); + quint64 time = getUnixTime(pos.frame1_time_us); + //emit valueChanged(uasId, "vis. time", pos.usec, time); + emit valueChanged(uasId, "vis-o. roll", "rad", pos.roll, time); + emit valueChanged(uasId, "vis-o. pitch", "rad", pos.pitch, time); + emit valueChanged(uasId, "vis-o. yaw", "rad", pos.yaw, time); + emit valueChanged(uasId, "vis-o. x", "m", pos.x, time); + emit valueChanged(uasId, "vis-o. y", "m", pos.y, time); + emit valueChanged(uasId, "vis-o. z", "m", pos.z, time); + } + break; case MAVLINK_MSG_ID_AUX_STATUS: { mavlink_aux_status_t status; mavlink_msg_aux_status_decode(&message, &status); diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index e62fc127a0b282962946ad4d6a769c163062815f..984456a9a359db90e33ce1ebfd892b49c2bd2c70 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -74,7 +74,9 @@ statusTimeout(new QTimer(this)), paramsOnceRequested(false), airframe(0), attitudeKnown(false), -paramManager(NULL) +paramManager(NULL), +attitudeStamped(false), +lastAttitude(0) { color = UASInterface::getNextColor(); setBattery(LIPOLY, 3); @@ -156,12 +158,15 @@ bool UAS::getSelected() const void UAS::receiveMessageNamedValue(const mavlink_message_t& message) { - if (message.msgid == MAVLINK_MSG_ID_NAMED_VALUE_FLOAT) { + if (message.msgid == MAVLINK_MSG_ID_NAMED_VALUE_FLOAT) + { mavlink_named_value_float_t val; mavlink_msg_named_value_float_decode(&message, &val); 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) { + } + else if (message.msgid == MAVLINK_MSG_ID_NAMED_VALUE_INT) + { mavlink_named_value_int_t val; mavlink_msg_named_value_int_decode(&message, &val); QByteArray bytes(val.name, MAVLINK_MSG_NAMED_VALUE_INT_FIELD_NAME_LEN); @@ -172,7 +177,8 @@ void UAS::receiveMessageNamedValue(const mavlink_message_t& message) void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) { if (!link) return; - if (!links->contains(link)) { + if (!links->contains(link)) + { addLink(link); // qDebug() << __FILE__ << __LINE__ << "ADDED LINK!" << link->getName(); } @@ -183,19 +189,27 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) // qDebug() << "UAS RECEIVED from" << message.sysid << "component" << message.compid << "msg id" << message.msgid << "seq no" << message.seq; - if (message.sysid == uasId) { + // Only accept messages from this system (condition 1) + // and only then if a) attitudeStamped is disabled OR b) attitudeStamped is enabled + // and we already got one attitude packet + if (message.sysid == uasId && (!attitudeStamped || (attitudeStamped && (lastAttitude != 0)) || message.msgid == MAVLINK_MSG_ID_ATTITUDE)) + { QString uasState; QString stateDescription; - switch (message.msgid) { + switch (message.msgid) + { case MAVLINK_MSG_ID_HEARTBEAT: lastHeartbeat = QGC::groundTimeUsecs(); emit heartbeat(this); // Set new type if it has changed - if (this->type != mavlink_msg_heartbeat_get_type(&message)) { + if (this->type != mavlink_msg_heartbeat_get_type(&message)) + { this->type = mavlink_msg_heartbeat_get_type(&message); - if (airframe == 0) { - switch (type) { + if (airframe == 0) + { + switch (type) + { case MAV_FIXED_WING: setAirframe(UASInterface::QGC_AIRFRAME_EASYSTAR); break; @@ -222,7 +236,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) emit statusChanged(this, uasState, stateDescription); onboardTimeOffset = 0; // Reset offset measurement break; - case MAVLINK_MSG_ID_SYS_STATUS: { + case MAVLINK_MSG_ID_SYS_STATUS: + { mavlink_sys_status_t state; mavlink_msg_sys_status_decode(&message, &state); @@ -235,7 +250,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) bool statechanged = false; bool modechanged = false; - if (state.status != this->status) { + if (state.status != this->status) + { statechanged = true; this->status = state.status; getStatusForCode((int)state.status, uasState, stateDescription); @@ -247,15 +263,17 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) stateAudio = " changed status to " + uasState; } - if (navMode != state.nav_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()); + //emit valueChanged(uasId, "Load", "%", ((float)state.load)/10.0f, getUnixTime()); - if (this->mode != static_cast(state.mode)) { + if (this->mode != static_cast(state.mode)) + { modechanged = true; this->mode = static_cast(state.mode); QString mode; @@ -325,7 +343,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) lpVoltage = filterVoltage(currentVoltage); if (startVoltage == 0) startVoltage = currentVoltage; timeRemaining = calculateTimeRemaining(); - if (!batteryRemainingEstimateEnabled) { + if (!batteryRemainingEstimateEnabled) + { chargeLevel = state.battery_remaining/10.0f; } //qDebug() << "Voltage: " << currentVoltage << " Chargelevel: " << getChargeLevel() << " Time remaining " << timeRemaining; @@ -333,9 +352,12 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) emit voltageChanged(message.sysid, state.vbat/1000.0f); // LOW BATTERY ALARM - if (lpVoltage < warnVoltage) { + if (lpVoltage < warnVoltage) + { startLowBattAlarm(); - } else { + } + else + { stopLowBattAlarm(); } @@ -354,45 +376,35 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) //qDebug() << __FILE__ << __LINE__ << "RCV LOSS: " << state.packet_drop; // AUDIO - if (modechanged && statechanged) { + if (modechanged && statechanged) + { // Output both messages audiostring += modeAudio + " and " + stateAudio; - } else { + } + else + { // Output the one message audiostring += modeAudio + stateAudio; } - if ((int)state.status == (int)MAV_STATE_CRITICAL || state.status == (int)MAV_STATE_EMERGENCY) { + if ((int)state.status == (int)MAV_STATE_CRITICAL || state.status == (int)MAV_STATE_EMERGENCY) + { GAudioOutput::instance()->startEmergency(); - } else if (modechanged || statechanged) { + } + else if (modechanged || statechanged) + { GAudioOutput::instance()->stopEmergency(); GAudioOutput::instance()->say(audiostring); } - if (state.status == MAV_STATE_POWEROFF) { + if (state.status == MAV_STATE_POWEROFF) + { emit systemRemoved(this); emit systemRemoved(); } } 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: { + case MAVLINK_MSG_ID_RAW_IMU: + { mavlink_raw_imu_t raw; mavlink_msg_raw_imu_decode(&message, &raw); quint64 time = getUnixTime(raw.usec); @@ -408,7 +420,8 @@ 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: { + case MAVLINK_MSG_ID_SCALED_IMU: + { mavlink_scaled_imu_t scaled; mavlink_msg_scaled_imu_decode(&message, &scaled); quint64 time = getUnixTime(scaled.usec); @@ -419,18 +432,17 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) 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); + emit valueChanged(uasId, "mag x", "uTesla", scaled.xmag/100.0f, time); + emit valueChanged(uasId, "mag y", "uTesla", scaled.ymag/100.0f, time); + emit valueChanged(uasId, "mag z", "uTesla", scaled.zmag/100.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; { mavlink_attitude_t attitude; mavlink_msg_attitude_decode(&message, &attitude); - quint64 time = getUnixTime(attitude.usec); + quint64 time = getUnixReferenceTime(attitude.usec); + lastAttitude = time; roll = QGC::limitAngleToPMPIf(attitude.roll); pitch = QGC::limitAngleToPMPIf(attitude.pitch); yaw = QGC::limitAngleToPMPIf(attitude.yaw); @@ -463,7 +475,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) emit attitudeSpeedChanged(uasId, attitude.rollspeed, attitude.pitchspeed, attitude.yawspeed, time); } break; - case MAVLINK_MSG_ID_VFR_HUD: { + case MAVLINK_MSG_ID_VFR_HUD: + { mavlink_vfr_hud_t hud; mavlink_msg_vfr_hud_decode(&message, &hud); quint64 time = getUnixTime(); @@ -476,18 +489,19 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) emit valueChanged(uasId, "throttle", "%", hud.throttle, time); emit thrustChanged(this, hud.throttle/100.0); - if (!attitudeKnown) { + 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()); + emit speedChanged(this, hud.airspeed, 0.0f, hud.climb, time); } break; - case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT: { + case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT: + { mavlink_nav_controller_output_t nav; mavlink_msg_nav_controller_output_decode(&message, &nav); quint64 time = getUnixTime(); @@ -563,7 +577,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) forwardMessage(message); } break; - case MAVLINK_MSG_ID_GLOBAL_POSITION: { + case MAVLINK_MSG_ID_GLOBAL_POSITION: + { mavlink_global_position_t pos; mavlink_msg_global_position_decode(&message, &pos); quint64 time = getUnixTime(); @@ -630,7 +645,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) } } break; - case MAVLINK_MSG_ID_GPS_RAW_INT: { + case MAVLINK_MSG_ID_GPS_RAW_INT: + { mavlink_gps_raw_int_t pos; mavlink_msg_gps_raw_int_decode(&message, &pos); @@ -668,21 +684,25 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) } } break; - case MAVLINK_MSG_ID_GPS_STATUS: { + case MAVLINK_MSG_ID_GPS_STATUS: + { mavlink_gps_status_t pos; mavlink_msg_gps_status_decode(&message, &pos); - for(int i = 0; i < (int)pos.satellites_visible; i++) { + for(int i = 0; i < (int)pos.satellites_visible; i++) + { emit gpsSatelliteStatusChanged(uasId, (unsigned char)pos.satellite_prn[i], (unsigned char)pos.satellite_elevation[i], (unsigned char)pos.satellite_azimuth[i], (unsigned char)pos.satellite_snr[i], static_cast(pos.satellite_used[i])); } } break; - case MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET: { + case MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET: + { mavlink_gps_local_origin_set_t pos; mavlink_msg_gps_local_origin_set_decode(&message, &pos); emit homePositionChanged(uasId, pos.latitude, pos.longitude, pos.altitude); } break; - case MAVLINK_MSG_ID_RAW_PRESSURE: { + case MAVLINK_MSG_ID_RAW_PRESSURE: + { mavlink_raw_pressure_t pressure; mavlink_msg_raw_pressure_decode(&message, &pressure); quint64 time = this->getUnixTime(pressure.usec); @@ -693,7 +713,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) } break; - case MAVLINK_MSG_ID_SCALED_PRESSURE: { + case MAVLINK_MSG_ID_SCALED_PRESSURE: + { mavlink_scaled_pressure_t pressure; mavlink_msg_scaled_pressure_decode(&message, &pressure); quint64 time = this->getUnixTime(pressure.usec); @@ -703,7 +724,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) } break; - case MAVLINK_MSG_ID_RC_CHANNELS_RAW: { + case MAVLINK_MSG_ID_RC_CHANNELS_RAW: + { mavlink_rc_channels_raw_t channels; mavlink_msg_rc_channels_raw_decode(&message, &channels); emit remoteControlRSSIChanged(channels.rssi/255.0f); @@ -715,9 +737,19 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) emit remoteControlChannelRawChanged(5, channels.chan6_raw); emit remoteControlChannelRawChanged(6, channels.chan7_raw); emit remoteControlChannelRawChanged(7, channels.chan8_raw); + quint64 time = getUnixTime(); + emit valueChanged(uasId, "rc in #1", "us", channels.chan1_raw, time); + emit valueChanged(uasId, "rc in #2", "us", channels.chan2_raw, time); + emit valueChanged(uasId, "rc in #3", "us", channels.chan3_raw, time); + emit valueChanged(uasId, "rc in #4", "us", channels.chan4_raw, time); + emit valueChanged(uasId, "rc in #5", "us", channels.chan5_raw, time); + emit valueChanged(uasId, "rc in #6", "us", channels.chan6_raw, time); + emit valueChanged(uasId, "rc in #7", "us", channels.chan7_raw, time); + emit valueChanged(uasId, "rc in #8", "us", channels.chan8_raw, time); } break; - case MAVLINK_MSG_ID_RC_CHANNELS_SCALED: { + case MAVLINK_MSG_ID_RC_CHANNELS_SCALED: + { mavlink_rc_channels_scaled_t channels; mavlink_msg_rc_channels_scaled_decode(&message, &channels); emit remoteControlRSSIChanged(channels.rssi/255.0f); @@ -731,7 +763,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) emit remoteControlChannelScaledChanged(7, channels.chan8_scaled/10000.0f); } break; - case MAVLINK_MSG_ID_PARAM_VALUE: { + case MAVLINK_MSG_ID_PARAM_VALUE: + { mavlink_param_value_t value; mavlink_msg_param_value_decode(&message, &value); QByteArray bytes((char*)value.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); @@ -740,7 +773,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) float val = value.param_value; // Insert component if necessary - if (!parameters.contains(component)) { + if (!parameters.contains(component)) + { parameters.insert(component, new QMap()); } @@ -756,73 +790,77 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) case MAVLINK_MSG_ID_ACTION_ACK: mavlink_action_ack_t ack; mavlink_msg_action_ack_decode(&message, &ack); - if (ack.result == 1) { + if (ack.result == 1) + { emit textMessageReceived(uasId, message.compid, 0, tr("SUCCESS: Executed action: %1").arg(ack.action)); - } else { + } + else + { emit textMessageReceived(uasId, message.compid, 0, tr("FAILURE: Rejected action: %1").arg(ack.action)); } break; case MAVLINK_MSG_ID_DEBUG: emit valueChanged(uasId, QString("debug ") + QString::number(mavlink_msg_debug_get_ind(&message)), "raw", mavlink_msg_debug_get_value(&message), MG::TIME::getGroundTimeNow()); break; - case MAVLINK_MSG_ID_ATTITUDE_CONTROLLER_OUTPUT: { - mavlink_attitude_controller_output_t out; - mavlink_msg_attitude_controller_output_decode(&message, &out); - quint64 time = MG::TIME::getGroundTimeNowUsecs(); - emit attitudeThrustSetPointChanged(this, out.roll/127.0f, out.pitch/127.0f, out.yaw/127.0f, (uint8_t)out.thrust, time); - emit valueChanged(uasId, "att control roll", "raw", out.roll, time/1000.0f); - emit valueChanged(uasId, "att control pitch", "raw", out.pitch, time/1000.0f); - emit valueChanged(uasId, "att control yaw", "raw", out.yaw, time/1000.0f); - } - break; - case MAVLINK_MSG_ID_POSITION_CONTROLLER_OUTPUT: { - mavlink_position_controller_output_t out; - mavlink_msg_position_controller_output_decode(&message, &out); - quint64 time = MG::TIME::getGroundTimeNow(); - //emit positionSetPointsChanged(uasId, out.x/127.0f, out.y/127.0f, out.z/127.0f, out.yaw, time); - emit valueChanged(uasId, "pos control x", "raw", out.x, time); - emit valueChanged(uasId, "pos control y", "raw", out.y, time); - emit valueChanged(uasId, "pos control z", "raw", out.z, time); + case MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT: + { + mavlink_roll_pitch_yaw_thrust_setpoint_t out; + mavlink_msg_roll_pitch_yaw_thrust_setpoint_decode(&message, &out); + quint64 time = getUnixTime(out.time_us); + emit attitudeThrustSetPointChanged(this, out.roll, out.pitch, out.yaw, out.thrust, time); + emit valueChanged(uasId, "att control roll", "rad", out.roll, time); + emit valueChanged(uasId, "att control pitch", "rad", out.pitch, time); + emit valueChanged(uasId, "att control yaw", "rad", out.yaw, time); + emit valueChanged(uasId, "att control thrust", "0-1", out.thrust, time); } break; - case MAVLINK_MSG_ID_WAYPOINT_COUNT: { + case MAVLINK_MSG_ID_WAYPOINT_COUNT: + { mavlink_waypoint_count_t wpc; mavlink_msg_waypoint_count_decode(&message, &wpc); - if (wpc.target_system == mavlink->getSystemId() && wpc.target_component == mavlink->getComponentId()) { + if (wpc.target_system == mavlink->getSystemId() && wpc.target_component == mavlink->getComponentId()) + { waypointManager.handleWaypointCount(message.sysid, message.compid, wpc.count); } } break; - case MAVLINK_MSG_ID_WAYPOINT: { + case MAVLINK_MSG_ID_WAYPOINT: + { mavlink_waypoint_t wp; mavlink_msg_waypoint_decode(&message, &wp); //qDebug() << "got waypoint (" << wp.seq << ") from ID " << message.sysid << " x=" << wp.x << " y=" << wp.y << " z=" << wp.z; - if(wp.target_system == mavlink->getSystemId() && wp.target_component == mavlink->getComponentId()) { + if(wp.target_system == mavlink->getSystemId() && wp.target_component == mavlink->getComponentId()) + { waypointManager.handleWaypoint(message.sysid, message.compid, &wp); } } break; - case MAVLINK_MSG_ID_WAYPOINT_ACK: { + case MAVLINK_MSG_ID_WAYPOINT_ACK: + { mavlink_waypoint_ack_t wpa; mavlink_msg_waypoint_ack_decode(&message, &wpa); - if(wpa.target_system == mavlink->getSystemId() && wpa.target_component == mavlink->getComponentId()) { + if(wpa.target_system == mavlink->getSystemId() && wpa.target_component == mavlink->getComponentId()) + { waypointManager.handleWaypointAck(message.sysid, message.compid, &wpa); } } break; - case MAVLINK_MSG_ID_WAYPOINT_REQUEST: { + case MAVLINK_MSG_ID_WAYPOINT_REQUEST: + { mavlink_waypoint_request_t wpr; mavlink_msg_waypoint_request_decode(&message, &wpr); - if(wpr.target_system == mavlink->getSystemId() && wpr.target_component == mavlink->getComponentId()) { + if(wpr.target_system == mavlink->getSystemId() && wpr.target_component == mavlink->getComponentId()) + { waypointManager.handleWaypointRequest(message.sysid, message.compid, &wpr); } } break; - case MAVLINK_MSG_ID_WAYPOINT_REACHED: { + case MAVLINK_MSG_ID_WAYPOINT_REACHED: + { mavlink_waypoint_reached_t wpr; mavlink_msg_waypoint_reached_decode(&message, &wpr); waypointManager.handleWaypointReached(message.sysid, message.compid, &wpr); @@ -832,20 +870,23 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) } break; - case MAVLINK_MSG_ID_WAYPOINT_CURRENT: { + case MAVLINK_MSG_ID_WAYPOINT_CURRENT: + { mavlink_waypoint_current_t wpc; mavlink_msg_waypoint_current_decode(&message, &wpc); waypointManager.handleWaypointCurrent(message.sysid, message.compid, &wpc); } break; - case MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT: { + case MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT: + { mavlink_local_position_setpoint_t p; mavlink_msg_local_position_setpoint_decode(&message, &p); emit positionSetPointsChanged(uasId, p.x, p.y, p.z, p.yaw, QGC::groundTimeUsecs()); } break; - case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW: { + case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW: + { mavlink_servo_output_raw_t servos; mavlink_msg_servo_output_raw_decode(&message, &servos); quint64 time = getUnixTime(); @@ -859,7 +900,21 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) emit valueChanged(uasId, "servo #8", "us", servos.servo8_raw, time); } break; - case MAVLINK_MSG_ID_STATUSTEXT: { + + case MAVLINK_MSG_ID_OPTICAL_FLOW: + { + mavlink_optical_flow_t flow; + mavlink_msg_optical_flow_decode(&message, &flow); + quint64 time = getUnixTime(flow.time); + + emit valueChanged(uasId, QString("opt_flow_%1.x").arg(flow.sensor_id), "Pixel", flow.flow_x, time); + emit valueChanged(uasId, QString("opt_flow_%1.y").arg(flow.sensor_id), "Pixel", flow.flow_y, time); + emit valueChanged(uasId, QString("opt_flow_%1.qual").arg(flow.sensor_id), "0-255", flow.quality, time); + emit valueChanged(uasId, QString("opt_flow_%1.dist").arg(flow.sensor_id), "m", flow.ground_distance, time); + } + break; + case MAVLINK_MSG_ID_STATUSTEXT: + { QByteArray b; b.resize(MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN); mavlink_msg_statustext_get_text(&message, (int8_t*)b.data()); @@ -872,7 +927,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) } break; #ifdef MAVLINK_ENABLED_PIXHAWK - case MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE: { + 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); @@ -885,7 +941,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) } break; - case MAVLINK_MSG_ID_ENCAPSULATED_DATA: { + case MAVLINK_MSG_ID_ENCAPSULATED_DATA: + { mavlink_encapsulated_data_t img; mavlink_msg_encapsulated_data_decode(&message, &img); int seq = img.seqnr; @@ -899,7 +956,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) imagePacketsArrived = 0; } - for (int i = 0; i < imagePayload; ++i) { + for (int i = 0; i < imagePayload; ++i) + { if (pos <= imageSize) { imageRecBuffer[pos] = img.data[i]; } @@ -919,34 +977,77 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) } break; #endif - case MAVLINK_MSG_ID_DEBUG_VECT: { - mavlink_debug_vect_t vect; - mavlink_msg_debug_vect_decode(&message, &vect); - QString str((const char*)vect.name); - quint64 time = getUnixTime(vect.usec); - emit valueChanged(uasId, str+".x", "raw", vect.x, time); - emit valueChanged(uasId, str+".y", "raw", vect.y, time); - emit valueChanged(uasId, str+".z", "raw", vect.z, time); - } - break; - //#ifdef MAVLINK_ENABLED_PIXHAWK - // case MAVLINK_MSG_ID_POINT_OF_INTEREST: - // { - // mavlink_point_of_interest_t poi; - // mavlink_msg_point_of_interest_decode(&message, &poi); - // emit poiFound(this, poi.type, poi.color, QString((QChar*)poi.name, MAVLINK_MSG_POINT_OF_INTEREST_FIELD_NAME_LEN), poi.x, poi.y, poi.z); - // } - // break; - // case MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION: - // { - // mavlink_point_of_interest_connection_t poi; - // mavlink_msg_point_of_interest_connection_decode(&message, &poi); - // emit poiConnectionFound(this, poi.type, poi.color, QString((QChar*)poi.name, MAVLINK_MSG_POINT_OF_INTEREST_CONNECTION_FIELD_NAME_LEN), poi.x1, poi.y1, poi.z1, poi.x2, poi.y2, poi.z2); - // } - // break; - //#endif + case MAVLINK_MSG_ID_DEBUG_VECT: + { + mavlink_debug_vect_t vect; + mavlink_msg_debug_vect_decode(&message, &vect); + QString str((const char*)vect.name); + quint64 time = getUnixTime(vect.usec); + emit valueChanged(uasId, str+".x", "raw", vect.x, time); + emit valueChanged(uasId, str+".y", "raw", vect.y, time); + emit valueChanged(uasId, str+".z", "raw", vect.z, time); + } + break; + case MAVLINK_MSG_ID_OBJECT_DETECTION_EVENT: + { + mavlink_object_detection_event_t event; + mavlink_msg_object_detection_event_decode(&message, &event); + QString str(event.name); + emit objectDetected(event.time, event.object_id, event.type, str, event.quality, event.bearing, event.distance); + } + break; + // WILL BE ENABLED ONCE MESSAGE IS IN COMMON MESSAGE SET +// case MAVLINK_MSG_ID_MEMORY_VECT: +// { +// mavlink_memory_vect_t vect; +// mavlink_msg_memory_vect_decode(&message, &vect); +// QString str("mem_%1"); +// quint64 time = getUnixTime(0); +// int16_t *mem0 = (int16_t *)&vect.value[0]; +// uint16_t *mem1 = (uint16_t *)&vect.value[0]; +// int32_t *mem2 = (int32_t *)&vect.value[0]; +// // uint32_t *mem3 = (uint32_t *)&vect.value[0]; causes overload problem +// float *mem4 = (float *)&vect.value[0]; +// if ( vect.ver == 0) vect.type = 0, vect.ver = 1; else ; +// if ( vect.ver == 1) +// { +// switch (vect.type) { +// default: +// case 0: +// for (int i = 0; i < 16; i++) +// emit valueChanged(uasId, str.arg(vect.address+(i*2)), "i16", mem0[i], time); +// break; +// case 1: +// for (int i = 0; i < 16; i++) +// emit valueChanged(uasId, str.arg(vect.address+(i*2)), "ui16", mem1[i], time); +// break; +// case 2: +// for (int i = 0; i < 16; i++) +// emit valueChanged(uasId, str.arg(vect.address+(i*2)), "Q15", (float)mem0[i]/32767.0, time); +// break; +// case 3: +// for (int i = 0; i < 16; i++) +// emit valueChanged(uasId, str.arg(vect.address+(i*2)), "1Q14", (float)mem0[i]/16383.0, time); +// break; +// case 4: +// for (int i = 0; i < 8; i++) +// emit valueChanged(uasId, str.arg(vect.address+(i*4)), "i32", mem2[i], time); +// break; +// case 5: +// for (int i = 0; i < 8; i++) +// emit valueChanged(uasId, str.arg(vect.address+(i*4)), "i32", mem2[i], time); +// break; +// case 6: +// for (int i = 0; i < 8; i++) +// emit valueChanged(uasId, str.arg(vect.address+(i*4)), "float", mem4[i], time); +// break; +// } +// } +// } +// break; #ifdef MAVLINK_ENABLED_UALBERTA - case MAVLINK_MSG_ID_NAV_FILTER_BIAS: { + case MAVLINK_MSG_ID_NAV_FILTER_BIAS: + { mavlink_nav_filter_bias_t bias; mavlink_msg_nav_filter_bias_decode(&message, &bias); quint64 time = getUnixTime(); @@ -958,7 +1059,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) emit valueChanged(uasId, "b_w[2]", "raw", bias.gyro_2, time); } break; - case MAVLINK_MSG_ID_RADIO_CALIBRATION: { + case MAVLINK_MSG_ID_RADIO_CALIBRATION: + { mavlink_radio_calibration_t radioMsg; mavlink_msg_radio_calibration_decode(&message, &radioMsg); QVector aileron; @@ -991,8 +1093,10 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) // Messages to ignore case MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET: break; - default: { - if (!unknownPackets.contains(message.msgid)) { + default: + { + if (!unknownPackets.contains(message.msgid)) + { unknownPackets.append(message.msgid); QString errString = tr("UNABLE TO DECODE MESSAGE NUMBER %1").arg(message.msgid); GAudioOutput::instance()->say(errString+tr(", please check console for details.")); @@ -1036,7 +1140,8 @@ void UAS::setLocalOriginAtCurrentGPSPosition() QTimer::singleShot(5000, &msgBox, SLOT(reject())); - if (ret == QMessageBox::Yes) { + if (ret == QMessageBox::Yes) + { mavlink_message_t msg; mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), 0, MAV_ACTION_SET_ORIGIN); // Send message twice to increase chance that it reaches its goal @@ -1126,9 +1231,11 @@ void UAS::startPressureCalibration() sendMessage(msg); } -quint64 UAS::getUnixTime(quint64 time) +quint64 UAS::getUnixReferenceTime(quint64 time) { - if (time == 0) { + // Same as getUnixTime, but does not react to attitudeStamped mode + if (time == 0) + { // qDebug() << "XNEW time:" < has to be // a Unix epoch timestamp. Do nothing. return time/1000; } } +/** + * @warning If attitudeStamped is enabled, this function will not actually return the precise time stamp + * of this measurement augmented to UNIX time, but will MOVE the timestamp IN TIME to match + * the last measured attitude. There is no reason why one would want this, except for + * system setups where the onboard clock is not present or broken and datasets should + * be collected that are still roughly synchronized. PLEASE NOTE THAT ENABLING ATTITUDE STAMPED + * RUINS THE SCIENTIFIC NATURE OF THE CORRECT LOGGING FUNCTIONS OF QGROUNDCONTROL! + */ +quint64 UAS::getUnixTime(quint64 time) +{ + quint64 ret = 0; + if (attitudeStamped) + { + ret = lastAttitude; + } + if (time == 0) + { + ret = QGC::groundTimeMilliseconds(); + } + // Check if time is smaller than 40 years, + // assuming no system without Unix timestamp + // runs longer than 40 years continuously without + // reboot. In worst case this will add/subtract the + // communication delay between GCS and MAV, + // it will never alter the timestamp in a safety + // critical way. + // + // Calculation: + // 40 years + // 365 days + // 24 hours + // 60 minutes + // 60 seconds + // 1000 milliseconds + // 1000 microseconds +#ifndef _MSC_VER + else if (time < 1261440000000000LLU) +#else + else if (time < 1261440000000000) +#endif + { + // qDebug() << "GEN time:" << time/1000 + onboardTimeOffset; + if (onboardTimeOffset == 0) + { + onboardTimeOffset = QGC::groundTimeMilliseconds() - time/1000; + } + ret = time/1000 + onboardTimeOffset; + } + else + { + // Time is not zero and larger than 40 years -> has to be + // a Unix epoch timestamp. Do nothing. + ret = time/1000; + } + return ret; +} + QList UAS::getParameterNames(int component) { - if (parameters.contains(component)) { + if (parameters.contains(component)) + { return parameters.value(component)->keys(); - } else { + } + else + { return QList(); } } @@ -1182,13 +1352,16 @@ QList UAS::getComponentIds() void UAS::setMode(int mode) { - if ((uint8_t)mode >= MAV_MODE_LOCKED && (uint8_t)mode <= MAV_MODE_RC_TRAINING) { + if ((uint8_t)mode >= MAV_MODE_LOCKED && (uint8_t)mode <= MAV_MODE_RC_TRAINING) + { //this->mode = mode; //no call assignament, update receive message from UAS mavlink_message_t msg; mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, (uint8_t)uasId, (uint8_t)mode); sendMessage(msg); qDebug() << "SENDING REQUEST TO SET MODE TO SYSTEM" << uasId << ", REQUEST TO SET MODE " << (uint8_t)mode; - } else { + } + else + { qDebug() << "uas Mode not assign: " << mode; } } @@ -1196,10 +1369,14 @@ void UAS::setMode(int mode) void UAS::sendMessage(mavlink_message_t message) { // Emit message on all links that are currently connected - foreach (LinkInterface* link, *links) { - if (link) { + foreach (LinkInterface* link, *links) + { + if (link) + { sendMessage(link, message); - } else { + } + else + { // Remove from list links->removeAt(links->indexOf(link)); } @@ -1211,13 +1388,17 @@ void UAS::forwardMessage(mavlink_message_t message) // Emit message on all links that are currently connected QListlink_list = LinkManager::instance()->getLinksForProtocol(mavlink); - foreach(LinkInterface* link, link_list) { - if (link) { + foreach(LinkInterface* link, link_list) + { + if (link) + { SerialLink* serial = dynamic_cast(link); - if(serial != 0) { - - for(int i=0; isize(); i++) { - if(serial != links->at(i)) { + if(serial != 0) + { + for(int i=0; isize(); i++) + { + if(serial != links->at(i)) + { qDebug()<<"Antenna tracking: Forwarding Over link: "<getName()<<" "<getSystemId(), mavlink->getComponentId(), link->getId(), message.len); // If link is connected - if (link->isConnected()) { + if (link->isConnected()) + { // Send the portion of the buffer now occupied by the message link->writeBytes((const char*)buffer, len); } @@ -1252,7 +1434,8 @@ float UAS::filterVoltage(float value) const QString UAS::getNavModeText(int mode) { - switch (mode) { + switch (mode) + { case MAV_NAV_GROUNDED: return QString("GROUNDED"); break; @@ -1287,7 +1470,8 @@ QString UAS::getNavModeText(int mode) void UAS::getStatusForCode(int statusCode, QString& uasState, QString& stateDescription) { - switch (statusCode) { + switch (statusCode) + { case MAV_STATE_UNINIT: uasState = tr("UNINIT"); stateDescription = tr("Unitialized, booting up."); @@ -1417,9 +1601,12 @@ void UAS::requestImage() **/ quint64 UAS::getUptime() const { - if(startTime == 0) { + if(startTime == 0) + { return 0; - } else { + } + else + { return MG::TIME::getGroundTimeNow() - startTime; } } @@ -1692,7 +1879,8 @@ void UAS::enableExtra3Transmission(int rate) */ void UAS::setParameter(const int component, const QString& id, const float value) { - if (!id.isNull()) { + if (!id.isNull()) + { mavlink_message_t msg; mavlink_param_set_t p; p.param_value = value; @@ -1700,9 +1888,11 @@ void UAS::setParameter(const int component, const QString& id, const float value p.target_component = (uint8_t)component; // Copy string into buffer, ensuring not to exceed the buffer size - for (unsigned int i = 0; i < sizeof(p.param_id); i++) { + for (unsigned int i = 0; i < sizeof(p.param_id); i++) + { // String characters - if ((int)i < id.length() && i < (sizeof(p.param_id) - 1)) { + if ((int)i < id.length() && i < (sizeof(p.param_id) - 1)) + { p.param_id[i] = id.toAscii()[i]; } // // Null termination at end of string or end of buffer @@ -1711,7 +1901,8 @@ void UAS::setParameter(const int component, const QString& id, const float value // p.param_id[i] = '\0'; // } // Zero fill - else { + else + { p.param_id[i] = 0; } } @@ -1736,8 +1927,10 @@ 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) { + if (airframe == 0) + { + switch (systemType) + { case MAV_FIXED_WING: airframe = QGC_AIRFRAME_EASYSTAR; break; @@ -1856,14 +2049,17 @@ void UAS::setManualControlCommands(double roll, double pitch, double yaw, double manualYawAngle = yaw * yawScaling; manualThrust = thrust * thrustScaling; - if(mode == (int)MAV_MODE_MANUAL) { + if(mode == (int)MAV_MODE_MANUAL) + { mavlink_message_t message; mavlink_msg_manual_control_pack(mavlink->getSystemId(), mavlink->getComponentId(), &message, this->uasId, (float)manualRollAngle, (float)manualPitchAngle, (float)manualYawAngle, (float)manualThrust, controlRollManual, controlPitchManual, controlYawManual, controlThrustManual); sendMessage(message); qDebug() << __FILE__ << __LINE__ << ": SENT MANUAL CONTROL MESSAGE: roll" << manualRollAngle << " pitch: " << manualPitchAngle << " yaw: " << manualYawAngle << " thrust: " << manualThrust; emit attitudeThrustSetPointChanged(this, roll, pitch, yaw, thrust, MG::TIME::getGroundTimeNow()); - } else { + } + else + { qDebug() << "JOYSTICK/MANUAL CONTROL: IGNORING COMMANDS: Set mode to MANUAL to send joystick commands first"; } } @@ -1875,7 +2071,8 @@ int UAS::getSystemType() void UAS::receiveButton(int buttonIndex) { - switch (buttonIndex) { + switch (buttonIndex) + { case 0: break; @@ -1953,14 +2150,12 @@ void UAS::clearWaypointList() void UAS::halt() { - 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, (int)MAV_ACTION_HALT); // Send message twice to increase chance of reception sendMessage(msg); sendMessage(msg); - } void UAS::go() @@ -1990,7 +2185,6 @@ void UAS::home() */ void UAS::emergencySTOP() { - 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, (int)MAV_ACTION_EMCY_LAND); @@ -2020,7 +2214,8 @@ bool UAS::emergencyKILL() QTimer::singleShot(5000, &msgBox, SLOT(reject())); - if (ret == QMessageBox::Yes) { + if (ret == QMessageBox::Yes) + { 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, (int)MAV_ACTION_EMCY_KILL); @@ -2034,26 +2229,22 @@ bool UAS::emergencyKILL() void UAS::startHil() { - 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,(int)MAV_ACTION_START_HILSIM); // Send message twice to increase chance of reception sendMessage(msg); sendMessage(msg); - } void UAS::stopHil() { - 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,(int)MAV_ACTION_STOP_HILSIM); // Send message twice to increase chance of reception sendMessage(msg); sendMessage(msg); - } @@ -2072,8 +2263,8 @@ void UAS::shutdown() // Close the message box shortly after the click to prevent accidental clicks QTimer::singleShot(5000, &msgBox, SLOT(reject())); - - if (ret == QMessageBox::Yes) { + if (ret == QMessageBox::Yes) + { // If the active UAS is set, execute command mavlink_message_t msg; // TODO Replace MG System ID with static function call and allow to change ID in GUI @@ -2102,9 +2293,12 @@ void UAS::setTargetPosition(float x, float y, float z, float yaw) QString UAS::getUASName(void) const { QString result; - if (name == "") { + if (name == "") + { result = tr("MAV ") + result.sprintf("%03d", getUASID()); - } else { + } + else + { result = name; } return result; @@ -2122,7 +2316,8 @@ const QString& UAS::getShortMode() const void UAS::addLink(LinkInterface* link) { - if (!links->contains(link)) { + if (!links->contains(link)) + { links->append(link); connect(link, SIGNAL(destroyed(QObject*)), this, SLOT(removeLink(QObject*))); } @@ -2131,7 +2326,8 @@ void UAS::addLink(LinkInterface* link) void UAS::removeLink(QObject* object) { LinkInterface* link = dynamic_cast(object); - if (link) { + if (link) + { links->removeAt(links->indexOf(link)); } } @@ -2151,7 +2347,8 @@ void UAS::setBattery(BatteryType type, int cells) { this->batteryType = type; this->cells = cells; - switch (batteryType) { + switch (batteryType) + { case NICD: break; case NIMH: @@ -2171,24 +2368,31 @@ void UAS::setBattery(BatteryType type, int cells) void UAS::setBatterySpecs(const QString& specs) { - if (specs.length() == 0 || specs.contains("%")) { + if (specs.length() == 0 || specs.contains("%")) + { batteryRemainingEstimateEnabled = false; bool ok; QString percent = specs; percent = percent.remove("%"); float temp = percent.toFloat(&ok); - if (ok) { + if (ok) + { warnLevelPercent = temp; - } else { + } + else + { emit textMessageReceived(0, 0, 0, "Could not set battery options, format is wrong"); } - } else { + } + else + { batteryRemainingEstimateEnabled = true; QString stringList = specs; stringList = stringList.remove("V"); stringList = stringList.remove("v"); QStringList parts = stringList.split(","); - if (parts.length() == 3) { + if (parts.length() == 3) + { float temp; bool ok; // Get the empty voltage @@ -2200,7 +2404,9 @@ void UAS::setBatterySpecs(const QString& specs) // Get the full voltage temp = parts.at(2).toFloat(&ok); if (ok) fullVoltage = temp; - } else { + } + else + { emit textMessageReceived(0, 0, 0, "Could not set battery options, format is wrong"); } } @@ -2208,9 +2414,12 @@ void UAS::setBatterySpecs(const QString& specs) QString UAS::getBatterySpecs() { - if (batteryRemainingEstimateEnabled) { + if (batteryRemainingEstimateEnabled) + { return QString("%1V,%2V,%3V").arg(emptyVoltage).arg(warnVoltage).arg(fullVoltage); - } else { + } + else + { return QString("%1%").arg(warnLevelPercent); } } @@ -2233,12 +2442,18 @@ int UAS::calculateTimeRemaining() */ float UAS::getChargeLevel() { - if (batteryRemainingEstimateEnabled) { - if (lpVoltage < emptyVoltage) { + if (batteryRemainingEstimateEnabled) + { + if (lpVoltage < emptyVoltage) + { chargeLevel = 0.0f; - } else if (lpVoltage > fullVoltage) { + } + else if (lpVoltage > fullVoltage) + { chargeLevel = 100.0f; - } else { + } + else + { chargeLevel = 100.0f * ((lpVoltage - emptyVoltage)/(fullVoltage - emptyVoltage)); } } @@ -2247,7 +2462,8 @@ float UAS::getChargeLevel() void UAS::startLowBattAlarm() { - if (!lowBattAlarm) { + if (!lowBattAlarm) + { GAudioOutput::instance()->alert(tr("SYSTEM %1 HAS LOW BATTERY").arg(getUASName())); QTimer::singleShot(2000, GAudioOutput::instance(), SLOT(startEmergency())); lowBattAlarm = true; @@ -2256,7 +2472,8 @@ void UAS::startLowBattAlarm() void UAS::stopLowBattAlarm() { - if (lowBattAlarm) { + if (lowBattAlarm) + { GAudioOutput::instance()->stopEmergency(); lowBattAlarm = false; } diff --git a/src/uas/UAS.h b/src/uas/UAS.h index d958cb66aca8d293a1cf84400b5e6815d50bdd93..fb83f832c7bfff8ea4e36b25719f10bf0cc8b71f 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -204,6 +204,8 @@ protected: //COMMENTS FOR TEST UNIT QGCUASParamManager* paramManager; ///< Parameter manager class QString shortStateText; ///< Short textual state description QString shortModeText; ///< Short textual mode description + bool attitudeStamped; ///< Should arriving data be timestamped with the last attitude? This helps with broken system time clocks on the MAV + quint64 lastAttitude; ///< Timestamp of last attitude measurement public: /** @brief Set the current battery type */ @@ -403,6 +405,8 @@ signals: protected: /** @brief Get the UNIX timestamp in milliseconds */ quint64 getUnixTime(quint64 time=0); + /** @brief Get the UNIX timestamp in milliseconds, ignore attitudeStamped mode */ + quint64 getUnixReferenceTime(quint64 time); protected slots: /** @brief Write settings to disk */ diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index 56ac5383fd5454002e0ef09fad35c2f09fef320b..0789d7c1f6aaaab360afb150539ab6c65193c783 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -472,6 +472,9 @@ signals: /** @brief Core specifications have changed */ void systemSpecsChanged(int uasId); + /** @brief Object detected */ + void objectDetected(unsigned int time, int id, int type, const QString& name, int quality, float bearing, float distance); + // HOME POSITION / ORIGIN CHANGES void homePositionChanged(int uas, double lat, double lon, double alt); diff --git a/src/ui/HDDisplay.ui b/src/ui/HDDisplay.ui index 2cc98282e09185540fb5d91e2b639cdf4cddb60c..72273ec74e97193dd5fd982364ff030f216609fb 100644 --- a/src/ui/HDDisplay.ui +++ b/src/ui/HDDisplay.ui @@ -14,6 +14,9 @@ Form + + 0 + diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc index 27f40b8498561284b41b4e838c9437ba589b4e92..924c66a0555ffaa1f64414a4ca27bb339c3e4bde 100644 --- a/src/ui/HSIDisplay.cc +++ b/src/ui/HSIDisplay.cc @@ -119,8 +119,8 @@ HSIDisplay::HSIDisplay(QWidget *parent) : // Add interaction elements QHBoxLayout* layout = new QHBoxLayout(this); - layout->setMargin(2); - layout->setSpacing(0); + layout->setMargin(0); + layout->setSpacing(12); QDoubleSpinBox* spinBox = new QDoubleSpinBox(this); spinBox->setMinimum(0.1); spinBox->setMaximum(9999); @@ -453,6 +453,23 @@ void HSIDisplay::updatePositionZControllerEnabled(bool enabled) zControlKnown = true; } +void HSIDisplay::updateObjectPosition(unsigned int time, int id, int type, const QString& name, int quality, float bearing, float distance) +{ + // FIXME add multi-object support + QPainter painter(this); + QColor color(Qt::yellow); + 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(cos(bearing)-sin(bearing)*distance, sin(bearing)+cos(bearing)*distance); + // Scale from metric to screen reference coordinates + QPointF p = metricBodyToRef(in); + drawCircle(p.x(), p.y(), radius, 0.4f, color, &painter); +} + QPointF HSIDisplay::metricWorldToBody(QPointF world) { // First translate to body-centered coordinates @@ -554,6 +571,7 @@ void HSIDisplay::setActiveUAS(UASInterface* uas) disconnect(this->uas, SIGNAL(visionLocalizationChanged(UASInterface*,int)), this, SLOT(updateVisionLocalization(UASInterface*,int))); disconnect(this->uas, SIGNAL(gpsLocalizationChanged(UASInterface*,int)), this, SLOT(updateGpsLocalization(UASInterface*,int))); disconnect(this->uas, SIGNAL(irUltraSoundLocalizationChanged(UASInterface*,int)), this, SLOT(updateInfraredUltrasoundLocalization(UASInterface*,int))); + disconnect(this->uas, SIGNAL(objectDetected(uint,int,int,QString,int,float,float)), this, SLOT(updateObjectPosition(uint,int,int,QString,int,float,float))); } connect(uas, SIGNAL(gpsSatelliteStatusChanged(int,int,float,float,float,bool)), this, SLOT(updateSatellite(int,int,float,float,float,bool))); @@ -573,6 +591,7 @@ void HSIDisplay::setActiveUAS(UASInterface* uas) connect(uas, SIGNAL(visionLocalizationChanged(UASInterface*,int)), this, SLOT(updateVisionLocalization(UASInterface*,int))); connect(uas, SIGNAL(gpsLocalizationChanged(UASInterface*,int)), this, SLOT(updateGpsLocalization(UASInterface*,int))); connect(uas, SIGNAL(irUltraSoundLocalizationChanged(UASInterface*,int)), this, SLOT(updateInfraredUltrasoundLocalization(UASInterface*,int))); + connect(uas, SIGNAL(objectDetected(uint,int,int,QString,int,float,float)), this, SLOT(updateObjectPosition(uint,int,int,QString,int,float,float))); this->uas = uas; diff --git a/src/ui/HSIDisplay.h b/src/ui/HSIDisplay.h index 57308609f5182354f2582b3c06f10be4478efa94..3a899926254ec2d615420a5121d8f7e3203a86f4 100644 --- a/src/ui/HSIDisplay.h +++ b/src/ui/HSIDisplay.h @@ -65,6 +65,7 @@ public slots: void updateAttitudeControllerEnabled(bool enabled); void updatePositionXYControllerEnabled(bool enabled); void updatePositionZControllerEnabled(bool enabled); + void updateObjectPosition(unsigned int time, int id, int type, const QString& name, int quality, float bearing, float distance); /** @brief Heading control enabled/disabled */ void updatePositionYawControllerEnabled(bool enabled); @@ -132,6 +133,11 @@ protected: QPointF metricBodyToRef(QPointF &metric); /** @brief Metric body coordinates to screen coordinates */ QPointF metricBodyToScreen(QPointF metric); + QMap objectNames; + QMap objectTypes; + QMap objectQualities; + QMap objectBearings; + QMap objectDistances; /** * @brief Private data container class to be used within the HSI widget diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index 8cb7844c591e1255b63a61b13c0ddda3b44a39a4..67fb579bcbc8a6cfb4b34d77db5c7ae85f8b4299 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -157,6 +157,11 @@ MainWindow::MainWindow(QWidget *parent): joystickWidget = 0; joystick = new JoystickInput(); + // Connect flighgear test link + // FIXME MOVE INTO UAV OBJECT + fgLink = new QGCFlightGearLink(); + fgLink->connectSimulation(); + // Load Toolbar toolBar = new QGCToolBar(this); this->addToolBar(toolBar); @@ -2117,4 +2122,4 @@ void MainWindow::arrangeSenseSoarCenterStack() void MainWindow::connectSenseSoarActions() { -} \ No newline at end of file +} diff --git a/src/ui/MainWindow.h b/src/ui/MainWindow.h index 9522d33112063530219a93e2d50c320b6fe93396..4c371bbd140ff2b574ebb3291728a20d0318bab8 100644 --- a/src/ui/MainWindow.h +++ b/src/ui/MainWindow.h @@ -76,6 +76,7 @@ This file is part of the QGROUNDCONTROL project #include "SlugsPadCameraControl.h" #include "UASControlParameters.h" +#include "QGCFlightGearLink.h" class QGCMapTool; @@ -453,6 +454,7 @@ protected: QGC_MAINWINDOW_STYLE currentStyle; Qt::WindowStates windowStateVal; bool lowPowerMode; ///< If enabled, QGC reduces the update rates of all widgets + QGCFlightGearLink* fgLink; private: Ui::MainWindow ui; diff --git a/src/ui/QGCToolBar.cc b/src/ui/QGCToolBar.cc index d07b089fac494e8a19d180fcc2b76336a9f88bef..e041c5b6eaaba0dd22955f8fd75fb6fc7b73189e 100644 --- a/src/ui/QGCToolBar.cc +++ b/src/ui/QGCToolBar.cc @@ -55,6 +55,7 @@ void QGCToolBar::setActiveUAS(UASInterface* active) disconnect(mav, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString))); disconnect(mav, SIGNAL(nameChanged(QString)), this, SLOT(updateName(QString))); disconnect(mav, SIGNAL(systemTypeSet(UASInterface*,uint)), this, SLOT(setSystemType(UASInterface*,uint))); + disconnect(mav, SIGNAL(textMessageReceived(int,int,int,QString)), this, SLOT(receiveTextMessage(int,int,int,QString))); } // Connect new system @@ -63,6 +64,7 @@ void QGCToolBar::setActiveUAS(UASInterface* active) connect(active, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString))); connect(active, SIGNAL(nameChanged(QString)), this, SLOT(updateName(QString))); connect(active, SIGNAL(systemTypeSet(UASInterface*,uint)), this, SLOT(setSystemType(UASInterface*,uint))); + connect(active, SIGNAL(textMessageReceived(int,int,int,QString)), this, SLOT(receiveTextMessage(int,int,int,QString))); // Update all values once nameLabel->setText(mav->getUASName()); @@ -81,6 +83,7 @@ void QGCToolBar::createCustomWidgets() stateLabel = new QLabel("------", this); wpLabel = new QLabel("---", this); distlabel = new QLabel("--- ---- m", this); + messageLabel = new QLabel("No system messages.", this); //symbolButton->setIcon(":"); symbolButton->setStyleSheet("QWidget { background-color: #050508; color: #DDDDDF; background-clip: border; } QToolButton { font-weight: bold; font-size: 12px; border: 0px solid #999999; border-radius: 5px; min-width:22px; max-width: 22px; min-height: 22px; max-height: 22px; padding: 0px; margin: 0px; background-color: none; }"); addWidget(symbolButton); @@ -89,6 +92,7 @@ void QGCToolBar::createCustomWidgets() addWidget(stateLabel); addWidget(wpLabel); addWidget(distlabel); + addWidget(messageLabel); toggleLoggingAction = new QAction(QIcon(":"), "Start Logging", this); logReplayAction = new QAction(QIcon(":"), "Start Replay", this); @@ -154,6 +158,14 @@ void QGCToolBar::setSystemType(UASInterface* uas, unsigned int systemType) } } +void QGCToolBar::receiveTextMessage(int uasid, int componentid, int severity, QString text) +{ + Q_UNUSED(uasid); + Q_UNUSED(componentid); + Q_UNUSED(severity); + messageLabel->setText(text); +} + QGCToolBar::~QGCToolBar() { delete toggleLoggingAction; diff --git a/src/ui/QGCToolBar.h b/src/ui/QGCToolBar.h index fc5648e4ca00a30e7b45eab945cc771616e6a0f2..cf054ca468102c3a2ce2166e7ca20ffa82ac2689 100644 --- a/src/ui/QGCToolBar.h +++ b/src/ui/QGCToolBar.h @@ -50,6 +50,8 @@ public slots: void updateName(const QString& name); /** @brief Set the MAV system type */ void setSystemType(UASInterface* uas, unsigned int systemType); + /** @brief Received system text message */ + void receiveTextMessage(int uasid, int componentid, int severity, QString text); protected: void createCustomWidgets(); @@ -63,7 +65,7 @@ protected: QLabel* stateLabel; QLabel* wpLabel; QLabel* distlabel; - + QLabel* messageLabel; }; #endif // QGCTOOLBAR_H diff --git a/src/ui/map/MAV2DIcon.cc b/src/ui/map/MAV2DIcon.cc index d36b2f91f7ab8ee3c8ab3e7ea55762b06069a602..cee22331d44c301ac8f8e26871abd12975515198 100644 --- a/src/ui/map/MAV2DIcon.cc +++ b/src/ui/map/MAV2DIcon.cc @@ -37,7 +37,6 @@ MAV2DIcon::MAV2DIcon(mapcontrol::MapGraphicItem* map, mapcontrol::OPMapWidget* p MAV2DIcon::~MAV2DIcon() { - //delete pic; } void MAV2DIcon::setSelectedUAS(bool selected) @@ -53,7 +52,6 @@ void MAV2DIcon::setSelectedUAS(bool selected) */ void MAV2DIcon::setYaw(float yaw) { - //// qDebug() << "MAV2Icon" << yaw; float diff = fabs(yaw - this->yaw); while (diff > (float)M_PI) { diff -= (float)M_PI; diff --git a/src/ui/uas/UASView.cc b/src/ui/uas/UASView.cc index 4d294e41249e1e9a1de6310bea30f9fd586777e0..46a823dc2fbe52fd66adf0d5240934afa439b4e4 100644 --- a/src/ui/uas/UASView.cc +++ b/src/ui/uas/UASView.cc @@ -602,6 +602,8 @@ void UASView::refresh() m_ui->heartbeatIcon->setStyleSheet(colorstyle.arg(warnColor.name())); QString style = QString("QGroupBox { border-radius: 12px; padding: 0px; margin: 0px; border: 2px solid %1; background-color: %2; }").arg(borderColor, warnColor.name()); m_ui->uasViewFrame->setStyleSheet(style); + + refreshTimer->setInterval(errorUpdateInterval); } iconIsRed = !iconIsRed; } else { @@ -609,10 +611,11 @@ void UASView::refresh() { // Fade heartbeat icon // Make color darker - heartbeatColor = heartbeatColor.darker(150); + heartbeatColor = heartbeatColor.darker(210); //m_ui->heartbeatIcon->setAutoFillBackground(true); m_ui->heartbeatIcon->setStyleSheet(colorstyle.arg(heartbeatColor.name())); + refreshTimer->setInterval(updateInterval); } } //setUpdatesEnabled(true); diff --git a/src/ui/uas/UASView.h b/src/ui/uas/UASView.h index c29edafbbdd1dd59784e51ce7654f89544063170..5e8323226a206c4710b38cd41ab63a746be79432 100644 --- a/src/ui/uas/UASView.h +++ b/src/ui/uas/UASView.h @@ -122,7 +122,8 @@ protected: QAction* selectAction; QAction* selectAirframeAction; QAction* setBatterySpecsAction; - static const int updateInterval = 700; + static const int updateInterval = 800; + static const int errorUpdateInterval = 200; bool lowPowerModeEnabled; ///< Low power mode reduces update rates diff --git a/thirdParty/mavlink/include/ardupilotmega/ardupilotmega.h b/thirdParty/mavlink/include/ardupilotmega/ardupilotmega.h index 4d13e0b543c9a021cd9811f4378bb84dfdc47d49..64827032d4512c8bc54eaebf643402449e77c8f6 100644 --- a/thirdParty/mavlink/include/ardupilotmega/ardupilotmega.h +++ b/thirdParty/mavlink/include/ardupilotmega/ardupilotmega.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://qgroundcontrol.org/mavlink/ - * Generated on Wednesday, July 27 2011, 14:17 UTC + * Generated on Saturday, August 13 2011, 08:44 UTC */ #ifndef ARDUPILOTMEGA_H #define ARDUPILOTMEGA_H @@ -33,12 +33,14 @@ extern "C" { // MESSAGE DEFINITIONS +#include "./mavlink_msg_sensor_offsets.h" +#include "./mavlink_msg_set_mag_offsets.h" // MESSAGE LENGTHS #undef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS { 3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 0, 0, 0, 0, 0, 5, 5, 26, 16, 36, 5, 6, 0, 0, 21, 0, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51 } +#define MAVLINK_MESSAGE_LENGTHS { 3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 18, 18, 20, 20, 0, 0, 0, 26, 16, 36, 5, 6, 56, 26, 21, 18, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51 } #ifdef __cplusplus } diff --git a/thirdParty/mavlink/include/ardupilotmega/mavlink.h b/thirdParty/mavlink/include/ardupilotmega/mavlink.h index 8ee2430d74ef6945bd88749e49e4bc4105c40747..a54a6df31be8cf0a3c3ab7dfd8e2d3ca7b14855c 100644 --- a/thirdParty/mavlink/include/ardupilotmega/mavlink.h +++ b/thirdParty/mavlink/include/ardupilotmega/mavlink.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Wednesday, July 27 2011, 14:17 UTC + * Generated on Saturday, August 13 2011, 08:44 UTC */ #ifndef MAVLINK_H #define MAVLINK_H diff --git a/thirdParty/mavlink/include/ardupilotmega/mavlink_msg_sensor_offsets.h b/thirdParty/mavlink/include/ardupilotmega/mavlink_msg_sensor_offsets.h new file mode 100644 index 0000000000000000000000000000000000000000..a3191c787073e5e7ff962713befe89a8b07bf7a2 --- /dev/null +++ b/thirdParty/mavlink/include/ardupilotmega/mavlink_msg_sensor_offsets.h @@ -0,0 +1,342 @@ +// MESSAGE SENSOR_OFFSETS PACKING + +#define MAVLINK_MSG_ID_SENSOR_OFFSETS 150 + +typedef struct __mavlink_sensor_offsets_t +{ + int16_t mag_ofs_x; ///< magnetometer X offset + int16_t mag_ofs_y; ///< magnetometer Y offset + int16_t mag_ofs_z; ///< magnetometer Z offset + float mag_declination; ///< magnetic declination (radians) + int32_t raw_press; ///< raw pressure from barometer + int32_t raw_temp; ///< raw temperature from barometer + float gyro_cal_x; ///< gyro X calibration + float gyro_cal_y; ///< gyro Y calibration + float gyro_cal_z; ///< gyro Z calibration + float accel_cal_x; ///< accel X calibration + float accel_cal_y; ///< accel Y calibration + float accel_cal_z; ///< accel Z calibration + +} mavlink_sensor_offsets_t; + + + +/** + * @brief Pack a sensor_offsets message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param mag_ofs_x magnetometer X offset + * @param mag_ofs_y magnetometer Y offset + * @param mag_ofs_z magnetometer Z offset + * @param mag_declination magnetic declination (radians) + * @param raw_press raw pressure from barometer + * @param raw_temp raw temperature from barometer + * @param gyro_cal_x gyro X calibration + * @param gyro_cal_y gyro Y calibration + * @param gyro_cal_z gyro Z calibration + * @param accel_cal_x accel X calibration + * @param accel_cal_y accel Y calibration + * @param accel_cal_z accel Z calibration + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_sensor_offsets_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, int16_t mag_ofs_x, int16_t mag_ofs_y, int16_t mag_ofs_z, float mag_declination, int32_t raw_press, int32_t raw_temp, float gyro_cal_x, float gyro_cal_y, float gyro_cal_z, float accel_cal_x, float accel_cal_y, float accel_cal_z) +{ + uint16_t i = 0; + msg->msgid = MAVLINK_MSG_ID_SENSOR_OFFSETS; + + i += put_int16_t_by_index(mag_ofs_x, i, msg->payload); // magnetometer X offset + i += put_int16_t_by_index(mag_ofs_y, i, msg->payload); // magnetometer Y offset + i += put_int16_t_by_index(mag_ofs_z, i, msg->payload); // magnetometer Z offset + i += put_float_by_index(mag_declination, i, msg->payload); // magnetic declination (radians) + i += put_int32_t_by_index(raw_press, i, msg->payload); // raw pressure from barometer + i += put_int32_t_by_index(raw_temp, i, msg->payload); // raw temperature from barometer + i += put_float_by_index(gyro_cal_x, i, msg->payload); // gyro X calibration + i += put_float_by_index(gyro_cal_y, i, msg->payload); // gyro Y calibration + i += put_float_by_index(gyro_cal_z, i, msg->payload); // gyro Z calibration + i += put_float_by_index(accel_cal_x, i, msg->payload); // accel X calibration + i += put_float_by_index(accel_cal_y, i, msg->payload); // accel Y calibration + i += put_float_by_index(accel_cal_z, i, msg->payload); // accel Z calibration + + return mavlink_finalize_message(msg, system_id, component_id, i); +} + +/** + * @brief Pack a sensor_offsets message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param mag_ofs_x magnetometer X offset + * @param mag_ofs_y magnetometer Y offset + * @param mag_ofs_z magnetometer Z offset + * @param mag_declination magnetic declination (radians) + * @param raw_press raw pressure from barometer + * @param raw_temp raw temperature from barometer + * @param gyro_cal_x gyro X calibration + * @param gyro_cal_y gyro Y calibration + * @param gyro_cal_z gyro Z calibration + * @param accel_cal_x accel X calibration + * @param accel_cal_y accel Y calibration + * @param accel_cal_z accel Z calibration + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_sensor_offsets_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, int16_t mag_ofs_x, int16_t mag_ofs_y, int16_t mag_ofs_z, float mag_declination, int32_t raw_press, int32_t raw_temp, float gyro_cal_x, float gyro_cal_y, float gyro_cal_z, float accel_cal_x, float accel_cal_y, float accel_cal_z) +{ + uint16_t i = 0; + msg->msgid = MAVLINK_MSG_ID_SENSOR_OFFSETS; + + i += put_int16_t_by_index(mag_ofs_x, i, msg->payload); // magnetometer X offset + i += put_int16_t_by_index(mag_ofs_y, i, msg->payload); // magnetometer Y offset + i += put_int16_t_by_index(mag_ofs_z, i, msg->payload); // magnetometer Z offset + i += put_float_by_index(mag_declination, i, msg->payload); // magnetic declination (radians) + i += put_int32_t_by_index(raw_press, i, msg->payload); // raw pressure from barometer + i += put_int32_t_by_index(raw_temp, i, msg->payload); // raw temperature from barometer + i += put_float_by_index(gyro_cal_x, i, msg->payload); // gyro X calibration + i += put_float_by_index(gyro_cal_y, i, msg->payload); // gyro Y calibration + i += put_float_by_index(gyro_cal_z, i, msg->payload); // gyro Z calibration + i += put_float_by_index(accel_cal_x, i, msg->payload); // accel X calibration + i += put_float_by_index(accel_cal_y, i, msg->payload); // accel Y calibration + i += put_float_by_index(accel_cal_z, i, msg->payload); // accel Z calibration + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); +} + +/** + * @brief Encode a sensor_offsets struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param sensor_offsets C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_sensor_offsets_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_sensor_offsets_t* sensor_offsets) +{ + return mavlink_msg_sensor_offsets_pack(system_id, component_id, msg, sensor_offsets->mag_ofs_x, sensor_offsets->mag_ofs_y, sensor_offsets->mag_ofs_z, sensor_offsets->mag_declination, sensor_offsets->raw_press, sensor_offsets->raw_temp, sensor_offsets->gyro_cal_x, sensor_offsets->gyro_cal_y, sensor_offsets->gyro_cal_z, sensor_offsets->accel_cal_x, sensor_offsets->accel_cal_y, sensor_offsets->accel_cal_z); +} + +/** + * @brief Send a sensor_offsets message + * @param chan MAVLink channel to send the message + * + * @param mag_ofs_x magnetometer X offset + * @param mag_ofs_y magnetometer Y offset + * @param mag_ofs_z magnetometer Z offset + * @param mag_declination magnetic declination (radians) + * @param raw_press raw pressure from barometer + * @param raw_temp raw temperature from barometer + * @param gyro_cal_x gyro X calibration + * @param gyro_cal_y gyro Y calibration + * @param gyro_cal_z gyro Z calibration + * @param accel_cal_x accel X calibration + * @param accel_cal_y accel Y calibration + * @param accel_cal_z accel Z calibration + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_sensor_offsets_send(mavlink_channel_t chan, int16_t mag_ofs_x, int16_t mag_ofs_y, int16_t mag_ofs_z, float mag_declination, int32_t raw_press, int32_t raw_temp, float gyro_cal_x, float gyro_cal_y, float gyro_cal_z, float accel_cal_x, float accel_cal_y, float accel_cal_z) +{ + mavlink_message_t msg; + mavlink_msg_sensor_offsets_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, mag_ofs_x, mag_ofs_y, mag_ofs_z, mag_declination, raw_press, raw_temp, gyro_cal_x, gyro_cal_y, gyro_cal_z, accel_cal_x, accel_cal_y, accel_cal_z); + mavlink_send_uart(chan, &msg); +} + +#endif +// MESSAGE SENSOR_OFFSETS UNPACKING + +/** + * @brief Get field mag_ofs_x from sensor_offsets message + * + * @return magnetometer X offset + */ +static inline int16_t mavlink_msg_sensor_offsets_get_mag_ofs_x(const mavlink_message_t* msg) +{ + generic_16bit r; + r.b[1] = (msg->payload)[0]; + r.b[0] = (msg->payload)[1]; + return (int16_t)r.s; +} + +/** + * @brief Get field mag_ofs_y from sensor_offsets message + * + * @return magnetometer Y offset + */ +static inline int16_t mavlink_msg_sensor_offsets_get_mag_ofs_y(const mavlink_message_t* msg) +{ + generic_16bit r; + r.b[1] = (msg->payload+sizeof(int16_t))[0]; + r.b[0] = (msg->payload+sizeof(int16_t))[1]; + return (int16_t)r.s; +} + +/** + * @brief Get field mag_ofs_z from sensor_offsets message + * + * @return magnetometer Z offset + */ +static inline int16_t mavlink_msg_sensor_offsets_get_mag_ofs_z(const mavlink_message_t* msg) +{ + generic_16bit r; + r.b[1] = (msg->payload+sizeof(int16_t)+sizeof(int16_t))[0]; + r.b[0] = (msg->payload+sizeof(int16_t)+sizeof(int16_t))[1]; + return (int16_t)r.s; +} + +/** + * @brief Get field mag_declination from sensor_offsets message + * + * @return magnetic declination (radians) + */ +static inline float mavlink_msg_sensor_offsets_get_mag_declination(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; + r.b[2] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; + r.b[1] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[2]; + r.b[0] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[3]; + return (float)r.f; +} + +/** + * @brief Get field raw_press from sensor_offsets message + * + * @return raw pressure from barometer + */ +static inline int32_t mavlink_msg_sensor_offsets_get_raw_press(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float))[3]; + return (int32_t)r.i; +} + +/** + * @brief Get field raw_temp from sensor_offsets message + * + * @return raw temperature from barometer + */ +static inline int32_t mavlink_msg_sensor_offsets_get_raw_temp(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t))[0]; + r.b[2] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t))[1]; + r.b[1] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t))[2]; + r.b[0] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t))[3]; + return (int32_t)r.i; +} + +/** + * @brief Get field gyro_cal_x from sensor_offsets message + * + * @return gyro X calibration + */ +static inline float mavlink_msg_sensor_offsets_get_gyro_cal_x(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t))[0]; + r.b[2] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t))[1]; + r.b[1] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t))[2]; + r.b[0] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t))[3]; + return (float)r.f; +} + +/** + * @brief Get field gyro_cal_y from sensor_offsets message + * + * @return gyro Y calibration + */ +static inline float mavlink_msg_sensor_offsets_get_gyro_cal_y(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float))[3]; + return (float)r.f; +} + +/** + * @brief Get field gyro_cal_z from sensor_offsets message + * + * @return gyro Z calibration + */ +static inline float mavlink_msg_sensor_offsets_get_gyro_cal_z(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float))[3]; + return (float)r.f; +} + +/** + * @brief Get field accel_cal_x from sensor_offsets message + * + * @return accel X calibration + */ +static inline float mavlink_msg_sensor_offsets_get_accel_cal_x(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; + return (float)r.f; +} + +/** + * @brief Get field accel_cal_y from sensor_offsets message + * + * @return accel Y calibration + */ +static inline float mavlink_msg_sensor_offsets_get_accel_cal_y(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; + return (float)r.f; +} + +/** + * @brief Get field accel_cal_z from sensor_offsets message + * + * @return accel Z calibration + */ +static inline float mavlink_msg_sensor_offsets_get_accel_cal_z(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; + return (float)r.f; +} + +/** + * @brief Decode a sensor_offsets message into a struct + * + * @param msg The message to decode + * @param sensor_offsets C-struct to decode the message contents into + */ +static inline void mavlink_msg_sensor_offsets_decode(const mavlink_message_t* msg, mavlink_sensor_offsets_t* sensor_offsets) +{ + sensor_offsets->mag_ofs_x = mavlink_msg_sensor_offsets_get_mag_ofs_x(msg); + sensor_offsets->mag_ofs_y = mavlink_msg_sensor_offsets_get_mag_ofs_y(msg); + sensor_offsets->mag_ofs_z = mavlink_msg_sensor_offsets_get_mag_ofs_z(msg); + sensor_offsets->mag_declination = mavlink_msg_sensor_offsets_get_mag_declination(msg); + sensor_offsets->raw_press = mavlink_msg_sensor_offsets_get_raw_press(msg); + sensor_offsets->raw_temp = mavlink_msg_sensor_offsets_get_raw_temp(msg); + sensor_offsets->gyro_cal_x = mavlink_msg_sensor_offsets_get_gyro_cal_x(msg); + sensor_offsets->gyro_cal_y = mavlink_msg_sensor_offsets_get_gyro_cal_y(msg); + sensor_offsets->gyro_cal_z = mavlink_msg_sensor_offsets_get_gyro_cal_z(msg); + sensor_offsets->accel_cal_x = mavlink_msg_sensor_offsets_get_accel_cal_x(msg); + sensor_offsets->accel_cal_y = mavlink_msg_sensor_offsets_get_accel_cal_y(msg); + sensor_offsets->accel_cal_z = mavlink_msg_sensor_offsets_get_accel_cal_z(msg); +} diff --git a/thirdParty/mavlink/include/ardupilotmega/mavlink_msg_set_mag_offsets.h b/thirdParty/mavlink/include/ardupilotmega/mavlink_msg_set_mag_offsets.h new file mode 100644 index 0000000000000000000000000000000000000000..10dd1a58fd57d9158e18b3949b9d54303081f745 --- /dev/null +++ b/thirdParty/mavlink/include/ardupilotmega/mavlink_msg_set_mag_offsets.h @@ -0,0 +1,178 @@ +// MESSAGE SET_MAG_OFFSETS PACKING + +#define MAVLINK_MSG_ID_SET_MAG_OFFSETS 151 + +typedef struct __mavlink_set_mag_offsets_t +{ + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + int16_t mag_ofs_x; ///< magnetometer X offset + int16_t mag_ofs_y; ///< magnetometer Y offset + int16_t mag_ofs_z; ///< magnetometer Z offset + +} mavlink_set_mag_offsets_t; + + + +/** + * @brief Pack a set_mag_offsets message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param mag_ofs_x magnetometer X offset + * @param mag_ofs_y magnetometer Y offset + * @param mag_ofs_z magnetometer Z offset + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_mag_offsets_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, int16_t mag_ofs_x, int16_t mag_ofs_y, int16_t mag_ofs_z) +{ + uint16_t i = 0; + msg->msgid = MAVLINK_MSG_ID_SET_MAG_OFFSETS; + + i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID + i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID + i += put_int16_t_by_index(mag_ofs_x, i, msg->payload); // magnetometer X offset + i += put_int16_t_by_index(mag_ofs_y, i, msg->payload); // magnetometer Y offset + i += put_int16_t_by_index(mag_ofs_z, i, msg->payload); // magnetometer Z offset + + return mavlink_finalize_message(msg, system_id, component_id, i); +} + +/** + * @brief Pack a set_mag_offsets message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param mag_ofs_x magnetometer X offset + * @param mag_ofs_y magnetometer Y offset + * @param mag_ofs_z magnetometer Z offset + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_mag_offsets_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, int16_t mag_ofs_x, int16_t mag_ofs_y, int16_t mag_ofs_z) +{ + uint16_t i = 0; + msg->msgid = MAVLINK_MSG_ID_SET_MAG_OFFSETS; + + i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID + i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID + i += put_int16_t_by_index(mag_ofs_x, i, msg->payload); // magnetometer X offset + i += put_int16_t_by_index(mag_ofs_y, i, msg->payload); // magnetometer Y offset + i += put_int16_t_by_index(mag_ofs_z, i, msg->payload); // magnetometer Z offset + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); +} + +/** + * @brief Encode a set_mag_offsets struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param set_mag_offsets C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_mag_offsets_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_mag_offsets_t* set_mag_offsets) +{ + return mavlink_msg_set_mag_offsets_pack(system_id, component_id, msg, set_mag_offsets->target_system, set_mag_offsets->target_component, set_mag_offsets->mag_ofs_x, set_mag_offsets->mag_ofs_y, set_mag_offsets->mag_ofs_z); +} + +/** + * @brief Send a set_mag_offsets message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param mag_ofs_x magnetometer X offset + * @param mag_ofs_y magnetometer Y offset + * @param mag_ofs_z magnetometer Z offset + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_set_mag_offsets_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int16_t mag_ofs_x, int16_t mag_ofs_y, int16_t mag_ofs_z) +{ + mavlink_message_t msg; + mavlink_msg_set_mag_offsets_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, mag_ofs_x, mag_ofs_y, mag_ofs_z); + mavlink_send_uart(chan, &msg); +} + +#endif +// MESSAGE SET_MAG_OFFSETS UNPACKING + +/** + * @brief Get field target_system from set_mag_offsets message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_set_mag_offsets_get_target_system(const mavlink_message_t* msg) +{ + return (uint8_t)(msg->payload)[0]; +} + +/** + * @brief Get field target_component from set_mag_offsets message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_set_mag_offsets_get_target_component(const mavlink_message_t* msg) +{ + return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; +} + +/** + * @brief Get field mag_ofs_x from set_mag_offsets message + * + * @return magnetometer X offset + */ +static inline int16_t mavlink_msg_set_mag_offsets_get_mag_ofs_x(const mavlink_message_t* msg) +{ + generic_16bit r; + r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; + r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[1]; + return (int16_t)r.s; +} + +/** + * @brief Get field mag_ofs_y from set_mag_offsets message + * + * @return magnetometer Y offset + */ +static inline int16_t mavlink_msg_set_mag_offsets_get_mag_ofs_y(const mavlink_message_t* msg) +{ + generic_16bit r; + r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(int16_t))[0]; + r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(int16_t))[1]; + return (int16_t)r.s; +} + +/** + * @brief Get field mag_ofs_z from set_mag_offsets message + * + * @return magnetometer Z offset + */ +static inline int16_t mavlink_msg_set_mag_offsets_get_mag_ofs_z(const mavlink_message_t* msg) +{ + generic_16bit r; + r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(int16_t)+sizeof(int16_t))[0]; + r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(int16_t)+sizeof(int16_t))[1]; + return (int16_t)r.s; +} + +/** + * @brief Decode a set_mag_offsets message into a struct + * + * @param msg The message to decode + * @param set_mag_offsets C-struct to decode the message contents into + */ +static inline void mavlink_msg_set_mag_offsets_decode(const mavlink_message_t* msg, mavlink_set_mag_offsets_t* set_mag_offsets) +{ + set_mag_offsets->target_system = mavlink_msg_set_mag_offsets_get_target_system(msg); + set_mag_offsets->target_component = mavlink_msg_set_mag_offsets_get_target_component(msg); + set_mag_offsets->mag_ofs_x = mavlink_msg_set_mag_offsets_get_mag_ofs_x(msg); + set_mag_offsets->mag_ofs_y = mavlink_msg_set_mag_offsets_get_mag_ofs_y(msg); + set_mag_offsets->mag_ofs_z = mavlink_msg_set_mag_offsets_get_mag_ofs_z(msg); +} diff --git a/thirdParty/mavlink/include/common/common.h b/thirdParty/mavlink/include/common/common.h index e7b08400924cbb9a7a47553784664a75ec3b6145..8e0348723e64b6ba0682af65d5323453e194b243 100644 --- a/thirdParty/mavlink/include/common/common.h +++ b/thirdParty/mavlink/include/common/common.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://qgroundcontrol.org/mavlink/ - * Generated on Monday, August 15 2011, 15:40 UTC + * Generated on Monday, August 22 2011, 15:48 UTC */ #ifndef COMMON_H #define COMMON_H @@ -55,7 +55,8 @@ enum MAV_CMD MAV_CMD_DO_REPEAT_RELAY=182, /* Cycle a relay on and off for a desired number of cyles with a desired period. | Relay number | Cycle count | Cycle time (seconds, decimal) | Empty | Empty | Empty | Empty | */ MAV_CMD_DO_SET_SERVO=183, /* Set a servo to a desired PWM value. | Servo number | PWM (microseconds, 1000 to 2000 typical) | Empty | Empty | Empty | Empty | Empty | */ MAV_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. | Servo number | PWM (microseconds, 1000 to 2000 typical) | Cycle count | Cycle time (seconds) | Empty | Empty | Empty | */ - MAV_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera system. | Camera ID (-1 for all) | Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw | Transmission mode: 0: video stream, >0: single images every n seconds (decimal) | Recording: 0: disabled, 1: enabled compressed, 2: enabled raw | Empty | Empty | Empty | */ + MAV_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera capturing. | Camera ID (-1 for all) | Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw | Transmission mode: 0: video stream, >0: single images every n seconds (decimal) | Recording: 0: disabled, 1: enabled compressed, 2: enabled raw | Empty | Empty | Empty | */ + MAV_CMD_DO_SET_ROI=201, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various devices such as cameras. | Region of interest mode. (see MAV_ROI enum) | Waypoint index/ target ID. (see MAV_ROI enum) | ROI index (allows a vehicle to manage multiple cameras etc.) | Empty | x the location of the fixed ROI (see MAV_FRAME) | y | z | */ MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration | Empty | Empty | Empty | Empty | Empty | Empty | Empty | */ MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. | Gyro calibration: 0: no, 1: yes | Magnetometer calibration: 0: no, 1: yes | Ground pressure: 0: no, 1: yes | Radio calibration: 0: no, 1: yes | Empty | Empty | Empty | */ MAV_CMD_PREFLIGHT_STORAGE=245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. | Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM | Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM | Reserved | Reserved | Empty | Empty | Empty | */ @@ -77,13 +78,14 @@ enum MAV_DATA_STREAM MAV_DATA_STREAM_ENUM_END }; -/** @brief The ROI (region of interest) for the vehicle. This can be be used by the vehicle for camera/vehicle attitude alignment (see MAV_CMD_NAV_ROI). */ +/** @brief The ROI (region of interest) for the vehicle. This can be be used by the vehicle for camera/vehicle attitude alignment (see MAV_CMD_NAV_ROI). */ enum MAV_ROI { - MAV_ROI_WPNEXT=0, /* Point toward next waypoint. | */ - MAV_ROI_WPINDEX=1, /* Point toward given waypoint. | */ - MAV_ROI_LOCATION=2, /* Point toward fixed location. | */ - MAV_ROI_TARGET=3, /* Point toward of given id. | */ + MAV_ROI_NONE=0, /* No region of interest. | */ + MAV_ROI_WPNEXT=1, /* Point toward next waypoint. | */ + MAV_ROI_WPINDEX=2, /* Point toward given waypoint. | */ + MAV_ROI_LOCATION=3, /* Point toward fixed location. | */ + MAV_ROI_TARGET=4, /* Point toward of given id. | */ MAV_ROI_ENUM_END }; @@ -136,18 +138,25 @@ enum MAV_ROI #include "./mavlink_msg_control_status.h" #include "./mavlink_msg_safety_set_allowed_area.h" #include "./mavlink_msg_safety_allowed_area.h" -#include "./mavlink_msg_attitude_controller_output.h" -#include "./mavlink_msg_position_controller_output.h" +#include "./mavlink_msg_set_roll_pitch_yaw_thrust.h" +#include "./mavlink_msg_set_roll_pitch_yaw_speed_thrust.h" +#include "./mavlink_msg_roll_pitch_yaw_thrust_setpoint.h" +#include "./mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h" #include "./mavlink_msg_nav_controller_output.h" #include "./mavlink_msg_position_target.h" #include "./mavlink_msg_state_correction.h" #include "./mavlink_msg_set_altitude.h" #include "./mavlink_msg_request_data_stream.h" +#include "./mavlink_msg_hil_state.h" +#include "./mavlink_msg_hil_controls.h" #include "./mavlink_msg_manual_control.h" +#include "./mavlink_msg_rc_channels_override.h" #include "./mavlink_msg_global_position_int.h" #include "./mavlink_msg_vfr_hud.h" #include "./mavlink_msg_command.h" #include "./mavlink_msg_command_ack.h" +#include "./mavlink_msg_optical_flow.h" +#include "./mavlink_msg_object_detection_event.h" #include "./mavlink_msg_debug_vect.h" #include "./mavlink_msg_named_value_float.h" #include "./mavlink_msg_named_value_int.h" @@ -158,7 +167,7 @@ enum MAV_ROI // MESSAGE LENGTHS #undef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS { 3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 0, 0, 0, 0, 0, 5, 5, 26, 16, 36, 5, 6, 0, 0, 21, 0, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51 } +#define MAVLINK_MESSAGE_LENGTHS { 3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 18, 18, 24, 24, 0, 0, 0, 26, 16, 36, 5, 6, 56, 26, 21, 18, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 18, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51 } #ifdef __cplusplus } diff --git a/thirdParty/mavlink/include/common/mavlink.h b/thirdParty/mavlink/include/common/mavlink.h index d7ddcf77af010cdeb70bf893d384fd35312acf3d..86a5baf89ca82ee6d87b81337a492c583787010f 100644 --- a/thirdParty/mavlink/include/common/mavlink.h +++ b/thirdParty/mavlink/include/common/mavlink.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Monday, August 15 2011, 15:40 UTC + * Generated on Monday, August 22 2011, 15:48 UTC */ #ifndef MAVLINK_H #define MAVLINK_H diff --git a/thirdParty/mavlink/include/common/mavlink_msg_attitude_controller_output.h b/thirdParty/mavlink/include/common/mavlink_msg_attitude_controller_output.h deleted file mode 100644 index 60fa775540642cfd8eb876fd33163edb75ab4946..0000000000000000000000000000000000000000 --- a/thirdParty/mavlink/include/common/mavlink_msg_attitude_controller_output.h +++ /dev/null @@ -1,169 +0,0 @@ -// MESSAGE ATTITUDE_CONTROLLER_OUTPUT PACKING - -#define MAVLINK_MSG_ID_ATTITUDE_CONTROLLER_OUTPUT 60 - -typedef struct __mavlink_attitude_controller_output_t -{ - uint8_t enabled; ///< 1: enabled, 0: disabled - int8_t roll; ///< Attitude roll: -128: -100%, 127: +100% - int8_t pitch; ///< Attitude pitch: -128: -100%, 127: +100% - int8_t yaw; ///< Attitude yaw: -128: -100%, 127: +100% - int8_t thrust; ///< Attitude thrust: -128: -100%, 127: +100% - -} mavlink_attitude_controller_output_t; - - - -/** - * @brief Pack a attitude_controller_output message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param enabled 1: enabled, 0: disabled - * @param roll Attitude roll: -128: -100%, 127: +100% - * @param pitch Attitude pitch: -128: -100%, 127: +100% - * @param yaw Attitude yaw: -128: -100%, 127: +100% - * @param thrust Attitude thrust: -128: -100%, 127: +100% - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_attitude_controller_output_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t enabled, int8_t roll, int8_t pitch, int8_t yaw, int8_t thrust) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_ATTITUDE_CONTROLLER_OUTPUT; - - i += put_uint8_t_by_index(enabled, i, msg->payload); // 1: enabled, 0: disabled - i += put_int8_t_by_index(roll, i, msg->payload); // Attitude roll: -128: -100%, 127: +100% - i += put_int8_t_by_index(pitch, i, msg->payload); // Attitude pitch: -128: -100%, 127: +100% - i += put_int8_t_by_index(yaw, i, msg->payload); // Attitude yaw: -128: -100%, 127: +100% - i += put_int8_t_by_index(thrust, i, msg->payload); // Attitude thrust: -128: -100%, 127: +100% - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -/** - * @brief Pack a attitude_controller_output message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param enabled 1: enabled, 0: disabled - * @param roll Attitude roll: -128: -100%, 127: +100% - * @param pitch Attitude pitch: -128: -100%, 127: +100% - * @param yaw Attitude yaw: -128: -100%, 127: +100% - * @param thrust Attitude thrust: -128: -100%, 127: +100% - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_attitude_controller_output_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t enabled, int8_t roll, int8_t pitch, int8_t yaw, int8_t thrust) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_ATTITUDE_CONTROLLER_OUTPUT; - - i += put_uint8_t_by_index(enabled, i, msg->payload); // 1: enabled, 0: disabled - i += put_int8_t_by_index(roll, i, msg->payload); // Attitude roll: -128: -100%, 127: +100% - i += put_int8_t_by_index(pitch, i, msg->payload); // Attitude pitch: -128: -100%, 127: +100% - i += put_int8_t_by_index(yaw, i, msg->payload); // Attitude yaw: -128: -100%, 127: +100% - i += put_int8_t_by_index(thrust, i, msg->payload); // Attitude thrust: -128: -100%, 127: +100% - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); -} - -/** - * @brief Encode a attitude_controller_output struct into a message - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param attitude_controller_output C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_attitude_controller_output_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_attitude_controller_output_t* attitude_controller_output) -{ - return mavlink_msg_attitude_controller_output_pack(system_id, component_id, msg, attitude_controller_output->enabled, attitude_controller_output->roll, attitude_controller_output->pitch, attitude_controller_output->yaw, attitude_controller_output->thrust); -} - -/** - * @brief Send a attitude_controller_output message - * @param chan MAVLink channel to send the message - * - * @param enabled 1: enabled, 0: disabled - * @param roll Attitude roll: -128: -100%, 127: +100% - * @param pitch Attitude pitch: -128: -100%, 127: +100% - * @param yaw Attitude yaw: -128: -100%, 127: +100% - * @param thrust Attitude thrust: -128: -100%, 127: +100% - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_attitude_controller_output_send(mavlink_channel_t chan, uint8_t enabled, int8_t roll, int8_t pitch, int8_t yaw, int8_t thrust) -{ - mavlink_message_t msg; - mavlink_msg_attitude_controller_output_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, enabled, roll, pitch, yaw, thrust); - mavlink_send_uart(chan, &msg); -} - -#endif -// MESSAGE ATTITUDE_CONTROLLER_OUTPUT UNPACKING - -/** - * @brief Get field enabled from attitude_controller_output message - * - * @return 1: enabled, 0: disabled - */ -static inline uint8_t mavlink_msg_attitude_controller_output_get_enabled(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload)[0]; -} - -/** - * @brief Get field roll from attitude_controller_output message - * - * @return Attitude roll: -128: -100%, 127: +100% - */ -static inline int8_t mavlink_msg_attitude_controller_output_get_roll(const mavlink_message_t* msg) -{ - return (int8_t)(msg->payload+sizeof(uint8_t))[0]; -} - -/** - * @brief Get field pitch from attitude_controller_output message - * - * @return Attitude pitch: -128: -100%, 127: +100% - */ -static inline int8_t mavlink_msg_attitude_controller_output_get_pitch(const mavlink_message_t* msg) -{ - return (int8_t)(msg->payload+sizeof(uint8_t)+sizeof(int8_t))[0]; -} - -/** - * @brief Get field yaw from attitude_controller_output message - * - * @return Attitude yaw: -128: -100%, 127: +100% - */ -static inline int8_t mavlink_msg_attitude_controller_output_get_yaw(const mavlink_message_t* msg) -{ - return (int8_t)(msg->payload+sizeof(uint8_t)+sizeof(int8_t)+sizeof(int8_t))[0]; -} - -/** - * @brief Get field thrust from attitude_controller_output message - * - * @return Attitude thrust: -128: -100%, 127: +100% - */ -static inline int8_t mavlink_msg_attitude_controller_output_get_thrust(const mavlink_message_t* msg) -{ - return (int8_t)(msg->payload+sizeof(uint8_t)+sizeof(int8_t)+sizeof(int8_t)+sizeof(int8_t))[0]; -} - -/** - * @brief Decode a attitude_controller_output message into a struct - * - * @param msg The message to decode - * @param attitude_controller_output C-struct to decode the message contents into - */ -static inline void mavlink_msg_attitude_controller_output_decode(const mavlink_message_t* msg, mavlink_attitude_controller_output_t* attitude_controller_output) -{ - attitude_controller_output->enabled = mavlink_msg_attitude_controller_output_get_enabled(msg); - attitude_controller_output->roll = mavlink_msg_attitude_controller_output_get_roll(msg); - attitude_controller_output->pitch = mavlink_msg_attitude_controller_output_get_pitch(msg); - attitude_controller_output->yaw = mavlink_msg_attitude_controller_output_get_yaw(msg); - attitude_controller_output->thrust = mavlink_msg_attitude_controller_output_get_thrust(msg); -} diff --git a/thirdParty/mavlink/include/common/mavlink_msg_full_state.h b/thirdParty/mavlink/include/common/mavlink_msg_full_state.h new file mode 100644 index 0000000000000000000000000000000000000000..ed177e22e2fcf3ab2494211338d711e4b3f2b51c --- /dev/null +++ b/thirdParty/mavlink/include/common/mavlink_msg_full_state.h @@ -0,0 +1,428 @@ +// MESSAGE FULL_STATE PACKING + +#define MAVLINK_MSG_ID_FULL_STATE 67 + +typedef struct __mavlink_full_state_t +{ + uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + float roll; ///< Roll angle (rad) + float pitch; ///< Pitch angle (rad) + float yaw; ///< Yaw angle (rad) + float rollspeed; ///< Roll angular speed (rad/s) + float pitchspeed; ///< Pitch angular speed (rad/s) + float yawspeed; ///< Yaw angular speed (rad/s) + int32_t lat; ///< Latitude, expressed as * 1E7 + int32_t lon; ///< Longitude, expressed as * 1E7 + int32_t alt; ///< Altitude in meters, expressed as * 1000 (millimeters) + int16_t vx; ///< Ground X Speed (Latitude), expressed as m/s * 100 + int16_t vy; ///< Ground Y Speed (Longitude), expressed as m/s * 100 + int16_t vz; ///< Ground Z Speed (Altitude), expressed as m/s * 100 + int16_t xacc; ///< X acceleration (mg) + int16_t yacc; ///< Y acceleration (mg) + int16_t zacc; ///< Z acceleration (mg) + +} mavlink_full_state_t; + + + +/** + * @brief Pack a full_state message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param roll Roll angle (rad) + * @param pitch Pitch angle (rad) + * @param yaw Yaw angle (rad) + * @param rollspeed Roll angular speed (rad/s) + * @param pitchspeed Pitch angular speed (rad/s) + * @param yawspeed Yaw angular speed (rad/s) + * @param lat Latitude, expressed as * 1E7 + * @param lon Longitude, expressed as * 1E7 + * @param alt Altitude in meters, expressed as * 1000 (millimeters) + * @param vx Ground X Speed (Latitude), expressed as m/s * 100 + * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 + * @param vz Ground Z Speed (Altitude), expressed as m/s * 100 + * @param xacc X acceleration (mg) + * @param yacc Y acceleration (mg) + * @param zacc Z acceleration (mg) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_full_state_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, int16_t xacc, int16_t yacc, int16_t zacc) +{ + uint16_t i = 0; + msg->msgid = MAVLINK_MSG_ID_FULL_STATE; + + i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) + i += put_float_by_index(roll, i, msg->payload); // Roll angle (rad) + i += put_float_by_index(pitch, i, msg->payload); // Pitch angle (rad) + i += put_float_by_index(yaw, i, msg->payload); // Yaw angle (rad) + i += put_float_by_index(rollspeed, i, msg->payload); // Roll angular speed (rad/s) + i += put_float_by_index(pitchspeed, i, msg->payload); // Pitch angular speed (rad/s) + i += put_float_by_index(yawspeed, i, msg->payload); // Yaw angular speed (rad/s) + i += put_int32_t_by_index(lat, i, msg->payload); // Latitude, expressed as * 1E7 + i += put_int32_t_by_index(lon, i, msg->payload); // Longitude, expressed as * 1E7 + i += put_int32_t_by_index(alt, i, msg->payload); // Altitude in meters, expressed as * 1000 (millimeters) + i += put_int16_t_by_index(vx, i, msg->payload); // Ground X Speed (Latitude), expressed as m/s * 100 + i += put_int16_t_by_index(vy, i, msg->payload); // Ground Y Speed (Longitude), expressed as m/s * 100 + i += put_int16_t_by_index(vz, i, msg->payload); // Ground Z Speed (Altitude), expressed as m/s * 100 + i += put_int16_t_by_index(xacc, i, msg->payload); // X acceleration (mg) + i += put_int16_t_by_index(yacc, i, msg->payload); // Y acceleration (mg) + i += put_int16_t_by_index(zacc, i, msg->payload); // Z acceleration (mg) + + return mavlink_finalize_message(msg, system_id, component_id, i); +} + +/** + * @brief Pack a full_state message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param roll Roll angle (rad) + * @param pitch Pitch angle (rad) + * @param yaw Yaw angle (rad) + * @param rollspeed Roll angular speed (rad/s) + * @param pitchspeed Pitch angular speed (rad/s) + * @param yawspeed Yaw angular speed (rad/s) + * @param lat Latitude, expressed as * 1E7 + * @param lon Longitude, expressed as * 1E7 + * @param alt Altitude in meters, expressed as * 1000 (millimeters) + * @param vx Ground X Speed (Latitude), expressed as m/s * 100 + * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 + * @param vz Ground Z Speed (Altitude), expressed as m/s * 100 + * @param xacc X acceleration (mg) + * @param yacc Y acceleration (mg) + * @param zacc Z acceleration (mg) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_full_state_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, int16_t xacc, int16_t yacc, int16_t zacc) +{ + uint16_t i = 0; + msg->msgid = MAVLINK_MSG_ID_FULL_STATE; + + i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) + i += put_float_by_index(roll, i, msg->payload); // Roll angle (rad) + i += put_float_by_index(pitch, i, msg->payload); // Pitch angle (rad) + i += put_float_by_index(yaw, i, msg->payload); // Yaw angle (rad) + i += put_float_by_index(rollspeed, i, msg->payload); // Roll angular speed (rad/s) + i += put_float_by_index(pitchspeed, i, msg->payload); // Pitch angular speed (rad/s) + i += put_float_by_index(yawspeed, i, msg->payload); // Yaw angular speed (rad/s) + i += put_int32_t_by_index(lat, i, msg->payload); // Latitude, expressed as * 1E7 + i += put_int32_t_by_index(lon, i, msg->payload); // Longitude, expressed as * 1E7 + i += put_int32_t_by_index(alt, i, msg->payload); // Altitude in meters, expressed as * 1000 (millimeters) + i += put_int16_t_by_index(vx, i, msg->payload); // Ground X Speed (Latitude), expressed as m/s * 100 + i += put_int16_t_by_index(vy, i, msg->payload); // Ground Y Speed (Longitude), expressed as m/s * 100 + i += put_int16_t_by_index(vz, i, msg->payload); // Ground Z Speed (Altitude), expressed as m/s * 100 + i += put_int16_t_by_index(xacc, i, msg->payload); // X acceleration (mg) + i += put_int16_t_by_index(yacc, i, msg->payload); // Y acceleration (mg) + i += put_int16_t_by_index(zacc, i, msg->payload); // Z acceleration (mg) + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); +} + +/** + * @brief Encode a full_state struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param full_state C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_full_state_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_full_state_t* full_state) +{ + return mavlink_msg_full_state_pack(system_id, component_id, msg, full_state->usec, full_state->roll, full_state->pitch, full_state->yaw, full_state->rollspeed, full_state->pitchspeed, full_state->yawspeed, full_state->lat, full_state->lon, full_state->alt, full_state->vx, full_state->vy, full_state->vz, full_state->xacc, full_state->yacc, full_state->zacc); +} + +/** + * @brief Send a full_state message + * @param chan MAVLink channel to send the message + * + * @param usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param roll Roll angle (rad) + * @param pitch Pitch angle (rad) + * @param yaw Yaw angle (rad) + * @param rollspeed Roll angular speed (rad/s) + * @param pitchspeed Pitch angular speed (rad/s) + * @param yawspeed Yaw angular speed (rad/s) + * @param lat Latitude, expressed as * 1E7 + * @param lon Longitude, expressed as * 1E7 + * @param alt Altitude in meters, expressed as * 1000 (millimeters) + * @param vx Ground X Speed (Latitude), expressed as m/s * 100 + * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 + * @param vz Ground Z Speed (Altitude), expressed as m/s * 100 + * @param xacc X acceleration (mg) + * @param yacc Y acceleration (mg) + * @param zacc Z acceleration (mg) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_full_state_send(mavlink_channel_t chan, uint64_t usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, int16_t xacc, int16_t yacc, int16_t zacc) +{ + mavlink_message_t msg; + mavlink_msg_full_state_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, usec, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, lat, lon, alt, vx, vy, vz, xacc, yacc, zacc); + mavlink_send_uart(chan, &msg); +} + +#endif +// MESSAGE FULL_STATE UNPACKING + +/** + * @brief Get field usec from full_state message + * + * @return Timestamp (microseconds since UNIX epoch or microseconds since system boot) + */ +static inline uint64_t mavlink_msg_full_state_get_usec(const mavlink_message_t* msg) +{ + generic_64bit r; + r.b[7] = (msg->payload)[0]; + r.b[6] = (msg->payload)[1]; + r.b[5] = (msg->payload)[2]; + r.b[4] = (msg->payload)[3]; + r.b[3] = (msg->payload)[4]; + r.b[2] = (msg->payload)[5]; + r.b[1] = (msg->payload)[6]; + r.b[0] = (msg->payload)[7]; + return (uint64_t)r.ll; +} + +/** + * @brief Get field roll from full_state message + * + * @return Roll angle (rad) + */ +static inline float mavlink_msg_full_state_get_roll(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint64_t))[0]; + r.b[2] = (msg->payload+sizeof(uint64_t))[1]; + r.b[1] = (msg->payload+sizeof(uint64_t))[2]; + r.b[0] = (msg->payload+sizeof(uint64_t))[3]; + return (float)r.f; +} + +/** + * @brief Get field pitch from full_state message + * + * @return Pitch angle (rad) + */ +static inline float mavlink_msg_full_state_get_pitch(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float))[3]; + return (float)r.f; +} + +/** + * @brief Get field yaw from full_state message + * + * @return Yaw angle (rad) + */ +static inline float mavlink_msg_full_state_get_yaw(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[3]; + return (float)r.f; +} + +/** + * @brief Get field rollspeed from full_state message + * + * @return Roll angular speed (rad/s) + */ +static inline float mavlink_msg_full_state_get_rollspeed(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; + return (float)r.f; +} + +/** + * @brief Get field pitchspeed from full_state message + * + * @return Pitch angular speed (rad/s) + */ +static inline float mavlink_msg_full_state_get_pitchspeed(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; + return (float)r.f; +} + +/** + * @brief Get field yawspeed from full_state message + * + * @return Yaw angular speed (rad/s) + */ +static inline float mavlink_msg_full_state_get_yawspeed(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; + return (float)r.f; +} + +/** + * @brief Get field lat from full_state message + * + * @return Latitude, expressed as * 1E7 + */ +static inline int32_t mavlink_msg_full_state_get_lat(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; + return (int32_t)r.i; +} + +/** + * @brief Get field lon from full_state message + * + * @return Longitude, expressed as * 1E7 + */ +static inline int32_t mavlink_msg_full_state_get_lon(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t))[0]; + r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t))[1]; + r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t))[2]; + r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t))[3]; + return (int32_t)r.i; +} + +/** + * @brief Get field alt from full_state message + * + * @return Altitude in meters, expressed as * 1000 (millimeters) + */ +static inline int32_t mavlink_msg_full_state_get_alt(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t))[0]; + r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t))[1]; + r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t))[2]; + r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t))[3]; + return (int32_t)r.i; +} + +/** + * @brief Get field vx from full_state message + * + * @return Ground X Speed (Latitude), expressed as m/s * 100 + */ +static inline int16_t mavlink_msg_full_state_get_vx(const mavlink_message_t* msg) +{ + generic_16bit r; + r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t))[0]; + r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t))[1]; + return (int16_t)r.s; +} + +/** + * @brief Get field vy from full_state message + * + * @return Ground Y Speed (Longitude), expressed as m/s * 100 + */ +static inline int16_t mavlink_msg_full_state_get_vy(const mavlink_message_t* msg) +{ + generic_16bit r; + r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t))[0]; + r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t))[1]; + return (int16_t)r.s; +} + +/** + * @brief Get field vz from full_state message + * + * @return Ground Z Speed (Altitude), expressed as m/s * 100 + */ +static inline int16_t mavlink_msg_full_state_get_vz(const mavlink_message_t* msg) +{ + generic_16bit r; + r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t))[0]; + r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t))[1]; + return (int16_t)r.s; +} + +/** + * @brief Get field xacc from full_state message + * + * @return X acceleration (mg) + */ +static inline int16_t mavlink_msg_full_state_get_xacc(const mavlink_message_t* msg) +{ + generic_16bit r; + r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; + r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; + return (int16_t)r.s; +} + +/** + * @brief Get field yacc from full_state message + * + * @return Y acceleration (mg) + */ +static inline int16_t mavlink_msg_full_state_get_yacc(const mavlink_message_t* msg) +{ + generic_16bit r; + r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; + r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; + return (int16_t)r.s; +} + +/** + * @brief Get field zacc from full_state message + * + * @return Z acceleration (mg) + */ +static inline int16_t mavlink_msg_full_state_get_zacc(const mavlink_message_t* msg) +{ + generic_16bit r; + r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; + r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; + return (int16_t)r.s; +} + +/** + * @brief Decode a full_state message into a struct + * + * @param msg The message to decode + * @param full_state C-struct to decode the message contents into + */ +static inline void mavlink_msg_full_state_decode(const mavlink_message_t* msg, mavlink_full_state_t* full_state) +{ + full_state->usec = mavlink_msg_full_state_get_usec(msg); + full_state->roll = mavlink_msg_full_state_get_roll(msg); + full_state->pitch = mavlink_msg_full_state_get_pitch(msg); + full_state->yaw = mavlink_msg_full_state_get_yaw(msg); + full_state->rollspeed = mavlink_msg_full_state_get_rollspeed(msg); + full_state->pitchspeed = mavlink_msg_full_state_get_pitchspeed(msg); + full_state->yawspeed = mavlink_msg_full_state_get_yawspeed(msg); + full_state->lat = mavlink_msg_full_state_get_lat(msg); + full_state->lon = mavlink_msg_full_state_get_lon(msg); + full_state->alt = mavlink_msg_full_state_get_alt(msg); + full_state->vx = mavlink_msg_full_state_get_vx(msg); + full_state->vy = mavlink_msg_full_state_get_vy(msg); + full_state->vz = mavlink_msg_full_state_get_vz(msg); + full_state->xacc = mavlink_msg_full_state_get_xacc(msg); + full_state->yacc = mavlink_msg_full_state_get_yacc(msg); + full_state->zacc = mavlink_msg_full_state_get_zacc(msg); +} diff --git a/thirdParty/mavlink/include/common/mavlink_msg_hil_controls.h b/thirdParty/mavlink/include/common/mavlink_msg_hil_controls.h new file mode 100644 index 0000000000000000000000000000000000000000..80f33ebd06a9497d2d3f8678d8a104960d280926 --- /dev/null +++ b/thirdParty/mavlink/include/common/mavlink_msg_hil_controls.h @@ -0,0 +1,232 @@ +// MESSAGE HIL_CONTROLS PACKING + +#define MAVLINK_MSG_ID_HIL_CONTROLS 68 + +typedef struct __mavlink_hil_controls_t +{ + uint64_t time_us; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + float roll_ailerons; ///< Control output -3 .. 1 + float pitch_elevator; ///< Control output -1 .. 1 + float yaw_rudder; ///< Control output -1 .. 1 + float throttle; ///< Throttle 0 .. 1 + uint8_t mode; ///< System mode (MAV_MODE) + uint8_t nav_mode; ///< Navigation mode (MAV_NAV_MODE) + +} mavlink_hil_controls_t; + + + +/** + * @brief Pack a hil_controls message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_us Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param roll_ailerons Control output -3 .. 1 + * @param pitch_elevator Control output -1 .. 1 + * @param yaw_rudder Control output -1 .. 1 + * @param throttle Throttle 0 .. 1 + * @param mode System mode (MAV_MODE) + * @param nav_mode Navigation mode (MAV_NAV_MODE) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_hil_controls_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t time_us, float roll_ailerons, float pitch_elevator, float yaw_rudder, float throttle, uint8_t mode, uint8_t nav_mode) +{ + uint16_t i = 0; + msg->msgid = MAVLINK_MSG_ID_HIL_CONTROLS; + + i += put_uint64_t_by_index(time_us, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) + i += put_float_by_index(roll_ailerons, i, msg->payload); // Control output -3 .. 1 + i += put_float_by_index(pitch_elevator, i, msg->payload); // Control output -1 .. 1 + i += put_float_by_index(yaw_rudder, i, msg->payload); // Control output -1 .. 1 + i += put_float_by_index(throttle, i, msg->payload); // Throttle 0 .. 1 + i += put_uint8_t_by_index(mode, i, msg->payload); // System mode (MAV_MODE) + i += put_uint8_t_by_index(nav_mode, i, msg->payload); // Navigation mode (MAV_NAV_MODE) + + return mavlink_finalize_message(msg, system_id, component_id, i); +} + +/** + * @brief Pack a hil_controls message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param time_us Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param roll_ailerons Control output -3 .. 1 + * @param pitch_elevator Control output -1 .. 1 + * @param yaw_rudder Control output -1 .. 1 + * @param throttle Throttle 0 .. 1 + * @param mode System mode (MAV_MODE) + * @param nav_mode Navigation mode (MAV_NAV_MODE) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_hil_controls_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t time_us, float roll_ailerons, float pitch_elevator, float yaw_rudder, float throttle, uint8_t mode, uint8_t nav_mode) +{ + uint16_t i = 0; + msg->msgid = MAVLINK_MSG_ID_HIL_CONTROLS; + + i += put_uint64_t_by_index(time_us, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) + i += put_float_by_index(roll_ailerons, i, msg->payload); // Control output -3 .. 1 + i += put_float_by_index(pitch_elevator, i, msg->payload); // Control output -1 .. 1 + i += put_float_by_index(yaw_rudder, i, msg->payload); // Control output -1 .. 1 + i += put_float_by_index(throttle, i, msg->payload); // Throttle 0 .. 1 + i += put_uint8_t_by_index(mode, i, msg->payload); // System mode (MAV_MODE) + i += put_uint8_t_by_index(nav_mode, i, msg->payload); // Navigation mode (MAV_NAV_MODE) + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); +} + +/** + * @brief Encode a hil_controls struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param hil_controls C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hil_controls_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_hil_controls_t* hil_controls) +{ + return mavlink_msg_hil_controls_pack(system_id, component_id, msg, hil_controls->time_us, hil_controls->roll_ailerons, hil_controls->pitch_elevator, hil_controls->yaw_rudder, hil_controls->throttle, hil_controls->mode, hil_controls->nav_mode); +} + +/** + * @brief Send a hil_controls message + * @param chan MAVLink channel to send the message + * + * @param time_us Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param roll_ailerons Control output -3 .. 1 + * @param pitch_elevator Control output -1 .. 1 + * @param yaw_rudder Control output -1 .. 1 + * @param throttle Throttle 0 .. 1 + * @param mode System mode (MAV_MODE) + * @param nav_mode Navigation mode (MAV_NAV_MODE) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_hil_controls_send(mavlink_channel_t chan, uint64_t time_us, float roll_ailerons, float pitch_elevator, float yaw_rudder, float throttle, uint8_t mode, uint8_t nav_mode) +{ + mavlink_message_t msg; + mavlink_msg_hil_controls_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, time_us, roll_ailerons, pitch_elevator, yaw_rudder, throttle, mode, nav_mode); + mavlink_send_uart(chan, &msg); +} + +#endif +// MESSAGE HIL_CONTROLS UNPACKING + +/** + * @brief Get field time_us from hil_controls message + * + * @return Timestamp (microseconds since UNIX epoch or microseconds since system boot) + */ +static inline uint64_t mavlink_msg_hil_controls_get_time_us(const mavlink_message_t* msg) +{ + generic_64bit r; + r.b[7] = (msg->payload)[0]; + r.b[6] = (msg->payload)[1]; + r.b[5] = (msg->payload)[2]; + r.b[4] = (msg->payload)[3]; + r.b[3] = (msg->payload)[4]; + r.b[2] = (msg->payload)[5]; + r.b[1] = (msg->payload)[6]; + r.b[0] = (msg->payload)[7]; + return (uint64_t)r.ll; +} + +/** + * @brief Get field roll_ailerons from hil_controls message + * + * @return Control output -3 .. 1 + */ +static inline float mavlink_msg_hil_controls_get_roll_ailerons(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint64_t))[0]; + r.b[2] = (msg->payload+sizeof(uint64_t))[1]; + r.b[1] = (msg->payload+sizeof(uint64_t))[2]; + r.b[0] = (msg->payload+sizeof(uint64_t))[3]; + return (float)r.f; +} + +/** + * @brief Get field pitch_elevator from hil_controls message + * + * @return Control output -1 .. 1 + */ +static inline float mavlink_msg_hil_controls_get_pitch_elevator(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float))[3]; + return (float)r.f; +} + +/** + * @brief Get field yaw_rudder from hil_controls message + * + * @return Control output -1 .. 1 + */ +static inline float mavlink_msg_hil_controls_get_yaw_rudder(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[3]; + return (float)r.f; +} + +/** + * @brief Get field throttle from hil_controls message + * + * @return Throttle 0 .. 1 + */ +static inline float mavlink_msg_hil_controls_get_throttle(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; + return (float)r.f; +} + +/** + * @brief Get field mode from hil_controls message + * + * @return System mode (MAV_MODE) + */ +static inline uint8_t mavlink_msg_hil_controls_get_mode(const mavlink_message_t* msg) +{ + return (uint8_t)(msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; +} + +/** + * @brief Get field nav_mode from hil_controls message + * + * @return Navigation mode (MAV_NAV_MODE) + */ +static inline uint8_t mavlink_msg_hil_controls_get_nav_mode(const mavlink_message_t* msg) +{ + return (uint8_t)(msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t))[0]; +} + +/** + * @brief Decode a hil_controls message into a struct + * + * @param msg The message to decode + * @param hil_controls C-struct to decode the message contents into + */ +static inline void mavlink_msg_hil_controls_decode(const mavlink_message_t* msg, mavlink_hil_controls_t* hil_controls) +{ + hil_controls->time_us = mavlink_msg_hil_controls_get_time_us(msg); + hil_controls->roll_ailerons = mavlink_msg_hil_controls_get_roll_ailerons(msg); + hil_controls->pitch_elevator = mavlink_msg_hil_controls_get_pitch_elevator(msg); + hil_controls->yaw_rudder = mavlink_msg_hil_controls_get_yaw_rudder(msg); + hil_controls->throttle = mavlink_msg_hil_controls_get_throttle(msg); + hil_controls->mode = mavlink_msg_hil_controls_get_mode(msg); + hil_controls->nav_mode = mavlink_msg_hil_controls_get_nav_mode(msg); +} diff --git a/thirdParty/mavlink/include/common/mavlink_msg_hil_state.h b/thirdParty/mavlink/include/common/mavlink_msg_hil_state.h new file mode 100644 index 0000000000000000000000000000000000000000..371cd4ae46255970abd252901ef905bd96ddf9af --- /dev/null +++ b/thirdParty/mavlink/include/common/mavlink_msg_hil_state.h @@ -0,0 +1,428 @@ +// MESSAGE HIL_STATE PACKING + +#define MAVLINK_MSG_ID_HIL_STATE 67 + +typedef struct __mavlink_hil_state_t +{ + uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + float roll; ///< Roll angle (rad) + float pitch; ///< Pitch angle (rad) + float yaw; ///< Yaw angle (rad) + float rollspeed; ///< Roll angular speed (rad/s) + float pitchspeed; ///< Pitch angular speed (rad/s) + float yawspeed; ///< Yaw angular speed (rad/s) + int32_t lat; ///< Latitude, expressed as * 1E7 + int32_t lon; ///< Longitude, expressed as * 1E7 + int32_t alt; ///< Altitude in meters, expressed as * 1000 (millimeters) + int16_t vx; ///< Ground X Speed (Latitude), expressed as m/s * 100 + int16_t vy; ///< Ground Y Speed (Longitude), expressed as m/s * 100 + int16_t vz; ///< Ground Z Speed (Altitude), expressed as m/s * 100 + int16_t xacc; ///< X acceleration (mg) + int16_t yacc; ///< Y acceleration (mg) + int16_t zacc; ///< Z acceleration (mg) + +} mavlink_hil_state_t; + + + +/** + * @brief Pack a hil_state message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param roll Roll angle (rad) + * @param pitch Pitch angle (rad) + * @param yaw Yaw angle (rad) + * @param rollspeed Roll angular speed (rad/s) + * @param pitchspeed Pitch angular speed (rad/s) + * @param yawspeed Yaw angular speed (rad/s) + * @param lat Latitude, expressed as * 1E7 + * @param lon Longitude, expressed as * 1E7 + * @param alt Altitude in meters, expressed as * 1000 (millimeters) + * @param vx Ground X Speed (Latitude), expressed as m/s * 100 + * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 + * @param vz Ground Z Speed (Altitude), expressed as m/s * 100 + * @param xacc X acceleration (mg) + * @param yacc Y acceleration (mg) + * @param zacc Z acceleration (mg) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_hil_state_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, int16_t xacc, int16_t yacc, int16_t zacc) +{ + uint16_t i = 0; + msg->msgid = MAVLINK_MSG_ID_HIL_STATE; + + i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) + i += put_float_by_index(roll, i, msg->payload); // Roll angle (rad) + i += put_float_by_index(pitch, i, msg->payload); // Pitch angle (rad) + i += put_float_by_index(yaw, i, msg->payload); // Yaw angle (rad) + i += put_float_by_index(rollspeed, i, msg->payload); // Roll angular speed (rad/s) + i += put_float_by_index(pitchspeed, i, msg->payload); // Pitch angular speed (rad/s) + i += put_float_by_index(yawspeed, i, msg->payload); // Yaw angular speed (rad/s) + i += put_int32_t_by_index(lat, i, msg->payload); // Latitude, expressed as * 1E7 + i += put_int32_t_by_index(lon, i, msg->payload); // Longitude, expressed as * 1E7 + i += put_int32_t_by_index(alt, i, msg->payload); // Altitude in meters, expressed as * 1000 (millimeters) + i += put_int16_t_by_index(vx, i, msg->payload); // Ground X Speed (Latitude), expressed as m/s * 100 + i += put_int16_t_by_index(vy, i, msg->payload); // Ground Y Speed (Longitude), expressed as m/s * 100 + i += put_int16_t_by_index(vz, i, msg->payload); // Ground Z Speed (Altitude), expressed as m/s * 100 + i += put_int16_t_by_index(xacc, i, msg->payload); // X acceleration (mg) + i += put_int16_t_by_index(yacc, i, msg->payload); // Y acceleration (mg) + i += put_int16_t_by_index(zacc, i, msg->payload); // Z acceleration (mg) + + return mavlink_finalize_message(msg, system_id, component_id, i); +} + +/** + * @brief Pack a hil_state message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param roll Roll angle (rad) + * @param pitch Pitch angle (rad) + * @param yaw Yaw angle (rad) + * @param rollspeed Roll angular speed (rad/s) + * @param pitchspeed Pitch angular speed (rad/s) + * @param yawspeed Yaw angular speed (rad/s) + * @param lat Latitude, expressed as * 1E7 + * @param lon Longitude, expressed as * 1E7 + * @param alt Altitude in meters, expressed as * 1000 (millimeters) + * @param vx Ground X Speed (Latitude), expressed as m/s * 100 + * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 + * @param vz Ground Z Speed (Altitude), expressed as m/s * 100 + * @param xacc X acceleration (mg) + * @param yacc Y acceleration (mg) + * @param zacc Z acceleration (mg) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_hil_state_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, int16_t xacc, int16_t yacc, int16_t zacc) +{ + uint16_t i = 0; + msg->msgid = MAVLINK_MSG_ID_HIL_STATE; + + i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) + i += put_float_by_index(roll, i, msg->payload); // Roll angle (rad) + i += put_float_by_index(pitch, i, msg->payload); // Pitch angle (rad) + i += put_float_by_index(yaw, i, msg->payload); // Yaw angle (rad) + i += put_float_by_index(rollspeed, i, msg->payload); // Roll angular speed (rad/s) + i += put_float_by_index(pitchspeed, i, msg->payload); // Pitch angular speed (rad/s) + i += put_float_by_index(yawspeed, i, msg->payload); // Yaw angular speed (rad/s) + i += put_int32_t_by_index(lat, i, msg->payload); // Latitude, expressed as * 1E7 + i += put_int32_t_by_index(lon, i, msg->payload); // Longitude, expressed as * 1E7 + i += put_int32_t_by_index(alt, i, msg->payload); // Altitude in meters, expressed as * 1000 (millimeters) + i += put_int16_t_by_index(vx, i, msg->payload); // Ground X Speed (Latitude), expressed as m/s * 100 + i += put_int16_t_by_index(vy, i, msg->payload); // Ground Y Speed (Longitude), expressed as m/s * 100 + i += put_int16_t_by_index(vz, i, msg->payload); // Ground Z Speed (Altitude), expressed as m/s * 100 + i += put_int16_t_by_index(xacc, i, msg->payload); // X acceleration (mg) + i += put_int16_t_by_index(yacc, i, msg->payload); // Y acceleration (mg) + i += put_int16_t_by_index(zacc, i, msg->payload); // Z acceleration (mg) + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); +} + +/** + * @brief Encode a hil_state struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param hil_state C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hil_state_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_hil_state_t* hil_state) +{ + return mavlink_msg_hil_state_pack(system_id, component_id, msg, hil_state->usec, hil_state->roll, hil_state->pitch, hil_state->yaw, hil_state->rollspeed, hil_state->pitchspeed, hil_state->yawspeed, hil_state->lat, hil_state->lon, hil_state->alt, hil_state->vx, hil_state->vy, hil_state->vz, hil_state->xacc, hil_state->yacc, hil_state->zacc); +} + +/** + * @brief Send a hil_state message + * @param chan MAVLink channel to send the message + * + * @param usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param roll Roll angle (rad) + * @param pitch Pitch angle (rad) + * @param yaw Yaw angle (rad) + * @param rollspeed Roll angular speed (rad/s) + * @param pitchspeed Pitch angular speed (rad/s) + * @param yawspeed Yaw angular speed (rad/s) + * @param lat Latitude, expressed as * 1E7 + * @param lon Longitude, expressed as * 1E7 + * @param alt Altitude in meters, expressed as * 1000 (millimeters) + * @param vx Ground X Speed (Latitude), expressed as m/s * 100 + * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 + * @param vz Ground Z Speed (Altitude), expressed as m/s * 100 + * @param xacc X acceleration (mg) + * @param yacc Y acceleration (mg) + * @param zacc Z acceleration (mg) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_hil_state_send(mavlink_channel_t chan, uint64_t usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, int16_t xacc, int16_t yacc, int16_t zacc) +{ + mavlink_message_t msg; + mavlink_msg_hil_state_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, usec, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, lat, lon, alt, vx, vy, vz, xacc, yacc, zacc); + mavlink_send_uart(chan, &msg); +} + +#endif +// MESSAGE HIL_STATE UNPACKING + +/** + * @brief Get field usec from hil_state message + * + * @return Timestamp (microseconds since UNIX epoch or microseconds since system boot) + */ +static inline uint64_t mavlink_msg_hil_state_get_usec(const mavlink_message_t* msg) +{ + generic_64bit r; + r.b[7] = (msg->payload)[0]; + r.b[6] = (msg->payload)[1]; + r.b[5] = (msg->payload)[2]; + r.b[4] = (msg->payload)[3]; + r.b[3] = (msg->payload)[4]; + r.b[2] = (msg->payload)[5]; + r.b[1] = (msg->payload)[6]; + r.b[0] = (msg->payload)[7]; + return (uint64_t)r.ll; +} + +/** + * @brief Get field roll from hil_state message + * + * @return Roll angle (rad) + */ +static inline float mavlink_msg_hil_state_get_roll(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint64_t))[0]; + r.b[2] = (msg->payload+sizeof(uint64_t))[1]; + r.b[1] = (msg->payload+sizeof(uint64_t))[2]; + r.b[0] = (msg->payload+sizeof(uint64_t))[3]; + return (float)r.f; +} + +/** + * @brief Get field pitch from hil_state message + * + * @return Pitch angle (rad) + */ +static inline float mavlink_msg_hil_state_get_pitch(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float))[3]; + return (float)r.f; +} + +/** + * @brief Get field yaw from hil_state message + * + * @return Yaw angle (rad) + */ +static inline float mavlink_msg_hil_state_get_yaw(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[3]; + return (float)r.f; +} + +/** + * @brief Get field rollspeed from hil_state message + * + * @return Roll angular speed (rad/s) + */ +static inline float mavlink_msg_hil_state_get_rollspeed(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; + return (float)r.f; +} + +/** + * @brief Get field pitchspeed from hil_state message + * + * @return Pitch angular speed (rad/s) + */ +static inline float mavlink_msg_hil_state_get_pitchspeed(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; + return (float)r.f; +} + +/** + * @brief Get field yawspeed from hil_state message + * + * @return Yaw angular speed (rad/s) + */ +static inline float mavlink_msg_hil_state_get_yawspeed(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; + return (float)r.f; +} + +/** + * @brief Get field lat from hil_state message + * + * @return Latitude, expressed as * 1E7 + */ +static inline int32_t mavlink_msg_hil_state_get_lat(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; + return (int32_t)r.i; +} + +/** + * @brief Get field lon from hil_state message + * + * @return Longitude, expressed as * 1E7 + */ +static inline int32_t mavlink_msg_hil_state_get_lon(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t))[0]; + r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t))[1]; + r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t))[2]; + r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t))[3]; + return (int32_t)r.i; +} + +/** + * @brief Get field alt from hil_state message + * + * @return Altitude in meters, expressed as * 1000 (millimeters) + */ +static inline int32_t mavlink_msg_hil_state_get_alt(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t))[0]; + r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t))[1]; + r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t))[2]; + r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t))[3]; + return (int32_t)r.i; +} + +/** + * @brief Get field vx from hil_state message + * + * @return Ground X Speed (Latitude), expressed as m/s * 100 + */ +static inline int16_t mavlink_msg_hil_state_get_vx(const mavlink_message_t* msg) +{ + generic_16bit r; + r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t))[0]; + r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t))[1]; + return (int16_t)r.s; +} + +/** + * @brief Get field vy from hil_state message + * + * @return Ground Y Speed (Longitude), expressed as m/s * 100 + */ +static inline int16_t mavlink_msg_hil_state_get_vy(const mavlink_message_t* msg) +{ + generic_16bit r; + r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t))[0]; + r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t))[1]; + return (int16_t)r.s; +} + +/** + * @brief Get field vz from hil_state message + * + * @return Ground Z Speed (Altitude), expressed as m/s * 100 + */ +static inline int16_t mavlink_msg_hil_state_get_vz(const mavlink_message_t* msg) +{ + generic_16bit r; + r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t))[0]; + r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t))[1]; + return (int16_t)r.s; +} + +/** + * @brief Get field xacc from hil_state message + * + * @return X acceleration (mg) + */ +static inline int16_t mavlink_msg_hil_state_get_xacc(const mavlink_message_t* msg) +{ + generic_16bit r; + r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; + r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; + return (int16_t)r.s; +} + +/** + * @brief Get field yacc from hil_state message + * + * @return Y acceleration (mg) + */ +static inline int16_t mavlink_msg_hil_state_get_yacc(const mavlink_message_t* msg) +{ + generic_16bit r; + r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; + r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; + return (int16_t)r.s; +} + +/** + * @brief Get field zacc from hil_state message + * + * @return Z acceleration (mg) + */ +static inline int16_t mavlink_msg_hil_state_get_zacc(const mavlink_message_t* msg) +{ + generic_16bit r; + r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; + r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; + return (int16_t)r.s; +} + +/** + * @brief Decode a hil_state message into a struct + * + * @param msg The message to decode + * @param hil_state C-struct to decode the message contents into + */ +static inline void mavlink_msg_hil_state_decode(const mavlink_message_t* msg, mavlink_hil_state_t* hil_state) +{ + hil_state->usec = mavlink_msg_hil_state_get_usec(msg); + hil_state->roll = mavlink_msg_hil_state_get_roll(msg); + hil_state->pitch = mavlink_msg_hil_state_get_pitch(msg); + hil_state->yaw = mavlink_msg_hil_state_get_yaw(msg); + hil_state->rollspeed = mavlink_msg_hil_state_get_rollspeed(msg); + hil_state->pitchspeed = mavlink_msg_hil_state_get_pitchspeed(msg); + hil_state->yawspeed = mavlink_msg_hil_state_get_yawspeed(msg); + hil_state->lat = mavlink_msg_hil_state_get_lat(msg); + hil_state->lon = mavlink_msg_hil_state_get_lon(msg); + hil_state->alt = mavlink_msg_hil_state_get_alt(msg); + hil_state->vx = mavlink_msg_hil_state_get_vx(msg); + hil_state->vy = mavlink_msg_hil_state_get_vy(msg); + hil_state->vz = mavlink_msg_hil_state_get_vz(msg); + hil_state->xacc = mavlink_msg_hil_state_get_xacc(msg); + hil_state->yacc = mavlink_msg_hil_state_get_yacc(msg); + hil_state->zacc = mavlink_msg_hil_state_get_zacc(msg); +} diff --git a/thirdParty/mavlink/include/common/mavlink_msg_object_detection_event.h b/thirdParty/mavlink/include/common/mavlink_msg_object_detection_event.h new file mode 100644 index 0000000000000000000000000000000000000000..bd45e811ef22de3f4a3e4dbd1de13bcf3d0bf142 --- /dev/null +++ b/thirdParty/mavlink/include/common/mavlink_msg_object_detection_event.h @@ -0,0 +1,224 @@ +// MESSAGE OBJECT_DETECTION_EVENT PACKING + +#define MAVLINK_MSG_ID_OBJECT_DETECTION_EVENT 140 + +typedef struct __mavlink_object_detection_event_t +{ + uint32_t time; ///< Timestamp in milliseconds since system boot + uint16_t object_id; ///< Object ID + uint8_t type; ///< Object type: 0: image, 1: letter, 2: ground vehicle, 3: air vehicle, 4: surface vehicle, 5: sub-surface vehicle, 6: human, 7: animal + char name[20]; ///< Name of the object as defined by the detector + uint8_t quality; ///< Detection quality / confidence. 0: bad, 255: maximum confidence + float bearing; ///< Angle of the object with respect to the body frame in NED coordinates in radians. 0: front + float distance; ///< Ground distance in meters + +} mavlink_object_detection_event_t; + +#define MAVLINK_MSG_OBJECT_DETECTION_EVENT_FIELD_NAME_LEN 20 + + +/** + * @brief Pack a object_detection_event message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time Timestamp in milliseconds since system boot + * @param object_id Object ID + * @param type Object type: 0: image, 1: letter, 2: ground vehicle, 3: air vehicle, 4: surface vehicle, 5: sub-surface vehicle, 6: human, 7: animal + * @param name Name of the object as defined by the detector + * @param quality Detection quality / confidence. 0: bad, 255: maximum confidence + * @param bearing Angle of the object with respect to the body frame in NED coordinates in radians. 0: front + * @param distance Ground distance in meters + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_object_detection_event_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint32_t time, uint16_t object_id, uint8_t type, const char* name, uint8_t quality, float bearing, float distance) +{ + uint16_t i = 0; + msg->msgid = MAVLINK_MSG_ID_OBJECT_DETECTION_EVENT; + + i += put_uint32_t_by_index(time, i, msg->payload); // Timestamp in milliseconds since system boot + i += put_uint16_t_by_index(object_id, i, msg->payload); // Object ID + i += put_uint8_t_by_index(type, i, msg->payload); // Object type: 0: image, 1: letter, 2: ground vehicle, 3: air vehicle, 4: surface vehicle, 5: sub-surface vehicle, 6: human, 7: animal + i += put_array_by_index((const int8_t*)name, sizeof(char)*20, i, msg->payload); // Name of the object as defined by the detector + i += put_uint8_t_by_index(quality, i, msg->payload); // Detection quality / confidence. 0: bad, 255: maximum confidence + i += put_float_by_index(bearing, i, msg->payload); // Angle of the object with respect to the body frame in NED coordinates in radians. 0: front + i += put_float_by_index(distance, i, msg->payload); // Ground distance in meters + + return mavlink_finalize_message(msg, system_id, component_id, i); +} + +/** + * @brief Pack a object_detection_event message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param time Timestamp in milliseconds since system boot + * @param object_id Object ID + * @param type Object type: 0: image, 1: letter, 2: ground vehicle, 3: air vehicle, 4: surface vehicle, 5: sub-surface vehicle, 6: human, 7: animal + * @param name Name of the object as defined by the detector + * @param quality Detection quality / confidence. 0: bad, 255: maximum confidence + * @param bearing Angle of the object with respect to the body frame in NED coordinates in radians. 0: front + * @param distance Ground distance in meters + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_object_detection_event_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint32_t time, uint16_t object_id, uint8_t type, const char* name, uint8_t quality, float bearing, float distance) +{ + uint16_t i = 0; + msg->msgid = MAVLINK_MSG_ID_OBJECT_DETECTION_EVENT; + + i += put_uint32_t_by_index(time, i, msg->payload); // Timestamp in milliseconds since system boot + i += put_uint16_t_by_index(object_id, i, msg->payload); // Object ID + i += put_uint8_t_by_index(type, i, msg->payload); // Object type: 0: image, 1: letter, 2: ground vehicle, 3: air vehicle, 4: surface vehicle, 5: sub-surface vehicle, 6: human, 7: animal + i += put_array_by_index((const int8_t*)name, sizeof(char)*20, i, msg->payload); // Name of the object as defined by the detector + i += put_uint8_t_by_index(quality, i, msg->payload); // Detection quality / confidence. 0: bad, 255: maximum confidence + i += put_float_by_index(bearing, i, msg->payload); // Angle of the object with respect to the body frame in NED coordinates in radians. 0: front + i += put_float_by_index(distance, i, msg->payload); // Ground distance in meters + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); +} + +/** + * @brief Encode a object_detection_event struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param object_detection_event C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_object_detection_event_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_object_detection_event_t* object_detection_event) +{ + return mavlink_msg_object_detection_event_pack(system_id, component_id, msg, object_detection_event->time, object_detection_event->object_id, object_detection_event->type, object_detection_event->name, object_detection_event->quality, object_detection_event->bearing, object_detection_event->distance); +} + +/** + * @brief Send a object_detection_event message + * @param chan MAVLink channel to send the message + * + * @param time Timestamp in milliseconds since system boot + * @param object_id Object ID + * @param type Object type: 0: image, 1: letter, 2: ground vehicle, 3: air vehicle, 4: surface vehicle, 5: sub-surface vehicle, 6: human, 7: animal + * @param name Name of the object as defined by the detector + * @param quality Detection quality / confidence. 0: bad, 255: maximum confidence + * @param bearing Angle of the object with respect to the body frame in NED coordinates in radians. 0: front + * @param distance Ground distance in meters + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_object_detection_event_send(mavlink_channel_t chan, uint32_t time, uint16_t object_id, uint8_t type, const char* name, uint8_t quality, float bearing, float distance) +{ + mavlink_message_t msg; + mavlink_msg_object_detection_event_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, time, object_id, type, name, quality, bearing, distance); + mavlink_send_uart(chan, &msg); +} + +#endif +// MESSAGE OBJECT_DETECTION_EVENT UNPACKING + +/** + * @brief Get field time from object_detection_event message + * + * @return Timestamp in milliseconds since system boot + */ +static inline uint32_t mavlink_msg_object_detection_event_get_time(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload)[0]; + r.b[2] = (msg->payload)[1]; + r.b[1] = (msg->payload)[2]; + r.b[0] = (msg->payload)[3]; + return (uint32_t)r.i; +} + +/** + * @brief Get field object_id from object_detection_event message + * + * @return Object ID + */ +static inline uint16_t mavlink_msg_object_detection_event_get_object_id(const mavlink_message_t* msg) +{ + generic_16bit r; + r.b[1] = (msg->payload+sizeof(uint32_t))[0]; + r.b[0] = (msg->payload+sizeof(uint32_t))[1]; + return (uint16_t)r.s; +} + +/** + * @brief Get field type from object_detection_event message + * + * @return Object type: 0: image, 1: letter, 2: ground vehicle, 3: air vehicle, 4: surface vehicle, 5: sub-surface vehicle, 6: human, 7: animal + */ +static inline uint8_t mavlink_msg_object_detection_event_get_type(const mavlink_message_t* msg) +{ + return (uint8_t)(msg->payload+sizeof(uint32_t)+sizeof(uint16_t))[0]; +} + +/** + * @brief Get field name from object_detection_event message + * + * @return Name of the object as defined by the detector + */ +static inline uint16_t mavlink_msg_object_detection_event_get_name(const mavlink_message_t* msg, char* r_data) +{ + + memcpy(r_data, msg->payload+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint8_t), sizeof(char)*20); + return sizeof(char)*20; +} + +/** + * @brief Get field quality from object_detection_event message + * + * @return Detection quality / confidence. 0: bad, 255: maximum confidence + */ +static inline uint8_t mavlink_msg_object_detection_event_get_quality(const mavlink_message_t* msg) +{ + return (uint8_t)(msg->payload+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(char)*20)[0]; +} + +/** + * @brief Get field bearing from object_detection_event message + * + * @return Angle of the object with respect to the body frame in NED coordinates in radians. 0: front + */ +static inline float mavlink_msg_object_detection_event_get_bearing(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(char)*20+sizeof(uint8_t))[0]; + r.b[2] = (msg->payload+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(char)*20+sizeof(uint8_t))[1]; + r.b[1] = (msg->payload+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(char)*20+sizeof(uint8_t))[2]; + r.b[0] = (msg->payload+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(char)*20+sizeof(uint8_t))[3]; + return (float)r.f; +} + +/** + * @brief Get field distance from object_detection_event message + * + * @return Ground distance in meters + */ +static inline float mavlink_msg_object_detection_event_get_distance(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(char)*20+sizeof(uint8_t)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(char)*20+sizeof(uint8_t)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(char)*20+sizeof(uint8_t)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(char)*20+sizeof(uint8_t)+sizeof(float))[3]; + return (float)r.f; +} + +/** + * @brief Decode a object_detection_event message into a struct + * + * @param msg The message to decode + * @param object_detection_event C-struct to decode the message contents into + */ +static inline void mavlink_msg_object_detection_event_decode(const mavlink_message_t* msg, mavlink_object_detection_event_t* object_detection_event) +{ + object_detection_event->time = mavlink_msg_object_detection_event_get_time(msg); + object_detection_event->object_id = mavlink_msg_object_detection_event_get_object_id(msg); + object_detection_event->type = mavlink_msg_object_detection_event_get_type(msg); + mavlink_msg_object_detection_event_get_name(msg, object_detection_event->name); + object_detection_event->quality = mavlink_msg_object_detection_event_get_quality(msg); + object_detection_event->bearing = mavlink_msg_object_detection_event_get_bearing(msg); + object_detection_event->distance = mavlink_msg_object_detection_event_get_distance(msg); +} diff --git a/thirdParty/mavlink/include/common/mavlink_msg_optical_flow.h b/thirdParty/mavlink/include/common/mavlink_msg_optical_flow.h new file mode 100644 index 0000000000000000000000000000000000000000..d0acc22ddc39b29155dcbb9ed59aa8586d794311 --- /dev/null +++ b/thirdParty/mavlink/include/common/mavlink_msg_optical_flow.h @@ -0,0 +1,206 @@ +// MESSAGE OPTICAL_FLOW PACKING + +#define MAVLINK_MSG_ID_OPTICAL_FLOW 100 + +typedef struct __mavlink_optical_flow_t +{ + uint64_t time; ///< Timestamp (UNIX) + uint8_t sensor_id; ///< Sensor ID + int16_t flow_x; ///< Flow in pixels in x-sensor direction + int16_t flow_y; ///< Flow in pixels in y-sensor direction + uint8_t quality; ///< Optical flow quality / confidence. 0: bad, 255: maximum quality + float ground_distance; ///< Ground distance in meters + +} mavlink_optical_flow_t; + + + +/** + * @brief Pack a optical_flow message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time Timestamp (UNIX) + * @param sensor_id Sensor ID + * @param flow_x Flow in pixels in x-sensor direction + * @param flow_y Flow in pixels in y-sensor direction + * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality + * @param ground_distance Ground distance in meters + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_optical_flow_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t time, uint8_t sensor_id, int16_t flow_x, int16_t flow_y, uint8_t quality, float ground_distance) +{ + uint16_t i = 0; + msg->msgid = MAVLINK_MSG_ID_OPTICAL_FLOW; + + i += put_uint64_t_by_index(time, i, msg->payload); // Timestamp (UNIX) + i += put_uint8_t_by_index(sensor_id, i, msg->payload); // Sensor ID + i += put_int16_t_by_index(flow_x, i, msg->payload); // Flow in pixels in x-sensor direction + i += put_int16_t_by_index(flow_y, i, msg->payload); // Flow in pixels in y-sensor direction + i += put_uint8_t_by_index(quality, i, msg->payload); // Optical flow quality / confidence. 0: bad, 255: maximum quality + i += put_float_by_index(ground_distance, i, msg->payload); // Ground distance in meters + + return mavlink_finalize_message(msg, system_id, component_id, i); +} + +/** + * @brief Pack a optical_flow message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param time Timestamp (UNIX) + * @param sensor_id Sensor ID + * @param flow_x Flow in pixels in x-sensor direction + * @param flow_y Flow in pixels in y-sensor direction + * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality + * @param ground_distance Ground distance in meters + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_optical_flow_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t time, uint8_t sensor_id, int16_t flow_x, int16_t flow_y, uint8_t quality, float ground_distance) +{ + uint16_t i = 0; + msg->msgid = MAVLINK_MSG_ID_OPTICAL_FLOW; + + i += put_uint64_t_by_index(time, i, msg->payload); // Timestamp (UNIX) + i += put_uint8_t_by_index(sensor_id, i, msg->payload); // Sensor ID + i += put_int16_t_by_index(flow_x, i, msg->payload); // Flow in pixels in x-sensor direction + i += put_int16_t_by_index(flow_y, i, msg->payload); // Flow in pixels in y-sensor direction + i += put_uint8_t_by_index(quality, i, msg->payload); // Optical flow quality / confidence. 0: bad, 255: maximum quality + i += put_float_by_index(ground_distance, i, msg->payload); // Ground distance in meters + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); +} + +/** + * @brief Encode a optical_flow struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param optical_flow C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_optical_flow_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_optical_flow_t* optical_flow) +{ + return mavlink_msg_optical_flow_pack(system_id, component_id, msg, optical_flow->time, optical_flow->sensor_id, optical_flow->flow_x, optical_flow->flow_y, optical_flow->quality, optical_flow->ground_distance); +} + +/** + * @brief Send a optical_flow message + * @param chan MAVLink channel to send the message + * + * @param time Timestamp (UNIX) + * @param sensor_id Sensor ID + * @param flow_x Flow in pixels in x-sensor direction + * @param flow_y Flow in pixels in y-sensor direction + * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality + * @param ground_distance Ground distance in meters + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_optical_flow_send(mavlink_channel_t chan, uint64_t time, uint8_t sensor_id, int16_t flow_x, int16_t flow_y, uint8_t quality, float ground_distance) +{ + mavlink_message_t msg; + mavlink_msg_optical_flow_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, time, sensor_id, flow_x, flow_y, quality, ground_distance); + mavlink_send_uart(chan, &msg); +} + +#endif +// MESSAGE OPTICAL_FLOW UNPACKING + +/** + * @brief Get field time from optical_flow message + * + * @return Timestamp (UNIX) + */ +static inline uint64_t mavlink_msg_optical_flow_get_time(const mavlink_message_t* msg) +{ + generic_64bit r; + r.b[7] = (msg->payload)[0]; + r.b[6] = (msg->payload)[1]; + r.b[5] = (msg->payload)[2]; + r.b[4] = (msg->payload)[3]; + r.b[3] = (msg->payload)[4]; + r.b[2] = (msg->payload)[5]; + r.b[1] = (msg->payload)[6]; + r.b[0] = (msg->payload)[7]; + return (uint64_t)r.ll; +} + +/** + * @brief Get field sensor_id from optical_flow message + * + * @return Sensor ID + */ +static inline uint8_t mavlink_msg_optical_flow_get_sensor_id(const mavlink_message_t* msg) +{ + return (uint8_t)(msg->payload+sizeof(uint64_t))[0]; +} + +/** + * @brief Get field flow_x from optical_flow message + * + * @return Flow in pixels in x-sensor direction + */ +static inline int16_t mavlink_msg_optical_flow_get_flow_x(const mavlink_message_t* msg) +{ + generic_16bit r; + r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[0]; + r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[1]; + return (int16_t)r.s; +} + +/** + * @brief Get field flow_y from optical_flow message + * + * @return Flow in pixels in y-sensor direction + */ +static inline int16_t mavlink_msg_optical_flow_get_flow_y(const mavlink_message_t* msg) +{ + generic_16bit r; + r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int16_t))[0]; + r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int16_t))[1]; + return (int16_t)r.s; +} + +/** + * @brief Get field quality from optical_flow message + * + * @return Optical flow quality / confidence. 0: bad, 255: maximum quality + */ +static inline uint8_t mavlink_msg_optical_flow_get_quality(const mavlink_message_t* msg) +{ + return (uint8_t)(msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int16_t)+sizeof(int16_t))[0]; +} + +/** + * @brief Get field ground_distance from optical_flow message + * + * @return Ground distance in meters + */ +static inline float mavlink_msg_optical_flow_get_ground_distance(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(uint8_t))[0]; + r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(uint8_t))[1]; + r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(uint8_t))[2]; + r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(uint8_t))[3]; + return (float)r.f; +} + +/** + * @brief Decode a optical_flow message into a struct + * + * @param msg The message to decode + * @param optical_flow C-struct to decode the message contents into + */ +static inline void mavlink_msg_optical_flow_decode(const mavlink_message_t* msg, mavlink_optical_flow_t* optical_flow) +{ + optical_flow->time = mavlink_msg_optical_flow_get_time(msg); + optical_flow->sensor_id = mavlink_msg_optical_flow_get_sensor_id(msg); + optical_flow->flow_x = mavlink_msg_optical_flow_get_flow_x(msg); + optical_flow->flow_y = mavlink_msg_optical_flow_get_flow_y(msg); + optical_flow->quality = mavlink_msg_optical_flow_get_quality(msg); + optical_flow->ground_distance = mavlink_msg_optical_flow_get_ground_distance(msg); +} diff --git a/thirdParty/mavlink/include/common/mavlink_msg_position_controller_output.h b/thirdParty/mavlink/include/common/mavlink_msg_position_controller_output.h deleted file mode 100644 index 389d9a2387a8b1b34fc02b5620811d42513af08a..0000000000000000000000000000000000000000 --- a/thirdParty/mavlink/include/common/mavlink_msg_position_controller_output.h +++ /dev/null @@ -1,169 +0,0 @@ -// MESSAGE POSITION_CONTROLLER_OUTPUT PACKING - -#define MAVLINK_MSG_ID_POSITION_CONTROLLER_OUTPUT 61 - -typedef struct __mavlink_position_controller_output_t -{ - uint8_t enabled; ///< 1: enabled, 0: disabled - int8_t x; ///< Position x: -128: -100%, 127: +100% - int8_t y; ///< Position y: -128: -100%, 127: +100% - int8_t z; ///< Position z: -128: -100%, 127: +100% - int8_t yaw; ///< Position yaw: -128: -100%, 127: +100% - -} mavlink_position_controller_output_t; - - - -/** - * @brief Pack a position_controller_output message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param enabled 1: enabled, 0: disabled - * @param x Position x: -128: -100%, 127: +100% - * @param y Position y: -128: -100%, 127: +100% - * @param z Position z: -128: -100%, 127: +100% - * @param yaw Position yaw: -128: -100%, 127: +100% - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_position_controller_output_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t enabled, int8_t x, int8_t y, int8_t z, int8_t yaw) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROLLER_OUTPUT; - - i += put_uint8_t_by_index(enabled, i, msg->payload); // 1: enabled, 0: disabled - i += put_int8_t_by_index(x, i, msg->payload); // Position x: -128: -100%, 127: +100% - i += put_int8_t_by_index(y, i, msg->payload); // Position y: -128: -100%, 127: +100% - i += put_int8_t_by_index(z, i, msg->payload); // Position z: -128: -100%, 127: +100% - i += put_int8_t_by_index(yaw, i, msg->payload); // Position yaw: -128: -100%, 127: +100% - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -/** - * @brief Pack a position_controller_output message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param enabled 1: enabled, 0: disabled - * @param x Position x: -128: -100%, 127: +100% - * @param y Position y: -128: -100%, 127: +100% - * @param z Position z: -128: -100%, 127: +100% - * @param yaw Position yaw: -128: -100%, 127: +100% - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_position_controller_output_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t enabled, int8_t x, int8_t y, int8_t z, int8_t yaw) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROLLER_OUTPUT; - - i += put_uint8_t_by_index(enabled, i, msg->payload); // 1: enabled, 0: disabled - i += put_int8_t_by_index(x, i, msg->payload); // Position x: -128: -100%, 127: +100% - i += put_int8_t_by_index(y, i, msg->payload); // Position y: -128: -100%, 127: +100% - i += put_int8_t_by_index(z, i, msg->payload); // Position z: -128: -100%, 127: +100% - i += put_int8_t_by_index(yaw, i, msg->payload); // Position yaw: -128: -100%, 127: +100% - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); -} - -/** - * @brief Encode a position_controller_output struct into a message - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param position_controller_output C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_position_controller_output_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_position_controller_output_t* position_controller_output) -{ - return mavlink_msg_position_controller_output_pack(system_id, component_id, msg, position_controller_output->enabled, position_controller_output->x, position_controller_output->y, position_controller_output->z, position_controller_output->yaw); -} - -/** - * @brief Send a position_controller_output message - * @param chan MAVLink channel to send the message - * - * @param enabled 1: enabled, 0: disabled - * @param x Position x: -128: -100%, 127: +100% - * @param y Position y: -128: -100%, 127: +100% - * @param z Position z: -128: -100%, 127: +100% - * @param yaw Position yaw: -128: -100%, 127: +100% - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_position_controller_output_send(mavlink_channel_t chan, uint8_t enabled, int8_t x, int8_t y, int8_t z, int8_t yaw) -{ - mavlink_message_t msg; - mavlink_msg_position_controller_output_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, enabled, x, y, z, yaw); - mavlink_send_uart(chan, &msg); -} - -#endif -// MESSAGE POSITION_CONTROLLER_OUTPUT UNPACKING - -/** - * @brief Get field enabled from position_controller_output message - * - * @return 1: enabled, 0: disabled - */ -static inline uint8_t mavlink_msg_position_controller_output_get_enabled(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload)[0]; -} - -/** - * @brief Get field x from position_controller_output message - * - * @return Position x: -128: -100%, 127: +100% - */ -static inline int8_t mavlink_msg_position_controller_output_get_x(const mavlink_message_t* msg) -{ - return (int8_t)(msg->payload+sizeof(uint8_t))[0]; -} - -/** - * @brief Get field y from position_controller_output message - * - * @return Position y: -128: -100%, 127: +100% - */ -static inline int8_t mavlink_msg_position_controller_output_get_y(const mavlink_message_t* msg) -{ - return (int8_t)(msg->payload+sizeof(uint8_t)+sizeof(int8_t))[0]; -} - -/** - * @brief Get field z from position_controller_output message - * - * @return Position z: -128: -100%, 127: +100% - */ -static inline int8_t mavlink_msg_position_controller_output_get_z(const mavlink_message_t* msg) -{ - return (int8_t)(msg->payload+sizeof(uint8_t)+sizeof(int8_t)+sizeof(int8_t))[0]; -} - -/** - * @brief Get field yaw from position_controller_output message - * - * @return Position yaw: -128: -100%, 127: +100% - */ -static inline int8_t mavlink_msg_position_controller_output_get_yaw(const mavlink_message_t* msg) -{ - return (int8_t)(msg->payload+sizeof(uint8_t)+sizeof(int8_t)+sizeof(int8_t)+sizeof(int8_t))[0]; -} - -/** - * @brief Decode a position_controller_output message into a struct - * - * @param msg The message to decode - * @param position_controller_output C-struct to decode the message contents into - */ -static inline void mavlink_msg_position_controller_output_decode(const mavlink_message_t* msg, mavlink_position_controller_output_t* position_controller_output) -{ - position_controller_output->enabled = mavlink_msg_position_controller_output_get_enabled(msg); - position_controller_output->x = mavlink_msg_position_controller_output_get_x(msg); - position_controller_output->y = mavlink_msg_position_controller_output_get_y(msg); - position_controller_output->z = mavlink_msg_position_controller_output_get_z(msg); - position_controller_output->yaw = mavlink_msg_position_controller_output_get_yaw(msg); -} diff --git a/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_override.h b/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_override.h new file mode 100644 index 0000000000000000000000000000000000000000..7595857a5e0cfb6f14c61817b24aedbec08e185d --- /dev/null +++ b/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_override.h @@ -0,0 +1,278 @@ +// MESSAGE RC_CHANNELS_OVERRIDE PACKING + +#define MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE 70 + +typedef struct __mavlink_rc_channels_override_t +{ + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + uint16_t chan1_raw; ///< RC channel 1 value, in microseconds + uint16_t chan2_raw; ///< RC channel 2 value, in microseconds + uint16_t chan3_raw; ///< RC channel 3 value, in microseconds + uint16_t chan4_raw; ///< RC channel 4 value, in microseconds + uint16_t chan5_raw; ///< RC channel 5 value, in microseconds + uint16_t chan6_raw; ///< RC channel 6 value, in microseconds + uint16_t chan7_raw; ///< RC channel 7 value, in microseconds + uint16_t chan8_raw; ///< RC channel 8 value, in microseconds + +} mavlink_rc_channels_override_t; + + + +/** + * @brief Pack a rc_channels_override message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param chan1_raw RC channel 1 value, in microseconds + * @param chan2_raw RC channel 2 value, in microseconds + * @param chan3_raw RC channel 3 value, in microseconds + * @param chan4_raw RC channel 4 value, in microseconds + * @param chan5_raw RC channel 5 value, in microseconds + * @param chan6_raw RC channel 6 value, in microseconds + * @param chan7_raw RC channel 7 value, in microseconds + * @param chan8_raw RC channel 8 value, in microseconds + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_rc_channels_override_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw) +{ + uint16_t i = 0; + msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE; + + i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID + i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID + i += put_uint16_t_by_index(chan1_raw, i, msg->payload); // RC channel 1 value, in microseconds + i += put_uint16_t_by_index(chan2_raw, i, msg->payload); // RC channel 2 value, in microseconds + i += put_uint16_t_by_index(chan3_raw, i, msg->payload); // RC channel 3 value, in microseconds + i += put_uint16_t_by_index(chan4_raw, i, msg->payload); // RC channel 4 value, in microseconds + i += put_uint16_t_by_index(chan5_raw, i, msg->payload); // RC channel 5 value, in microseconds + i += put_uint16_t_by_index(chan6_raw, i, msg->payload); // RC channel 6 value, in microseconds + i += put_uint16_t_by_index(chan7_raw, i, msg->payload); // RC channel 7 value, in microseconds + i += put_uint16_t_by_index(chan8_raw, i, msg->payload); // RC channel 8 value, in microseconds + + return mavlink_finalize_message(msg, system_id, component_id, i); +} + +/** + * @brief Pack a rc_channels_override message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param chan1_raw RC channel 1 value, in microseconds + * @param chan2_raw RC channel 2 value, in microseconds + * @param chan3_raw RC channel 3 value, in microseconds + * @param chan4_raw RC channel 4 value, in microseconds + * @param chan5_raw RC channel 5 value, in microseconds + * @param chan6_raw RC channel 6 value, in microseconds + * @param chan7_raw RC channel 7 value, in microseconds + * @param chan8_raw RC channel 8 value, in microseconds + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_rc_channels_override_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw) +{ + uint16_t i = 0; + msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE; + + i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID + i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID + i += put_uint16_t_by_index(chan1_raw, i, msg->payload); // RC channel 1 value, in microseconds + i += put_uint16_t_by_index(chan2_raw, i, msg->payload); // RC channel 2 value, in microseconds + i += put_uint16_t_by_index(chan3_raw, i, msg->payload); // RC channel 3 value, in microseconds + i += put_uint16_t_by_index(chan4_raw, i, msg->payload); // RC channel 4 value, in microseconds + i += put_uint16_t_by_index(chan5_raw, i, msg->payload); // RC channel 5 value, in microseconds + i += put_uint16_t_by_index(chan6_raw, i, msg->payload); // RC channel 6 value, in microseconds + i += put_uint16_t_by_index(chan7_raw, i, msg->payload); // RC channel 7 value, in microseconds + i += put_uint16_t_by_index(chan8_raw, i, msg->payload); // RC channel 8 value, in microseconds + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); +} + +/** + * @brief Encode a rc_channels_override struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param rc_channels_override C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_rc_channels_override_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_rc_channels_override_t* rc_channels_override) +{ + return mavlink_msg_rc_channels_override_pack(system_id, component_id, msg, rc_channels_override->target_system, rc_channels_override->target_component, rc_channels_override->chan1_raw, rc_channels_override->chan2_raw, rc_channels_override->chan3_raw, rc_channels_override->chan4_raw, rc_channels_override->chan5_raw, rc_channels_override->chan6_raw, rc_channels_override->chan7_raw, rc_channels_override->chan8_raw); +} + +/** + * @brief Send a rc_channels_override message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param chan1_raw RC channel 1 value, in microseconds + * @param chan2_raw RC channel 2 value, in microseconds + * @param chan3_raw RC channel 3 value, in microseconds + * @param chan4_raw RC channel 4 value, in microseconds + * @param chan5_raw RC channel 5 value, in microseconds + * @param chan6_raw RC channel 6 value, in microseconds + * @param chan7_raw RC channel 7 value, in microseconds + * @param chan8_raw RC channel 8 value, in microseconds + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_rc_channels_override_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw) +{ + mavlink_message_t msg; + mavlink_msg_rc_channels_override_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw); + mavlink_send_uart(chan, &msg); +} + +#endif +// MESSAGE RC_CHANNELS_OVERRIDE UNPACKING + +/** + * @brief Get field target_system from rc_channels_override message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_rc_channels_override_get_target_system(const mavlink_message_t* msg) +{ + return (uint8_t)(msg->payload)[0]; +} + +/** + * @brief Get field target_component from rc_channels_override message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_rc_channels_override_get_target_component(const mavlink_message_t* msg) +{ + return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; +} + +/** + * @brief Get field chan1_raw from rc_channels_override message + * + * @return RC channel 1 value, in microseconds + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan1_raw(const mavlink_message_t* msg) +{ + generic_16bit r; + r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; + r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[1]; + return (uint16_t)r.s; +} + +/** + * @brief Get field chan2_raw from rc_channels_override message + * + * @return RC channel 2 value, in microseconds + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan2_raw(const mavlink_message_t* msg) +{ + generic_16bit r; + r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[0]; + r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[1]; + return (uint16_t)r.s; +} + +/** + * @brief Get field chan3_raw from rc_channels_override message + * + * @return RC channel 3 value, in microseconds + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan3_raw(const mavlink_message_t* msg) +{ + generic_16bit r; + r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; + r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; + return (uint16_t)r.s; +} + +/** + * @brief Get field chan4_raw from rc_channels_override message + * + * @return RC channel 4 value, in microseconds + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan4_raw(const mavlink_message_t* msg) +{ + generic_16bit r; + r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; + r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; + return (uint16_t)r.s; +} + +/** + * @brief Get field chan5_raw from rc_channels_override message + * + * @return RC channel 5 value, in microseconds + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan5_raw(const mavlink_message_t* msg) +{ + generic_16bit r; + r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; + r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; + return (uint16_t)r.s; +} + +/** + * @brief Get field chan6_raw from rc_channels_override message + * + * @return RC channel 6 value, in microseconds + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan6_raw(const mavlink_message_t* msg) +{ + generic_16bit r; + r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; + r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; + return (uint16_t)r.s; +} + +/** + * @brief Get field chan7_raw from rc_channels_override message + * + * @return RC channel 7 value, in microseconds + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan7_raw(const mavlink_message_t* msg) +{ + generic_16bit r; + r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; + r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; + return (uint16_t)r.s; +} + +/** + * @brief Get field chan8_raw from rc_channels_override message + * + * @return RC channel 8 value, in microseconds + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan8_raw(const mavlink_message_t* msg) +{ + generic_16bit r; + r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; + r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; + return (uint16_t)r.s; +} + +/** + * @brief Decode a rc_channels_override message into a struct + * + * @param msg The message to decode + * @param rc_channels_override C-struct to decode the message contents into + */ +static inline void mavlink_msg_rc_channels_override_decode(const mavlink_message_t* msg, mavlink_rc_channels_override_t* rc_channels_override) +{ + rc_channels_override->target_system = mavlink_msg_rc_channels_override_get_target_system(msg); + rc_channels_override->target_component = mavlink_msg_rc_channels_override_get_target_component(msg); + rc_channels_override->chan1_raw = mavlink_msg_rc_channels_override_get_chan1_raw(msg); + rc_channels_override->chan2_raw = mavlink_msg_rc_channels_override_get_chan2_raw(msg); + rc_channels_override->chan3_raw = mavlink_msg_rc_channels_override_get_chan3_raw(msg); + rc_channels_override->chan4_raw = mavlink_msg_rc_channels_override_get_chan4_raw(msg); + rc_channels_override->chan5_raw = mavlink_msg_rc_channels_override_get_chan5_raw(msg); + rc_channels_override->chan6_raw = mavlink_msg_rc_channels_override_get_chan6_raw(msg); + rc_channels_override->chan7_raw = mavlink_msg_rc_channels_override_get_chan7_raw(msg); + rc_channels_override->chan8_raw = mavlink_msg_rc_channels_override_get_chan8_raw(msg); +} diff --git a/thirdParty/mavlink/include/common/mavlink_msg_request_data_stream.h b/thirdParty/mavlink/include/common/mavlink_msg_request_data_stream.h index f390e4e370c69002aac857b86f5dbec9bbc716f8..15ee1a6a5fa39867628fe87ea9b3dd09b28f0d84 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_request_data_stream.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_request_data_stream.h @@ -7,7 +7,7 @@ typedef struct __mavlink_request_data_stream_t uint8_t target_system; ///< The target requested to send the message stream. uint8_t target_component; ///< The target requested to send the message stream. uint8_t req_stream_id; ///< The ID of the requested message type - uint16_t req_message_rate; ///< The requested interval between two messages of this type + uint16_t req_message_rate; ///< Update rate in Hertz uint8_t start_stop; ///< 1 to start sending, 0 to stop sending. } mavlink_request_data_stream_t; @@ -23,7 +23,7 @@ typedef struct __mavlink_request_data_stream_t * @param target_system The target requested to send the message stream. * @param target_component The target requested to send the message stream. * @param req_stream_id The ID of the requested message type - * @param req_message_rate The requested interval between two messages of this type + * @param req_message_rate Update rate in Hertz * @param start_stop 1 to start sending, 0 to stop sending. * @return length of the message in bytes (excluding serial stream start sign) */ @@ -35,7 +35,7 @@ static inline uint16_t mavlink_msg_request_data_stream_pack(uint8_t system_id, u i += put_uint8_t_by_index(target_system, i, msg->payload); // The target requested to send the message stream. i += put_uint8_t_by_index(target_component, i, msg->payload); // The target requested to send the message stream. i += put_uint8_t_by_index(req_stream_id, i, msg->payload); // The ID of the requested message type - i += put_uint16_t_by_index(req_message_rate, i, msg->payload); // The requested interval between two messages of this type + i += put_uint16_t_by_index(req_message_rate, i, msg->payload); // Update rate in Hertz i += put_uint8_t_by_index(start_stop, i, msg->payload); // 1 to start sending, 0 to stop sending. return mavlink_finalize_message(msg, system_id, component_id, i); @@ -50,7 +50,7 @@ static inline uint16_t mavlink_msg_request_data_stream_pack(uint8_t system_id, u * @param target_system The target requested to send the message stream. * @param target_component The target requested to send the message stream. * @param req_stream_id The ID of the requested message type - * @param req_message_rate The requested interval between two messages of this type + * @param req_message_rate Update rate in Hertz * @param start_stop 1 to start sending, 0 to stop sending. * @return length of the message in bytes (excluding serial stream start sign) */ @@ -62,7 +62,7 @@ static inline uint16_t mavlink_msg_request_data_stream_pack_chan(uint8_t system_ i += put_uint8_t_by_index(target_system, i, msg->payload); // The target requested to send the message stream. i += put_uint8_t_by_index(target_component, i, msg->payload); // The target requested to send the message stream. i += put_uint8_t_by_index(req_stream_id, i, msg->payload); // The ID of the requested message type - i += put_uint16_t_by_index(req_message_rate, i, msg->payload); // The requested interval between two messages of this type + i += put_uint16_t_by_index(req_message_rate, i, msg->payload); // Update rate in Hertz i += put_uint8_t_by_index(start_stop, i, msg->payload); // 1 to start sending, 0 to stop sending. return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); @@ -88,7 +88,7 @@ static inline uint16_t mavlink_msg_request_data_stream_encode(uint8_t system_id, * @param target_system The target requested to send the message stream. * @param target_component The target requested to send the message stream. * @param req_stream_id The ID of the requested message type - * @param req_message_rate The requested interval between two messages of this type + * @param req_message_rate Update rate in Hertz * @param start_stop 1 to start sending, 0 to stop sending. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS @@ -136,7 +136,7 @@ static inline uint8_t mavlink_msg_request_data_stream_get_req_stream_id(const ma /** * @brief Get field req_message_rate from request_data_stream message * - * @return The requested interval between two messages of this type + * @return Update rate in Hertz */ static inline uint16_t mavlink_msg_request_data_stream_get_req_message_rate(const mavlink_message_t* msg) { diff --git a/thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h b/thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h new file mode 100644 index 0000000000000000000000000000000000000000..ac7a85de3bfe3691f6894af34b8cd23e2da1f8e8 --- /dev/null +++ b/thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h @@ -0,0 +1,198 @@ +// MESSAGE ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT PACKING + +#define MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT 58 + +typedef struct __mavlink_roll_pitch_yaw_speed_thrust_setpoint_t +{ + uint64_t time_us; ///< Timestamp in micro seconds since unix epoch + float roll_speed; ///< Desired roll angular speed in rad/s + float pitch_speed; ///< Desired pitch angular speed in rad/s + float yaw_speed; ///< Desired yaw angular speed in rad/s + float thrust; ///< Collective thrust, normalized to 0 .. 1 + +} mavlink_roll_pitch_yaw_speed_thrust_setpoint_t; + + + +/** + * @brief Pack a roll_pitch_yaw_speed_thrust_setpoint message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_us Timestamp in micro seconds since unix epoch + * @param roll_speed Desired roll angular speed in rad/s + * @param pitch_speed Desired pitch angular speed in rad/s + * @param yaw_speed Desired yaw angular speed in rad/s + * @param thrust Collective thrust, normalized to 0 .. 1 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t time_us, float roll_speed, float pitch_speed, float yaw_speed, float thrust) +{ + uint16_t i = 0; + msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT; + + i += put_uint64_t_by_index(time_us, i, msg->payload); // Timestamp in micro seconds since unix epoch + i += put_float_by_index(roll_speed, i, msg->payload); // Desired roll angular speed in rad/s + i += put_float_by_index(pitch_speed, i, msg->payload); // Desired pitch angular speed in rad/s + i += put_float_by_index(yaw_speed, i, msg->payload); // Desired yaw angular speed in rad/s + i += put_float_by_index(thrust, i, msg->payload); // Collective thrust, normalized to 0 .. 1 + + return mavlink_finalize_message(msg, system_id, component_id, i); +} + +/** + * @brief Pack a roll_pitch_yaw_speed_thrust_setpoint message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param time_us Timestamp in micro seconds since unix epoch + * @param roll_speed Desired roll angular speed in rad/s + * @param pitch_speed Desired pitch angular speed in rad/s + * @param yaw_speed Desired yaw angular speed in rad/s + * @param thrust Collective thrust, normalized to 0 .. 1 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t time_us, float roll_speed, float pitch_speed, float yaw_speed, float thrust) +{ + uint16_t i = 0; + msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT; + + i += put_uint64_t_by_index(time_us, i, msg->payload); // Timestamp in micro seconds since unix epoch + i += put_float_by_index(roll_speed, i, msg->payload); // Desired roll angular speed in rad/s + i += put_float_by_index(pitch_speed, i, msg->payload); // Desired pitch angular speed in rad/s + i += put_float_by_index(yaw_speed, i, msg->payload); // Desired yaw angular speed in rad/s + i += put_float_by_index(thrust, i, msg->payload); // Collective thrust, normalized to 0 .. 1 + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); +} + +/** + * @brief Encode a roll_pitch_yaw_speed_thrust_setpoint struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param roll_pitch_yaw_speed_thrust_setpoint C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_roll_pitch_yaw_speed_thrust_setpoint_t* roll_pitch_yaw_speed_thrust_setpoint) +{ + return mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack(system_id, component_id, msg, roll_pitch_yaw_speed_thrust_setpoint->time_us, roll_pitch_yaw_speed_thrust_setpoint->roll_speed, roll_pitch_yaw_speed_thrust_setpoint->pitch_speed, roll_pitch_yaw_speed_thrust_setpoint->yaw_speed, roll_pitch_yaw_speed_thrust_setpoint->thrust); +} + +/** + * @brief Send a roll_pitch_yaw_speed_thrust_setpoint message + * @param chan MAVLink channel to send the message + * + * @param time_us Timestamp in micro seconds since unix epoch + * @param roll_speed Desired roll angular speed in rad/s + * @param pitch_speed Desired pitch angular speed in rad/s + * @param yaw_speed Desired yaw angular speed in rad/s + * @param thrust Collective thrust, normalized to 0 .. 1 + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_send(mavlink_channel_t chan, uint64_t time_us, float roll_speed, float pitch_speed, float yaw_speed, float thrust) +{ + mavlink_message_t msg; + mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, time_us, roll_speed, pitch_speed, yaw_speed, thrust); + mavlink_send_uart(chan, &msg); +} + +#endif +// MESSAGE ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT UNPACKING + +/** + * @brief Get field time_us from roll_pitch_yaw_speed_thrust_setpoint message + * + * @return Timestamp in micro seconds since unix epoch + */ +static inline uint64_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_time_us(const mavlink_message_t* msg) +{ + generic_64bit r; + r.b[7] = (msg->payload)[0]; + r.b[6] = (msg->payload)[1]; + r.b[5] = (msg->payload)[2]; + r.b[4] = (msg->payload)[3]; + r.b[3] = (msg->payload)[4]; + r.b[2] = (msg->payload)[5]; + r.b[1] = (msg->payload)[6]; + r.b[0] = (msg->payload)[7]; + return (uint64_t)r.ll; +} + +/** + * @brief Get field roll_speed from roll_pitch_yaw_speed_thrust_setpoint message + * + * @return Desired roll angular speed in rad/s + */ +static inline float mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_roll_speed(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint64_t))[0]; + r.b[2] = (msg->payload+sizeof(uint64_t))[1]; + r.b[1] = (msg->payload+sizeof(uint64_t))[2]; + r.b[0] = (msg->payload+sizeof(uint64_t))[3]; + return (float)r.f; +} + +/** + * @brief Get field pitch_speed from roll_pitch_yaw_speed_thrust_setpoint message + * + * @return Desired pitch angular speed in rad/s + */ +static inline float mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_pitch_speed(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float))[3]; + return (float)r.f; +} + +/** + * @brief Get field yaw_speed from roll_pitch_yaw_speed_thrust_setpoint message + * + * @return Desired yaw angular speed in rad/s + */ +static inline float mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_yaw_speed(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[3]; + return (float)r.f; +} + +/** + * @brief Get field thrust from roll_pitch_yaw_speed_thrust_setpoint message + * + * @return Collective thrust, normalized to 0 .. 1 + */ +static inline float mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_thrust(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; + return (float)r.f; +} + +/** + * @brief Decode a roll_pitch_yaw_speed_thrust_setpoint message into a struct + * + * @param msg The message to decode + * @param roll_pitch_yaw_speed_thrust_setpoint C-struct to decode the message contents into + */ +static inline void mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_decode(const mavlink_message_t* msg, mavlink_roll_pitch_yaw_speed_thrust_setpoint_t* roll_pitch_yaw_speed_thrust_setpoint) +{ + roll_pitch_yaw_speed_thrust_setpoint->time_us = mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_time_us(msg); + roll_pitch_yaw_speed_thrust_setpoint->roll_speed = mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_roll_speed(msg); + roll_pitch_yaw_speed_thrust_setpoint->pitch_speed = mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_pitch_speed(msg); + roll_pitch_yaw_speed_thrust_setpoint->yaw_speed = mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_yaw_speed(msg); + roll_pitch_yaw_speed_thrust_setpoint->thrust = mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_thrust(msg); +} diff --git a/thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h b/thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h new file mode 100644 index 0000000000000000000000000000000000000000..cc944642748d251f3f9c976a2d1d1550cc816019 --- /dev/null +++ b/thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h @@ -0,0 +1,198 @@ +// MESSAGE ROLL_PITCH_YAW_THRUST_SETPOINT PACKING + +#define MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT 57 + +typedef struct __mavlink_roll_pitch_yaw_thrust_setpoint_t +{ + uint64_t time_us; ///< Timestamp in micro seconds since unix epoch + float roll; ///< Desired roll angle in radians + float pitch; ///< Desired pitch angle in radians + float yaw; ///< Desired yaw angle in radians + float thrust; ///< Collective thrust, normalized to 0 .. 1 + +} mavlink_roll_pitch_yaw_thrust_setpoint_t; + + + +/** + * @brief Pack a roll_pitch_yaw_thrust_setpoint message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_us Timestamp in micro seconds since unix epoch + * @param roll Desired roll angle in radians + * @param pitch Desired pitch angle in radians + * @param yaw Desired yaw angle in radians + * @param thrust Collective thrust, normalized to 0 .. 1 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t time_us, float roll, float pitch, float yaw, float thrust) +{ + uint16_t i = 0; + msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT; + + i += put_uint64_t_by_index(time_us, i, msg->payload); // Timestamp in micro seconds since unix epoch + i += put_float_by_index(roll, i, msg->payload); // Desired roll angle in radians + i += put_float_by_index(pitch, i, msg->payload); // Desired pitch angle in radians + i += put_float_by_index(yaw, i, msg->payload); // Desired yaw angle in radians + i += put_float_by_index(thrust, i, msg->payload); // Collective thrust, normalized to 0 .. 1 + + return mavlink_finalize_message(msg, system_id, component_id, i); +} + +/** + * @brief Pack a roll_pitch_yaw_thrust_setpoint message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param time_us Timestamp in micro seconds since unix epoch + * @param roll Desired roll angle in radians + * @param pitch Desired pitch angle in radians + * @param yaw Desired yaw angle in radians + * @param thrust Collective thrust, normalized to 0 .. 1 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t time_us, float roll, float pitch, float yaw, float thrust) +{ + uint16_t i = 0; + msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT; + + i += put_uint64_t_by_index(time_us, i, msg->payload); // Timestamp in micro seconds since unix epoch + i += put_float_by_index(roll, i, msg->payload); // Desired roll angle in radians + i += put_float_by_index(pitch, i, msg->payload); // Desired pitch angle in radians + i += put_float_by_index(yaw, i, msg->payload); // Desired yaw angle in radians + i += put_float_by_index(thrust, i, msg->payload); // Collective thrust, normalized to 0 .. 1 + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); +} + +/** + * @brief Encode a roll_pitch_yaw_thrust_setpoint struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param roll_pitch_yaw_thrust_setpoint C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_roll_pitch_yaw_thrust_setpoint_t* roll_pitch_yaw_thrust_setpoint) +{ + return mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack(system_id, component_id, msg, roll_pitch_yaw_thrust_setpoint->time_us, roll_pitch_yaw_thrust_setpoint->roll, roll_pitch_yaw_thrust_setpoint->pitch, roll_pitch_yaw_thrust_setpoint->yaw, roll_pitch_yaw_thrust_setpoint->thrust); +} + +/** + * @brief Send a roll_pitch_yaw_thrust_setpoint message + * @param chan MAVLink channel to send the message + * + * @param time_us Timestamp in micro seconds since unix epoch + * @param roll Desired roll angle in radians + * @param pitch Desired pitch angle in radians + * @param yaw Desired yaw angle in radians + * @param thrust Collective thrust, normalized to 0 .. 1 + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(mavlink_channel_t chan, uint64_t time_us, float roll, float pitch, float yaw, float thrust) +{ + mavlink_message_t msg; + mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, time_us, roll, pitch, yaw, thrust); + mavlink_send_uart(chan, &msg); +} + +#endif +// MESSAGE ROLL_PITCH_YAW_THRUST_SETPOINT UNPACKING + +/** + * @brief Get field time_us from roll_pitch_yaw_thrust_setpoint message + * + * @return Timestamp in micro seconds since unix epoch + */ +static inline uint64_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_time_us(const mavlink_message_t* msg) +{ + generic_64bit r; + r.b[7] = (msg->payload)[0]; + r.b[6] = (msg->payload)[1]; + r.b[5] = (msg->payload)[2]; + r.b[4] = (msg->payload)[3]; + r.b[3] = (msg->payload)[4]; + r.b[2] = (msg->payload)[5]; + r.b[1] = (msg->payload)[6]; + r.b[0] = (msg->payload)[7]; + return (uint64_t)r.ll; +} + +/** + * @brief Get field roll from roll_pitch_yaw_thrust_setpoint message + * + * @return Desired roll angle in radians + */ +static inline float mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_roll(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint64_t))[0]; + r.b[2] = (msg->payload+sizeof(uint64_t))[1]; + r.b[1] = (msg->payload+sizeof(uint64_t))[2]; + r.b[0] = (msg->payload+sizeof(uint64_t))[3]; + return (float)r.f; +} + +/** + * @brief Get field pitch from roll_pitch_yaw_thrust_setpoint message + * + * @return Desired pitch angle in radians + */ +static inline float mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_pitch(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float))[3]; + return (float)r.f; +} + +/** + * @brief Get field yaw from roll_pitch_yaw_thrust_setpoint message + * + * @return Desired yaw angle in radians + */ +static inline float mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_yaw(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[3]; + return (float)r.f; +} + +/** + * @brief Get field thrust from roll_pitch_yaw_thrust_setpoint message + * + * @return Collective thrust, normalized to 0 .. 1 + */ +static inline float mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_thrust(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; + return (float)r.f; +} + +/** + * @brief Decode a roll_pitch_yaw_thrust_setpoint message into a struct + * + * @param msg The message to decode + * @param roll_pitch_yaw_thrust_setpoint C-struct to decode the message contents into + */ +static inline void mavlink_msg_roll_pitch_yaw_thrust_setpoint_decode(const mavlink_message_t* msg, mavlink_roll_pitch_yaw_thrust_setpoint_t* roll_pitch_yaw_thrust_setpoint) +{ + roll_pitch_yaw_thrust_setpoint->time_us = mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_time_us(msg); + roll_pitch_yaw_thrust_setpoint->roll = mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_roll(msg); + roll_pitch_yaw_thrust_setpoint->pitch = mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_pitch(msg); + roll_pitch_yaw_thrust_setpoint->yaw = mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_yaw(msg); + roll_pitch_yaw_thrust_setpoint->thrust = mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_thrust(msg); +} diff --git a/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw.h b/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw.h new file mode 100644 index 0000000000000000000000000000000000000000..1cc68dff921cd42edf3eb0a7a383a9821637087a --- /dev/null +++ b/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw.h @@ -0,0 +1,184 @@ +// MESSAGE SET_ROLL_PITCH_YAW PACKING + +#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW 55 + +typedef struct __mavlink_set_roll_pitch_yaw_t +{ + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + float roll; ///< Desired roll angle in radians + float pitch; ///< Desired pitch angle in radians + float yaw; ///< Desired yaw angle in radians + +} mavlink_set_roll_pitch_yaw_t; + + + +/** + * @brief Pack a set_roll_pitch_yaw message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param roll Desired roll angle in radians + * @param pitch Desired pitch angle in radians + * @param yaw Desired yaw angle in radians + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_roll_pitch_yaw_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, float roll, float pitch, float yaw) +{ + uint16_t i = 0; + msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW; + + i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID + i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID + i += put_float_by_index(roll, i, msg->payload); // Desired roll angle in radians + i += put_float_by_index(pitch, i, msg->payload); // Desired pitch angle in radians + i += put_float_by_index(yaw, i, msg->payload); // Desired yaw angle in radians + + return mavlink_finalize_message(msg, system_id, component_id, i); +} + +/** + * @brief Pack a set_roll_pitch_yaw message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param roll Desired roll angle in radians + * @param pitch Desired pitch angle in radians + * @param yaw Desired yaw angle in radians + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_roll_pitch_yaw_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, float roll, float pitch, float yaw) +{ + uint16_t i = 0; + msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW; + + i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID + i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID + i += put_float_by_index(roll, i, msg->payload); // Desired roll angle in radians + i += put_float_by_index(pitch, i, msg->payload); // Desired pitch angle in radians + i += put_float_by_index(yaw, i, msg->payload); // Desired yaw angle in radians + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); +} + +/** + * @brief Encode a set_roll_pitch_yaw struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param set_roll_pitch_yaw C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_roll_pitch_yaw_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_roll_pitch_yaw_t* set_roll_pitch_yaw) +{ + return mavlink_msg_set_roll_pitch_yaw_pack(system_id, component_id, msg, set_roll_pitch_yaw->target_system, set_roll_pitch_yaw->target_component, set_roll_pitch_yaw->roll, set_roll_pitch_yaw->pitch, set_roll_pitch_yaw->yaw); +} + +/** + * @brief Send a set_roll_pitch_yaw message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param roll Desired roll angle in radians + * @param pitch Desired pitch angle in radians + * @param yaw Desired yaw angle in radians + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_set_roll_pitch_yaw_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float roll, float pitch, float yaw) +{ + mavlink_message_t msg; + mavlink_msg_set_roll_pitch_yaw_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, roll, pitch, yaw); + mavlink_send_uart(chan, &msg); +} + +#endif +// MESSAGE SET_ROLL_PITCH_YAW UNPACKING + +/** + * @brief Get field target_system from set_roll_pitch_yaw message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_set_roll_pitch_yaw_get_target_system(const mavlink_message_t* msg) +{ + return (uint8_t)(msg->payload)[0]; +} + +/** + * @brief Get field target_component from set_roll_pitch_yaw message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_set_roll_pitch_yaw_get_target_component(const mavlink_message_t* msg) +{ + return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; +} + +/** + * @brief Get field roll from set_roll_pitch_yaw message + * + * @return Desired roll angle in radians + */ +static inline float mavlink_msg_set_roll_pitch_yaw_get_roll(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; + r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[1]; + r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[2]; + r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[3]; + return (float)r.f; +} + +/** + * @brief Get field pitch from set_roll_pitch_yaw message + * + * @return Desired pitch angle in radians + */ +static inline float mavlink_msg_set_roll_pitch_yaw_get_pitch(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[3]; + return (float)r.f; +} + +/** + * @brief Get field yaw from set_roll_pitch_yaw message + * + * @return Desired yaw angle in radians + */ +static inline float mavlink_msg_set_roll_pitch_yaw_get_yaw(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[3]; + return (float)r.f; +} + +/** + * @brief Decode a set_roll_pitch_yaw message into a struct + * + * @param msg The message to decode + * @param set_roll_pitch_yaw C-struct to decode the message contents into + */ +static inline void mavlink_msg_set_roll_pitch_yaw_decode(const mavlink_message_t* msg, mavlink_set_roll_pitch_yaw_t* set_roll_pitch_yaw) +{ + set_roll_pitch_yaw->target_system = mavlink_msg_set_roll_pitch_yaw_get_target_system(msg); + set_roll_pitch_yaw->target_component = mavlink_msg_set_roll_pitch_yaw_get_target_component(msg); + set_roll_pitch_yaw->roll = mavlink_msg_set_roll_pitch_yaw_get_roll(msg); + set_roll_pitch_yaw->pitch = mavlink_msg_set_roll_pitch_yaw_get_pitch(msg); + set_roll_pitch_yaw->yaw = mavlink_msg_set_roll_pitch_yaw_get_yaw(msg); +} diff --git a/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw_speed.h b/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw_speed.h new file mode 100644 index 0000000000000000000000000000000000000000..7855daf765a2e176d7020e1d23295e9351c22083 --- /dev/null +++ b/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw_speed.h @@ -0,0 +1,184 @@ +// MESSAGE SET_ROLL_PITCH_YAW_SPEED PACKING + +#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED 56 + +typedef struct __mavlink_set_roll_pitch_yaw_speed_t +{ + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + float roll_speed; ///< Desired roll angular speed in rad/s + float pitch_speed; ///< Desired pitch angular speed in rad/s + float yaw_speed; ///< Desired yaw angular speed in rad/s + +} mavlink_set_roll_pitch_yaw_speed_t; + + + +/** + * @brief Pack a set_roll_pitch_yaw_speed message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param roll_speed Desired roll angular speed in rad/s + * @param pitch_speed Desired pitch angular speed in rad/s + * @param yaw_speed Desired yaw angular speed in rad/s + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, float roll_speed, float pitch_speed, float yaw_speed) +{ + uint16_t i = 0; + msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED; + + i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID + i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID + i += put_float_by_index(roll_speed, i, msg->payload); // Desired roll angular speed in rad/s + i += put_float_by_index(pitch_speed, i, msg->payload); // Desired pitch angular speed in rad/s + i += put_float_by_index(yaw_speed, i, msg->payload); // Desired yaw angular speed in rad/s + + return mavlink_finalize_message(msg, system_id, component_id, i); +} + +/** + * @brief Pack a set_roll_pitch_yaw_speed message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param roll_speed Desired roll angular speed in rad/s + * @param pitch_speed Desired pitch angular speed in rad/s + * @param yaw_speed Desired yaw angular speed in rad/s + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, float roll_speed, float pitch_speed, float yaw_speed) +{ + uint16_t i = 0; + msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED; + + i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID + i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID + i += put_float_by_index(roll_speed, i, msg->payload); // Desired roll angular speed in rad/s + i += put_float_by_index(pitch_speed, i, msg->payload); // Desired pitch angular speed in rad/s + i += put_float_by_index(yaw_speed, i, msg->payload); // Desired yaw angular speed in rad/s + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); +} + +/** + * @brief Encode a set_roll_pitch_yaw_speed struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param set_roll_pitch_yaw_speed C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_roll_pitch_yaw_speed_t* set_roll_pitch_yaw_speed) +{ + return mavlink_msg_set_roll_pitch_yaw_speed_pack(system_id, component_id, msg, set_roll_pitch_yaw_speed->target_system, set_roll_pitch_yaw_speed->target_component, set_roll_pitch_yaw_speed->roll_speed, set_roll_pitch_yaw_speed->pitch_speed, set_roll_pitch_yaw_speed->yaw_speed); +} + +/** + * @brief Send a set_roll_pitch_yaw_speed message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param roll_speed Desired roll angular speed in rad/s + * @param pitch_speed Desired pitch angular speed in rad/s + * @param yaw_speed Desired yaw angular speed in rad/s + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_set_roll_pitch_yaw_speed_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float roll_speed, float pitch_speed, float yaw_speed) +{ + mavlink_message_t msg; + mavlink_msg_set_roll_pitch_yaw_speed_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, roll_speed, pitch_speed, yaw_speed); + mavlink_send_uart(chan, &msg); +} + +#endif +// MESSAGE SET_ROLL_PITCH_YAW_SPEED UNPACKING + +/** + * @brief Get field target_system from set_roll_pitch_yaw_speed message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_set_roll_pitch_yaw_speed_get_target_system(const mavlink_message_t* msg) +{ + return (uint8_t)(msg->payload)[0]; +} + +/** + * @brief Get field target_component from set_roll_pitch_yaw_speed message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_set_roll_pitch_yaw_speed_get_target_component(const mavlink_message_t* msg) +{ + return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; +} + +/** + * @brief Get field roll_speed from set_roll_pitch_yaw_speed message + * + * @return Desired roll angular speed in rad/s + */ +static inline float mavlink_msg_set_roll_pitch_yaw_speed_get_roll_speed(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; + r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[1]; + r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[2]; + r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[3]; + return (float)r.f; +} + +/** + * @brief Get field pitch_speed from set_roll_pitch_yaw_speed message + * + * @return Desired pitch angular speed in rad/s + */ +static inline float mavlink_msg_set_roll_pitch_yaw_speed_get_pitch_speed(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[3]; + return (float)r.f; +} + +/** + * @brief Get field yaw_speed from set_roll_pitch_yaw_speed message + * + * @return Desired yaw angular speed in rad/s + */ +static inline float mavlink_msg_set_roll_pitch_yaw_speed_get_yaw_speed(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[3]; + return (float)r.f; +} + +/** + * @brief Decode a set_roll_pitch_yaw_speed message into a struct + * + * @param msg The message to decode + * @param set_roll_pitch_yaw_speed C-struct to decode the message contents into + */ +static inline void mavlink_msg_set_roll_pitch_yaw_speed_decode(const mavlink_message_t* msg, mavlink_set_roll_pitch_yaw_speed_t* set_roll_pitch_yaw_speed) +{ + set_roll_pitch_yaw_speed->target_system = mavlink_msg_set_roll_pitch_yaw_speed_get_target_system(msg); + set_roll_pitch_yaw_speed->target_component = mavlink_msg_set_roll_pitch_yaw_speed_get_target_component(msg); + set_roll_pitch_yaw_speed->roll_speed = mavlink_msg_set_roll_pitch_yaw_speed_get_roll_speed(msg); + set_roll_pitch_yaw_speed->pitch_speed = mavlink_msg_set_roll_pitch_yaw_speed_get_pitch_speed(msg); + set_roll_pitch_yaw_speed->yaw_speed = mavlink_msg_set_roll_pitch_yaw_speed_get_yaw_speed(msg); +} diff --git a/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h b/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h new file mode 100644 index 0000000000000000000000000000000000000000..dd12ea4cb09bc882873096c2ecb606019c2e3f31 --- /dev/null +++ b/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h @@ -0,0 +1,206 @@ +// MESSAGE SET_ROLL_PITCH_YAW_SPEED_THRUST PACKING + +#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST 56 + +typedef struct __mavlink_set_roll_pitch_yaw_speed_thrust_t +{ + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + float roll_speed; ///< Desired roll angular speed in rad/s + float pitch_speed; ///< Desired pitch angular speed in rad/s + float yaw_speed; ///< Desired yaw angular speed in rad/s + float thrust; ///< Collective thrust, normalized to 0 .. 1 + +} mavlink_set_roll_pitch_yaw_speed_thrust_t; + + + +/** + * @brief Pack a set_roll_pitch_yaw_speed_thrust message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param roll_speed Desired roll angular speed in rad/s + * @param pitch_speed Desired pitch angular speed in rad/s + * @param yaw_speed Desired yaw angular speed in rad/s + * @param thrust Collective thrust, normalized to 0 .. 1 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, float roll_speed, float pitch_speed, float yaw_speed, float thrust) +{ + uint16_t i = 0; + msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST; + + i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID + i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID + i += put_float_by_index(roll_speed, i, msg->payload); // Desired roll angular speed in rad/s + i += put_float_by_index(pitch_speed, i, msg->payload); // Desired pitch angular speed in rad/s + i += put_float_by_index(yaw_speed, i, msg->payload); // Desired yaw angular speed in rad/s + i += put_float_by_index(thrust, i, msg->payload); // Collective thrust, normalized to 0 .. 1 + + return mavlink_finalize_message(msg, system_id, component_id, i); +} + +/** + * @brief Pack a set_roll_pitch_yaw_speed_thrust message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param roll_speed Desired roll angular speed in rad/s + * @param pitch_speed Desired pitch angular speed in rad/s + * @param yaw_speed Desired yaw angular speed in rad/s + * @param thrust Collective thrust, normalized to 0 .. 1 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, float roll_speed, float pitch_speed, float yaw_speed, float thrust) +{ + uint16_t i = 0; + msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST; + + i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID + i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID + i += put_float_by_index(roll_speed, i, msg->payload); // Desired roll angular speed in rad/s + i += put_float_by_index(pitch_speed, i, msg->payload); // Desired pitch angular speed in rad/s + i += put_float_by_index(yaw_speed, i, msg->payload); // Desired yaw angular speed in rad/s + i += put_float_by_index(thrust, i, msg->payload); // Collective thrust, normalized to 0 .. 1 + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); +} + +/** + * @brief Encode a set_roll_pitch_yaw_speed_thrust struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param set_roll_pitch_yaw_speed_thrust C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_roll_pitch_yaw_speed_thrust_t* set_roll_pitch_yaw_speed_thrust) +{ + return mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack(system_id, component_id, msg, set_roll_pitch_yaw_speed_thrust->target_system, set_roll_pitch_yaw_speed_thrust->target_component, set_roll_pitch_yaw_speed_thrust->roll_speed, set_roll_pitch_yaw_speed_thrust->pitch_speed, set_roll_pitch_yaw_speed_thrust->yaw_speed, set_roll_pitch_yaw_speed_thrust->thrust); +} + +/** + * @brief Send a set_roll_pitch_yaw_speed_thrust message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param roll_speed Desired roll angular speed in rad/s + * @param pitch_speed Desired pitch angular speed in rad/s + * @param yaw_speed Desired yaw angular speed in rad/s + * @param thrust Collective thrust, normalized to 0 .. 1 + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_set_roll_pitch_yaw_speed_thrust_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float roll_speed, float pitch_speed, float yaw_speed, float thrust) +{ + mavlink_message_t msg; + mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, roll_speed, pitch_speed, yaw_speed, thrust); + mavlink_send_uart(chan, &msg); +} + +#endif +// MESSAGE SET_ROLL_PITCH_YAW_SPEED_THRUST UNPACKING + +/** + * @brief Get field target_system from set_roll_pitch_yaw_speed_thrust message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_target_system(const mavlink_message_t* msg) +{ + return (uint8_t)(msg->payload)[0]; +} + +/** + * @brief Get field target_component from set_roll_pitch_yaw_speed_thrust message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_target_component(const mavlink_message_t* msg) +{ + return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; +} + +/** + * @brief Get field roll_speed from set_roll_pitch_yaw_speed_thrust message + * + * @return Desired roll angular speed in rad/s + */ +static inline float mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_roll_speed(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; + r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[1]; + r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[2]; + r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[3]; + return (float)r.f; +} + +/** + * @brief Get field pitch_speed from set_roll_pitch_yaw_speed_thrust message + * + * @return Desired pitch angular speed in rad/s + */ +static inline float mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_pitch_speed(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[3]; + return (float)r.f; +} + +/** + * @brief Get field yaw_speed from set_roll_pitch_yaw_speed_thrust message + * + * @return Desired yaw angular speed in rad/s + */ +static inline float mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_yaw_speed(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[3]; + return (float)r.f; +} + +/** + * @brief Get field thrust from set_roll_pitch_yaw_speed_thrust message + * + * @return Collective thrust, normalized to 0 .. 1 + */ +static inline float mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_thrust(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; + return (float)r.f; +} + +/** + * @brief Decode a set_roll_pitch_yaw_speed_thrust message into a struct + * + * @param msg The message to decode + * @param set_roll_pitch_yaw_speed_thrust C-struct to decode the message contents into + */ +static inline void mavlink_msg_set_roll_pitch_yaw_speed_thrust_decode(const mavlink_message_t* msg, mavlink_set_roll_pitch_yaw_speed_thrust_t* set_roll_pitch_yaw_speed_thrust) +{ + set_roll_pitch_yaw_speed_thrust->target_system = mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_target_system(msg); + set_roll_pitch_yaw_speed_thrust->target_component = mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_target_component(msg); + set_roll_pitch_yaw_speed_thrust->roll_speed = mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_roll_speed(msg); + set_roll_pitch_yaw_speed_thrust->pitch_speed = mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_pitch_speed(msg); + set_roll_pitch_yaw_speed_thrust->yaw_speed = mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_yaw_speed(msg); + set_roll_pitch_yaw_speed_thrust->thrust = mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_thrust(msg); +} diff --git a/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw_thrust.h b/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw_thrust.h new file mode 100644 index 0000000000000000000000000000000000000000..7bf7d26247de458a5c10f7600da0358d7924e659 --- /dev/null +++ b/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw_thrust.h @@ -0,0 +1,206 @@ +// MESSAGE SET_ROLL_PITCH_YAW_THRUST PACKING + +#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST 55 + +typedef struct __mavlink_set_roll_pitch_yaw_thrust_t +{ + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + float roll; ///< Desired roll angle in radians + float pitch; ///< Desired pitch angle in radians + float yaw; ///< Desired yaw angle in radians + float thrust; ///< Collective thrust, normalized to 0 .. 1 + +} mavlink_set_roll_pitch_yaw_thrust_t; + + + +/** + * @brief Pack a set_roll_pitch_yaw_thrust message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param roll Desired roll angle in radians + * @param pitch Desired pitch angle in radians + * @param yaw Desired yaw angle in radians + * @param thrust Collective thrust, normalized to 0 .. 1 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, float roll, float pitch, float yaw, float thrust) +{ + uint16_t i = 0; + msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST; + + i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID + i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID + i += put_float_by_index(roll, i, msg->payload); // Desired roll angle in radians + i += put_float_by_index(pitch, i, msg->payload); // Desired pitch angle in radians + i += put_float_by_index(yaw, i, msg->payload); // Desired yaw angle in radians + i += put_float_by_index(thrust, i, msg->payload); // Collective thrust, normalized to 0 .. 1 + + return mavlink_finalize_message(msg, system_id, component_id, i); +} + +/** + * @brief Pack a set_roll_pitch_yaw_thrust message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param roll Desired roll angle in radians + * @param pitch Desired pitch angle in radians + * @param yaw Desired yaw angle in radians + * @param thrust Collective thrust, normalized to 0 .. 1 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, float roll, float pitch, float yaw, float thrust) +{ + uint16_t i = 0; + msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST; + + i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID + i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID + i += put_float_by_index(roll, i, msg->payload); // Desired roll angle in radians + i += put_float_by_index(pitch, i, msg->payload); // Desired pitch angle in radians + i += put_float_by_index(yaw, i, msg->payload); // Desired yaw angle in radians + i += put_float_by_index(thrust, i, msg->payload); // Collective thrust, normalized to 0 .. 1 + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); +} + +/** + * @brief Encode a set_roll_pitch_yaw_thrust struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param set_roll_pitch_yaw_thrust C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_roll_pitch_yaw_thrust_t* set_roll_pitch_yaw_thrust) +{ + return mavlink_msg_set_roll_pitch_yaw_thrust_pack(system_id, component_id, msg, set_roll_pitch_yaw_thrust->target_system, set_roll_pitch_yaw_thrust->target_component, set_roll_pitch_yaw_thrust->roll, set_roll_pitch_yaw_thrust->pitch, set_roll_pitch_yaw_thrust->yaw, set_roll_pitch_yaw_thrust->thrust); +} + +/** + * @brief Send a set_roll_pitch_yaw_thrust message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param roll Desired roll angle in radians + * @param pitch Desired pitch angle in radians + * @param yaw Desired yaw angle in radians + * @param thrust Collective thrust, normalized to 0 .. 1 + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_set_roll_pitch_yaw_thrust_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float roll, float pitch, float yaw, float thrust) +{ + mavlink_message_t msg; + mavlink_msg_set_roll_pitch_yaw_thrust_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, roll, pitch, yaw, thrust); + mavlink_send_uart(chan, &msg); +} + +#endif +// MESSAGE SET_ROLL_PITCH_YAW_THRUST UNPACKING + +/** + * @brief Get field target_system from set_roll_pitch_yaw_thrust message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_set_roll_pitch_yaw_thrust_get_target_system(const mavlink_message_t* msg) +{ + return (uint8_t)(msg->payload)[0]; +} + +/** + * @brief Get field target_component from set_roll_pitch_yaw_thrust message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_set_roll_pitch_yaw_thrust_get_target_component(const mavlink_message_t* msg) +{ + return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; +} + +/** + * @brief Get field roll from set_roll_pitch_yaw_thrust message + * + * @return Desired roll angle in radians + */ +static inline float mavlink_msg_set_roll_pitch_yaw_thrust_get_roll(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; + r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[1]; + r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[2]; + r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[3]; + return (float)r.f; +} + +/** + * @brief Get field pitch from set_roll_pitch_yaw_thrust message + * + * @return Desired pitch angle in radians + */ +static inline float mavlink_msg_set_roll_pitch_yaw_thrust_get_pitch(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[3]; + return (float)r.f; +} + +/** + * @brief Get field yaw from set_roll_pitch_yaw_thrust message + * + * @return Desired yaw angle in radians + */ +static inline float mavlink_msg_set_roll_pitch_yaw_thrust_get_yaw(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[3]; + return (float)r.f; +} + +/** + * @brief Get field thrust from set_roll_pitch_yaw_thrust message + * + * @return Collective thrust, normalized to 0 .. 1 + */ +static inline float mavlink_msg_set_roll_pitch_yaw_thrust_get_thrust(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; + return (float)r.f; +} + +/** + * @brief Decode a set_roll_pitch_yaw_thrust message into a struct + * + * @param msg The message to decode + * @param set_roll_pitch_yaw_thrust C-struct to decode the message contents into + */ +static inline void mavlink_msg_set_roll_pitch_yaw_thrust_decode(const mavlink_message_t* msg, mavlink_set_roll_pitch_yaw_thrust_t* set_roll_pitch_yaw_thrust) +{ + set_roll_pitch_yaw_thrust->target_system = mavlink_msg_set_roll_pitch_yaw_thrust_get_target_system(msg); + set_roll_pitch_yaw_thrust->target_component = mavlink_msg_set_roll_pitch_yaw_thrust_get_target_component(msg); + set_roll_pitch_yaw_thrust->roll = mavlink_msg_set_roll_pitch_yaw_thrust_get_roll(msg); + set_roll_pitch_yaw_thrust->pitch = mavlink_msg_set_roll_pitch_yaw_thrust_get_pitch(msg); + set_roll_pitch_yaw_thrust->yaw = mavlink_msg_set_roll_pitch_yaw_thrust_get_yaw(msg); + set_roll_pitch_yaw_thrust->thrust = mavlink_msg_set_roll_pitch_yaw_thrust_get_thrust(msg); +} diff --git a/thirdParty/mavlink/include/minimal/mavlink.h b/thirdParty/mavlink/include/minimal/mavlink.h index 45a5e9566a8f66618fc3b88cc4469b032ebabc7b..38389146abe8b01e562eacc67c2e200690b4dde6 100644 --- a/thirdParty/mavlink/include/minimal/mavlink.h +++ b/thirdParty/mavlink/include/minimal/mavlink.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Wednesday, July 27 2011, 14:17 UTC + * Generated on Friday, August 5 2011, 07:37 UTC */ #ifndef MAVLINK_H #define MAVLINK_H diff --git a/thirdParty/mavlink/include/minimal/minimal.h b/thirdParty/mavlink/include/minimal/minimal.h index 61cd3fe22be39ed8f8bdaaf855398f3d7cce25d2..ee09135a68cf8baf23eaa915b6791e0b1168d99c 100644 --- a/thirdParty/mavlink/include/minimal/minimal.h +++ b/thirdParty/mavlink/include/minimal/minimal.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://qgroundcontrol.org/mavlink/ - * Generated on Wednesday, July 27 2011, 14:17 UTC + * Generated on Friday, August 5 2011, 07:37 UTC */ #ifndef MINIMAL_H #define MINIMAL_H diff --git a/thirdParty/mavlink/include/pixhawk/mavlink.h b/thirdParty/mavlink/include/pixhawk/mavlink.h index 16a37599e1fa218650bf065dbd66ec620892162c..57a763b36723d0002ed3fc496cbfea74d5f5e5e7 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Wednesday, July 27 2011, 14:17 UTC + * Generated on Wednesday, August 17 2011, 06:03 UTC */ #ifndef MAVLINK_H #define MAVLINK_H diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_visual_odometry.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_visual_odometry.h new file mode 100644 index 0000000000000000000000000000000000000000..0fd504c1ff9e9ea7d597ed0ac200744c8fdd9959 --- /dev/null +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_visual_odometry.h @@ -0,0 +1,268 @@ +// MESSAGE VISUAL_ODOMETRY PACKING + +#define MAVLINK_MSG_ID_VISUAL_ODOMETRY 180 + +typedef struct __mavlink_visual_odometry_t +{ + uint64_t frame1_time_us; ///< Time at which frame 1 was captured (in microseconds since unix epoch) + uint64_t frame2_time_us; ///< Time at which frame 2 was captured (in microseconds since unix epoch) + float x; ///< Relative X position + float y; ///< Relative Y position + float z; ///< Relative Z position + float roll; ///< Relative roll angle in rad + float pitch; ///< Relative pitch angle in rad + float yaw; ///< Relative yaw angle in rad + +} mavlink_visual_odometry_t; + + + +/** + * @brief Pack a visual_odometry message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param frame1_time_us Time at which frame 1 was captured (in microseconds since unix epoch) + * @param frame2_time_us Time at which frame 2 was captured (in microseconds since unix epoch) + * @param x Relative X position + * @param y Relative Y position + * @param z Relative Z position + * @param roll Relative roll angle in rad + * @param pitch Relative pitch angle in rad + * @param yaw Relative yaw angle in rad + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_visual_odometry_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t frame1_time_us, uint64_t frame2_time_us, float x, float y, float z, float roll, float pitch, float yaw) +{ + uint16_t i = 0; + msg->msgid = MAVLINK_MSG_ID_VISUAL_ODOMETRY; + + i += put_uint64_t_by_index(frame1_time_us, i, msg->payload); // Time at which frame 1 was captured (in microseconds since unix epoch) + i += put_uint64_t_by_index(frame2_time_us, i, msg->payload); // Time at which frame 2 was captured (in microseconds since unix epoch) + i += put_float_by_index(x, i, msg->payload); // Relative X position + i += put_float_by_index(y, i, msg->payload); // Relative Y position + i += put_float_by_index(z, i, msg->payload); // Relative Z position + i += put_float_by_index(roll, i, msg->payload); // Relative roll angle in rad + i += put_float_by_index(pitch, i, msg->payload); // Relative pitch angle in rad + i += put_float_by_index(yaw, i, msg->payload); // Relative yaw angle in rad + + return mavlink_finalize_message(msg, system_id, component_id, i); +} + +/** + * @brief Pack a visual_odometry message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param frame1_time_us Time at which frame 1 was captured (in microseconds since unix epoch) + * @param frame2_time_us Time at which frame 2 was captured (in microseconds since unix epoch) + * @param x Relative X position + * @param y Relative Y position + * @param z Relative Z position + * @param roll Relative roll angle in rad + * @param pitch Relative pitch angle in rad + * @param yaw Relative yaw angle in rad + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_visual_odometry_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t frame1_time_us, uint64_t frame2_time_us, float x, float y, float z, float roll, float pitch, float yaw) +{ + uint16_t i = 0; + msg->msgid = MAVLINK_MSG_ID_VISUAL_ODOMETRY; + + i += put_uint64_t_by_index(frame1_time_us, i, msg->payload); // Time at which frame 1 was captured (in microseconds since unix epoch) + i += put_uint64_t_by_index(frame2_time_us, i, msg->payload); // Time at which frame 2 was captured (in microseconds since unix epoch) + i += put_float_by_index(x, i, msg->payload); // Relative X position + i += put_float_by_index(y, i, msg->payload); // Relative Y position + i += put_float_by_index(z, i, msg->payload); // Relative Z position + i += put_float_by_index(roll, i, msg->payload); // Relative roll angle in rad + i += put_float_by_index(pitch, i, msg->payload); // Relative pitch angle in rad + i += put_float_by_index(yaw, i, msg->payload); // Relative yaw angle in rad + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); +} + +/** + * @brief Encode a visual_odometry struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param visual_odometry C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_visual_odometry_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_visual_odometry_t* visual_odometry) +{ + return mavlink_msg_visual_odometry_pack(system_id, component_id, msg, visual_odometry->frame1_time_us, visual_odometry->frame2_time_us, visual_odometry->x, visual_odometry->y, visual_odometry->z, visual_odometry->roll, visual_odometry->pitch, visual_odometry->yaw); +} + +/** + * @brief Send a visual_odometry message + * @param chan MAVLink channel to send the message + * + * @param frame1_time_us Time at which frame 1 was captured (in microseconds since unix epoch) + * @param frame2_time_us Time at which frame 2 was captured (in microseconds since unix epoch) + * @param x Relative X position + * @param y Relative Y position + * @param z Relative Z position + * @param roll Relative roll angle in rad + * @param pitch Relative pitch angle in rad + * @param yaw Relative yaw angle in rad + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_visual_odometry_send(mavlink_channel_t chan, uint64_t frame1_time_us, uint64_t frame2_time_us, float x, float y, float z, float roll, float pitch, float yaw) +{ + mavlink_message_t msg; + mavlink_msg_visual_odometry_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, frame1_time_us, frame2_time_us, x, y, z, roll, pitch, yaw); + mavlink_send_uart(chan, &msg); +} + +#endif +// MESSAGE VISUAL_ODOMETRY UNPACKING + +/** + * @brief Get field frame1_time_us from visual_odometry message + * + * @return Time at which frame 1 was captured (in microseconds since unix epoch) + */ +static inline uint64_t mavlink_msg_visual_odometry_get_frame1_time_us(const mavlink_message_t* msg) +{ + generic_64bit r; + r.b[7] = (msg->payload)[0]; + r.b[6] = (msg->payload)[1]; + r.b[5] = (msg->payload)[2]; + r.b[4] = (msg->payload)[3]; + r.b[3] = (msg->payload)[4]; + r.b[2] = (msg->payload)[5]; + r.b[1] = (msg->payload)[6]; + r.b[0] = (msg->payload)[7]; + return (uint64_t)r.ll; +} + +/** + * @brief Get field frame2_time_us from visual_odometry message + * + * @return Time at which frame 2 was captured (in microseconds since unix epoch) + */ +static inline uint64_t mavlink_msg_visual_odometry_get_frame2_time_us(const mavlink_message_t* msg) +{ + generic_64bit r; + r.b[7] = (msg->payload+sizeof(uint64_t))[0]; + r.b[6] = (msg->payload+sizeof(uint64_t))[1]; + r.b[5] = (msg->payload+sizeof(uint64_t))[2]; + r.b[4] = (msg->payload+sizeof(uint64_t))[3]; + r.b[3] = (msg->payload+sizeof(uint64_t))[4]; + r.b[2] = (msg->payload+sizeof(uint64_t))[5]; + r.b[1] = (msg->payload+sizeof(uint64_t))[6]; + r.b[0] = (msg->payload+sizeof(uint64_t))[7]; + return (uint64_t)r.ll; +} + +/** + * @brief Get field x from visual_odometry message + * + * @return Relative X position + */ +static inline float mavlink_msg_visual_odometry_get_x(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t))[0]; + r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t))[1]; + r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t))[2]; + r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t))[3]; + return (float)r.f; +} + +/** + * @brief Get field y from visual_odometry message + * + * @return Relative Y position + */ +static inline float mavlink_msg_visual_odometry_get_y(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float))[3]; + return (float)r.f; +} + +/** + * @brief Get field z from visual_odometry message + * + * @return Relative Z position + */ +static inline float mavlink_msg_visual_odometry_get_z(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float)+sizeof(float))[3]; + return (float)r.f; +} + +/** + * @brief Get field roll from visual_odometry message + * + * @return Relative roll angle in rad + */ +static inline float mavlink_msg_visual_odometry_get_roll(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; + return (float)r.f; +} + +/** + * @brief Get field pitch from visual_odometry message + * + * @return Relative pitch angle in rad + */ +static inline float mavlink_msg_visual_odometry_get_pitch(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; + return (float)r.f; +} + +/** + * @brief Get field yaw from visual_odometry message + * + * @return Relative yaw angle in rad + */ +static inline float mavlink_msg_visual_odometry_get_yaw(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; + return (float)r.f; +} + +/** + * @brief Decode a visual_odometry message into a struct + * + * @param msg The message to decode + * @param visual_odometry C-struct to decode the message contents into + */ +static inline void mavlink_msg_visual_odometry_decode(const mavlink_message_t* msg, mavlink_visual_odometry_t* visual_odometry) +{ + visual_odometry->frame1_time_us = mavlink_msg_visual_odometry_get_frame1_time_us(msg); + visual_odometry->frame2_time_us = mavlink_msg_visual_odometry_get_frame2_time_us(msg); + visual_odometry->x = mavlink_msg_visual_odometry_get_x(msg); + visual_odometry->y = mavlink_msg_visual_odometry_get_y(msg); + visual_odometry->z = mavlink_msg_visual_odometry_get_z(msg); + visual_odometry->roll = mavlink_msg_visual_odometry_get_roll(msg); + visual_odometry->pitch = mavlink_msg_visual_odometry_get_pitch(msg); + visual_odometry->yaw = mavlink_msg_visual_odometry_get_yaw(msg); +} diff --git a/thirdParty/mavlink/include/pixhawk/pixhawk.h b/thirdParty/mavlink/include/pixhawk/pixhawk.h index 30474d09cb8c1268d12d14fe373d79f0f06551a4..5907478c2bbd9a1aa449f72da46b39ab44ae1d13 100644 --- a/thirdParty/mavlink/include/pixhawk/pixhawk.h +++ b/thirdParty/mavlink/include/pixhawk/pixhawk.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://qgroundcontrol.org/mavlink/ - * Generated on Wednesday, July 27 2011, 14:17 UTC + * Generated on Wednesday, August 17 2011, 06:03 UTC */ #ifndef PIXHAWK_H #define PIXHAWK_H @@ -66,12 +66,13 @@ enum DATA_TYPES #include "./mavlink_msg_data_transmission_handshake.h" #include "./mavlink_msg_encapsulated_data.h" #include "./mavlink_msg_brief_feature.h" +#include "./mavlink_msg_visual_odometry.h" // MESSAGE LENGTHS #undef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS { 3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 0, 0, 0, 0, 0, 5, 5, 26, 16, 36, 5, 6, 0, 0, 21, 0, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 21, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 11, 52, 1, 92, 0, 0, 0, 0, 0, 0, 0, 32, 32, 20, 0, 0, 0, 0, 0, 0, 20, 18, 0, 0, 0, 0, 0, 0, 0, 0, 26, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 16, 12, 0, 0, 0, 0, 0, 0, 0, 4, 255, 12, 6, 18, 0, 0, 0, 0, 0, 106, 42, 54, 0, 0, 0, 0, 0, 0, 0, 8, 255, 53, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51 } +#define MAVLINK_MESSAGE_LENGTHS { 3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 18, 18, 24, 24, 0, 0, 0, 26, 16, 36, 5, 6, 56, 26, 21, 18, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 21, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 11, 52, 1, 92, 0, 0, 0, 0, 0, 0, 0, 32, 32, 20, 0, 0, 0, 0, 0, 0, 20, 18, 0, 0, 0, 0, 0, 0, 0, 0, 26, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 16, 12, 0, 0, 0, 0, 0, 0, 0, 4, 255, 12, 6, 18, 0, 0, 0, 0, 0, 106, 42, 54, 0, 0, 0, 0, 0, 0, 0, 8, 255, 53, 0, 0, 0, 0, 0, 0, 0, 40, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51 } #ifdef __cplusplus } diff --git a/thirdParty/mavlink/include/slugs/mavlink.h b/thirdParty/mavlink/include/slugs/mavlink.h index bf0e146b4c4ac71ac936d8deb85044d42555ee1c..29ce7ed5554b80cf15e4d041ed91ecf44217de84 100644 --- a/thirdParty/mavlink/include/slugs/mavlink.h +++ b/thirdParty/mavlink/include/slugs/mavlink.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Wednesday, July 27 2011, 14:17 UTC + * Generated on Friday, August 5 2011, 07:37 UTC */ #ifndef MAVLINK_H #define MAVLINK_H diff --git a/thirdParty/mavlink/include/slugs/slugs.h b/thirdParty/mavlink/include/slugs/slugs.h index b8a6ff330ac6767501f59b0c042cd7b9966b3ab3..b04cb7922f08e9aa1733bbd5f934f9be18496b6f 100644 --- a/thirdParty/mavlink/include/slugs/slugs.h +++ b/thirdParty/mavlink/include/slugs/slugs.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://qgroundcontrol.org/mavlink/ - * Generated on Wednesday, July 27 2011, 14:17 UTC + * Generated on Friday, August 5 2011, 07:37 UTC */ #ifndef SLUGS_H #define SLUGS_H @@ -48,7 +48,7 @@ extern "C" { // MESSAGE LENGTHS #undef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS { 3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 0, 0, 0, 0, 0, 5, 5, 26, 16, 36, 5, 6, 0, 0, 21, 0, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 4, 10, 24, 18, 0, 0, 30, 24, 0, 7, 13, 3, 0, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51 } +#define MAVLINK_MESSAGE_LENGTHS { 3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 14, 14, 0, 0, 0, 5, 5, 26, 16, 36, 5, 6, 56, 0, 21, 18, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 4, 10, 24, 18, 0, 0, 30, 24, 0, 7, 13, 3, 0, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51 } #ifdef __cplusplus } diff --git a/thirdParty/mavlink/include/ualberta/mavlink.h b/thirdParty/mavlink/include/ualberta/mavlink.h index 30b060f630050f4b68a710e97ca0c3eed2ca0e9b..e12f74a8df8d6811ffda3cbec7ed6dc2f040dbae 100644 --- a/thirdParty/mavlink/include/ualberta/mavlink.h +++ b/thirdParty/mavlink/include/ualberta/mavlink.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Wednesday, July 27 2011, 14:17 UTC + * Generated on Friday, August 5 2011, 07:37 UTC */ #ifndef MAVLINK_H #define MAVLINK_H diff --git a/thirdParty/mavlink/include/ualberta/ualberta.h b/thirdParty/mavlink/include/ualberta/ualberta.h index b492ff62d65eead6b12bd73363dcbb4b22791ae2..be77cc2e5d7e6b898d2ec61f8f1760fa65a9c575 100644 --- a/thirdParty/mavlink/include/ualberta/ualberta.h +++ b/thirdParty/mavlink/include/ualberta/ualberta.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://qgroundcontrol.org/mavlink/ - * Generated on Wednesday, July 27 2011, 14:17 UTC + * Generated on Friday, August 5 2011, 07:37 UTC */ #ifndef UALBERTA_H #define UALBERTA_H @@ -71,7 +71,7 @@ enum UALBERTA_PILOT_MODE // MESSAGE LENGTHS #undef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS { 3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 0, 0, 0, 0, 0, 5, 5, 26, 16, 36, 5, 6, 0, 0, 21, 0, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 32, 42, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51 } +#define MAVLINK_MESSAGE_LENGTHS { 3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 14, 14, 0, 0, 0, 5, 5, 26, 16, 36, 5, 6, 56, 0, 21, 18, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 32, 42, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51 } #ifdef __cplusplus } diff --git a/thirdParty/mavlink/message_definitions/ardupilotmega.xml b/thirdParty/mavlink/message_definitions/ardupilotmega.xml index 39d96eab515cf9fb19b6fdf0bf167e369be00a50..d7d6aaacd414de6f984013295fea2e3910d95bea 100644 --- a/thirdParty/mavlink/message_definitions/ardupilotmega.xml +++ b/thirdParty/mavlink/message_definitions/ardupilotmega.xml @@ -1,6 +1,39 @@ - + - common.xml - - + common.xml + + + + Offsets and calibrations values for hardware + sensors. This makes it easier to debug the calibration process. + magnetometer X offset + magnetometer Y offset + magnetometer Z offset + magnetic declination (radians) + raw pressure from barometer + raw temperature from barometer + gyro X calibration + gyro Y calibration + gyro Z calibration + accel X calibration + accel Y calibration + accel Z calibration + + + + set the magnetometer offsets + System ID + Component ID + magnetometer X offset + magnetometer Y offset + magnetometer Z offset + + + diff --git a/thirdParty/mavlink/message_definitions/common.xml b/thirdParty/mavlink/message_definitions/common.xml index 5df9ba827ee9112b0755dd4ece6918f48a71bbb7..dd4ea84430da5a7ceed5ae92bdfaaff54621d75c 100644 --- a/thirdParty/mavlink/message_definitions/common.xml +++ b/thirdParty/mavlink/message_definitions/common.xml @@ -1,892 +1,941 @@ - + -2 - - - - Commands to be executed by the MAV. They can be executed on user request, + 2 + + + Commands to be executed by the MAV. They can be executed on user request, or as part of a mission script. If the action is used in a mission, the parameter mapping to the waypoint/mission message is as follows: Param 1, Param 2, Param 3, Param 4, X: Param 5, Y:Param 6, Z:Param 7. This command list is similar what ARINC 424 is for commercial aircraft: A data format how to interpret waypoint/mission data. - - Navigate to waypoint. - Hold time in decimal seconds. (ignored by fixed wing, time to stay at waypoint for rotary wing) - Acceptance radius in meters (if the sphere with this radius is hit, the waypoint counts as reached) - 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control. - Desired yaw angle at waypoint (rotary wing) - Latitude - Longitude - Altitude - - - Loiter around this waypoint an unlimited amount of time - Empty - Empty - Radius around waypoint, in meters. If positive loiter clockwise, else counter-clockwise - Desired yaw angle. - Latitude - Longitude - Altitude - - - Loiter around this waypoint for X turns - Turns - Empty - Radius around waypoint, in meters. If positive loiter clockwise, else counter-clockwise - Desired yaw angle. - Latitude - Longitude - Altitude - - - Loiter around this waypoint for X seconds - Seconds (decimal) - Empty - Radius around waypoint, in meters. If positive loiter clockwise, else counter-clockwise - Desired yaw angle. - Latitude - Longitude - Altitude - - - Return to launch location - Empty - Empty - Empty - Empty - Empty - Empty - Empty - - - Land at location - Empty - Empty - Empty - Desired yaw angle. - Latitude - Longitude - Altitude - - - Takeoff from ground / hand - Minimum pitch (if airspeed sensor present), desired pitch without sensor - Empty - Empty - Yaw angle (if magnetometer present), ignored without magnetometer - Latitude - Longitude - Altitude - - - - Sets the region of interest (ROI) for a sensor set or the + + Navigate to waypoint. + Hold time in decimal seconds. (ignored by fixed wing, time to stay at waypoint for rotary wing) + Acceptance radius in meters (if the sphere with this radius is hit, the waypoint counts as reached) + 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control. + Desired yaw angle at waypoint (rotary wing) + Latitude + Longitude + Altitude + + + Loiter around this waypoint an unlimited amount of time + Empty + Empty + Radius around waypoint, in meters. If positive loiter clockwise, else counter-clockwise + Desired yaw angle. + Latitude + Longitude + Altitude + + + Loiter around this waypoint for X turns + Turns + Empty + Radius around waypoint, in meters. If positive loiter clockwise, else counter-clockwise + Desired yaw angle. + Latitude + Longitude + Altitude + + + Loiter around this waypoint for X seconds + Seconds (decimal) + Empty + Radius around waypoint, in meters. If positive loiter clockwise, else counter-clockwise + Desired yaw angle. + Latitude + Longitude + Altitude + + + Return to launch location + Empty + Empty + Empty + Empty + Empty + Empty + Empty + + + Land at location + Empty + Empty + Empty + Desired yaw angle. + Latitude + Longitude + Altitude + + + Takeoff from ground / hand + Minimum pitch (if airspeed sensor present), desired pitch without sensor + Empty + Empty + Yaw angle (if magnetometer present), ignored without magnetometer + Latitude + Longitude + Altitude + + + Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. - Region of intereset mode. (see MAV_ROI enum) - Waypoint index/ target ID. (see MAV_ROI enum) - ROI index (allows a vehicle to manage multiple ROI's) - Empty - x the location of the fixed ROI (see MAV_FRAME) - y - z - - - - Control autonomous path planning on the MAV. - 0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning - 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid - Empty - Yaw angle at goal, in compass degrees, [0..360] - Latitude/X of goal - Longitude/Y of goal - Altitude/Z of goal - - - NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration - Empty - Empty - Empty - Empty - Empty - Empty - Empty - - - Delay mission state machine. - Delay in seconds (decimal) - Empty - Empty - Empty - Empty - Empty - Empty - - - Ascend/descend at rate. Delay mission state machine until desired altitude reached. - Descent / Ascend rate (m/s) - Empty - Empty - Empty - Empty - Empty - Finish Altitude - - - Delay mission state machine until within desired distance of next NAV point. - Distance (meters) - Empty - Empty - Empty - Empty - Empty - Empty - - - Reach a certain target angle. - target angle: [0-360], 0 is north - speed during yaw change:[deg per second] - direction: negative: counter clockwise, positive: clockwise [-1,1] - relative offset or absolute angle: [ 1,0] - Empty - Empty - Empty - - - NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration - Empty - Empty - Empty - Empty - Empty - Empty - Empty - - - Set system mode. - Mode, as defined by ENUM MAV_MODE - Empty - Empty - Empty - Empty - Empty - Empty - - - Jump to the desired command in the mission list. Repeat this action only the specified number of times - Sequence number - Repeat count - Empty - Empty - Empty - Empty - Empty - - - Change speed and/or throttle set points. - Speed type (0=Airspeed, 1=Ground Speed) - Speed (m/s, -1 indicates no change) - Throttle ( Percent, -1 indicates no change) - Empty - Empty - Empty - Empty - - - Changes the home location either to the current location or a specified location. - Use current (1=use current location, 0=use specified location) - Empty - Empty - Empty - Latitude - Longitude - Altitude - - - Set a system parameter. Caution! Use of this command requires knowledge of the numeric enumeration value of the parameter. - Parameter number - Parameter value - Empty - Empty - Empty - Empty - Empty - - - Set a relay to a condition. - Relay number - Setting (1=on, 0=off, others possible depending on system hardware) - Empty - Empty - Empty - Empty - Empty - - - Cycle a relay on and off for a desired number of cyles with a desired period. - Relay number - Cycle count - Cycle time (seconds, decimal) - Empty - Empty - Empty - Empty - - - Set a servo to a desired PWM value. - Servo number - PWM (microseconds, 1000 to 2000 typical) - Empty - Empty - Empty - Empty - Empty - - - Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. - Servo number - PWM (microseconds, 1000 to 2000 typical) - Cycle count - Cycle time (seconds) - Empty - Empty - Empty - - - Control onboard camera system. - Camera ID (-1 for all) - Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw - Transmission mode: 0: video stream, >0: single images every n seconds (decimal) - Recording: 0: disabled, 1: enabled compressed, 2: enabled raw - Empty - Empty - Empty - - - NOP - This command is only used to mark the upper limit of the DO commands in the enumeration - Empty - Empty - Empty - Empty - Empty - Empty - Empty - - - Trigger calibration. This command will be only accepted if in pre-flight mode. - Gyro calibration: 0: no, 1: yes - Magnetometer calibration: 0: no, 1: yes - Ground pressure: 0: no, 1: yes - Radio calibration: 0: no, 1: yes - Empty - Empty - Empty - - - Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. - Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM - Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM - Reserved - Reserved - Empty - Empty - Empty - - - - Data stream IDs. A data stream is not a fixed set of messages, but rather a + Region of intereset mode. (see MAV_ROI enum) + Waypoint index/ target ID. (see MAV_ROI enum) + ROI index (allows a vehicle to manage multiple ROI's) + Empty + x the location of the fixed ROI (see MAV_FRAME) + y + z + + + Control autonomous path planning on the MAV. + 0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning + 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid + Empty + Yaw angle at goal, in compass degrees, [0..360] + Latitude/X of goal + Longitude/Y of goal + Altitude/Z of goal + + + NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration + Empty + Empty + Empty + Empty + Empty + Empty + Empty + + + Delay mission state machine. + Delay in seconds (decimal) + Empty + Empty + Empty + Empty + Empty + Empty + + + Ascend/descend at rate. Delay mission state machine until desired altitude reached. + Descent / Ascend rate (m/s) + Empty + Empty + Empty + Empty + Empty + Finish Altitude + + + Delay mission state machine until within desired distance of next NAV point. + Distance (meters) + Empty + Empty + Empty + Empty + Empty + Empty + + + Reach a certain target angle. + target angle: [0-360], 0 is north + speed during yaw change:[deg per second] + direction: negative: counter clockwise, positive: clockwise [-1,1] + relative offset or absolute angle: [ 1,0] + Empty + Empty + Empty + + + NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration + Empty + Empty + Empty + Empty + Empty + Empty + Empty + + + Set system mode. + Mode, as defined by ENUM MAV_MODE + Empty + Empty + Empty + Empty + Empty + Empty + + + Jump to the desired command in the mission list. Repeat this action only the specified number of times + Sequence number + Repeat count + Empty + Empty + Empty + Empty + Empty + + + Change speed and/or throttle set points. + Speed type (0=Airspeed, 1=Ground Speed) + Speed (m/s, -1 indicates no change) + Throttle ( Percent, -1 indicates no change) + Empty + Empty + Empty + Empty + + + Changes the home location either to the current location or a specified location. + Use current (1=use current location, 0=use specified location) + Empty + Empty + Empty + Latitude + Longitude + Altitude + + + Set a system parameter. Caution! Use of this command requires knowledge of the numeric enumeration value of the parameter. + Parameter number + Parameter value + Empty + Empty + Empty + Empty + Empty + + + Set a relay to a condition. + Relay number + Setting (1=on, 0=off, others possible depending on system hardware) + Empty + Empty + Empty + Empty + Empty + + + Cycle a relay on and off for a desired number of cyles with a desired period. + Relay number + Cycle count + Cycle time (seconds, decimal) + Empty + Empty + Empty + Empty + + + Set a servo to a desired PWM value. + Servo number + PWM (microseconds, 1000 to 2000 typical) + Empty + Empty + Empty + Empty + Empty + + + Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. + Servo number + PWM (microseconds, 1000 to 2000 typical) + Cycle count + Cycle time (seconds) + Empty + Empty + Empty + + + Control onboard camera capturing. + Camera ID (-1 for all) + Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw + Transmission mode: 0: video stream, >0: single images every n seconds (decimal) + Recording: 0: disabled, 1: enabled compressed, 2: enabled raw + Empty + Empty + Empty + + + Sets the region of interest (ROI) for a sensor set or the + vehicle itself. This can then be used by the vehicles control + system to control the vehicle attitude and the attitude of various + devices such as cameras. + + Region of interest mode. (see MAV_ROI enum) + Waypoint index/ target ID. (see MAV_ROI enum) + ROI index (allows a vehicle to manage multiple cameras etc.) + Empty + x the location of the fixed ROI (see MAV_FRAME) + y + z + + + NOP - This command is only used to mark the upper limit of the DO commands in the enumeration + Empty + Empty + Empty + Empty + Empty + Empty + Empty + + + Trigger calibration. This command will be only accepted if in pre-flight mode. + Gyro calibration: 0: no, 1: yes + Magnetometer calibration: 0: no, 1: yes + Ground pressure: 0: no, 1: yes + Radio calibration: 0: no, 1: yes + Empty + Empty + Empty + + + Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. + Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM + Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM + Reserved + Reserved + Empty + Empty + Empty + + + + Data stream IDs. A data stream is not a fixed set of messages, but rather a recommendation to the autopilot software. Individual autopilots may or may not obey the recommended messages. - Enable all data streams - Enable IMU_RAW, GPS_RAW, GPS_STATUS packets. - Enable GPS_STATUS, CONTROL_STATUS, AUX_STATUS - Enable RC_CHANNELS_SCALED, RC_CHANNELS_RAW, SERVO_OUTPUT_RAW - Enable ATTITUDE_CONTROLLER_OUTPUT, POSITION_CONTROLLER_OUTPUT, NAV_CONTROLLER_OUTPUT. - Enable LOCAL_POSITION, GLOBAL_POSITION/GLOBAL_POSITION_INT messages. - Dependent on the autopilot - Dependent on the autopilot - Dependent on the autopilot - - - - The ROI (region of interest) for the vehicle. This can be - be used by the vehicle for camera/vehicle attitude alignment (see - MAV_CMD_NAV_ROI). - - Point toward next waypoint. - Point toward given waypoint. - Point toward fixed location. - Point toward of given id. - - - - - - - The heartbeat message shows that a system is present and responding. The type of the MAV and Autopilot hardware allow the receiving system to treat further messages from this system appropriate (e.g. by laying out the user interface based on the autopilot). - Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) - Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM - MAVLink version - - - - The boot message indicates that a system is starting. The onboard software version allows to keep track of onboard soft/firmware revisions. - The onboard software version - - - - The system time is the time of the master clock, typically the computer clock of the main onboard computer. - Timestamp of the master clock in microseconds since UNIX epoch. - - - - A ping message either requesting or responding to a ping. This allows to measure the system latencies, including serial port, radio modem and UDP connections. - PING sequence - 0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system - 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system - Unix timestamp in microseconds - - - - UTC date and time from GPS module - GPS UTC date ddmmyy - GPS UTC time hhmmss - - - - Request to control this MAV - System the GCS requests control for - 0: request control of this MAV, 1: Release control of this MAV - 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. - Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" - - - - Accept / deny control of this MAV - ID of the GCS this message - 0: request control of this MAV, 1: Release control of this MAV - 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control - - - - Emit an encrypted signature / key identifying this system. PLEASE NOTE: This protocol has been kept simple, so transmitting the key requires an encrypted channel for true safety. - key - - - - This message acknowledges an action. IMPORTANT: The acknowledgement can be also negative, e.g. the MAV rejects a reset message because it is in-flight. The action ids are defined in ENUM MAV_ACTION in mavlink/include/mavlink_types.h - The action id - 0: Action DENIED, 1: Action executed - - - - An action message allows to execute a certain onboard action. These include liftoff, land, storing parameters too EEPROM, shutddown, etc. The action ids are defined in ENUM MAV_ACTION in mavlink/include/mavlink_types.h - The system executing the action - The component executing the action - The action id - - - - Set the system mode, as defined by enum MAV_MODE in mavlink/include/mavlink_types.h. There is no target component id as the mode is by definition for the overall aircraft, not only for one component. - The system setting the mode - The new mode - - - - Set the system navigation mode, as defined by enum MAV_NAV_MODE in mavlink/include/mavlink_types.h. The navigation mode applies to the whole aircraft and thus all components. - The system setting the mode - The new navigation mode - - - - Request to read the onboard parameter with the param_id string id. Onboard parameters are stored as key[const char*] -> value[float]. This allows to send a parameter to any other component (such as the GCS) without the need of previous knowledge of possible parameter names. Thus the same GCS can store different parameters for different autopilots. See also http://qgroundcontrol.org/parameter_interface for a full documentation of QGroundControl and IMU code. - System ID - Component ID - Onboard parameter id - Parameter index. Send -1 to use the param ID field as identifier - - - - Request all parameters of this component. After his request, all parameters are emitted. - System ID - Component ID - - - - Emit the value of a onboard parameter. The inclusion of param_count and param_index in the message allows the recipient to keep track of received parameters and allows him to re-request missing parameters after a loss or timeout. - Onboard parameter id - Onboard parameter value - Total number of onboard parameters - Index of this onboard parameter - - - - Set a parameter value TEMPORARILY to RAM. It will be reset to default on system reboot. Send the ACTION MAV_ACTION_STORAGE_WRITE to PERMANENTLY write the RAM contents to EEPROM. IMPORTANT: The receiving component should acknowledge the new parameter value by sending a param_value message to all communication partners. This will also ensure that multiple GCS all have an up-to-date list of all parameters. If the sending GCS did not receive a PARAM_VALUE message within its timeout time, it should re-send the PARAM_SET message. - System ID - Component ID - Onboard parameter id - Onboard parameter value - - - - The global position, as returned by the Global Positioning System (GPS). This is + + Enable all data streams + + + Enable IMU_RAW, GPS_RAW, GPS_STATUS packets. + + + Enable GPS_STATUS, CONTROL_STATUS, AUX_STATUS + + + Enable RC_CHANNELS_SCALED, RC_CHANNELS_RAW, SERVO_OUTPUT_RAW + + + Enable ATTITUDE_CONTROLLER_OUTPUT, POSITION_CONTROLLER_OUTPUT, NAV_CONTROLLER_OUTPUT. + + + Enable LOCAL_POSITION, GLOBAL_POSITION/GLOBAL_POSITION_INT messages. + + + Dependent on the autopilot + + + Dependent on the autopilot + + + Dependent on the autopilot + + + + The ROI (region of interest) for the vehicle. This can be + be used by the vehicle for camera/vehicle attitude alignment (see + MAV_CMD_NAV_ROI). + + + No region of interest. + + + Point toward next waypoint. + + + Point toward given waypoint. + + + Point toward fixed location. + + + Point toward of given id. + + + + + + The heartbeat message shows that a system is present and responding. The type of the MAV and Autopilot hardware allow the receiving system to treat further messages from this system appropriate (e.g. by laying out the user interface based on the autopilot). + Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM + MAVLink version + + + The boot message indicates that a system is starting. The onboard software version allows to keep track of onboard soft/firmware revisions. + The onboard software version + + + The system time is the time of the master clock, typically the computer clock of the main onboard computer. + Timestamp of the master clock in microseconds since UNIX epoch. + + + A ping message either requesting or responding to a ping. This allows to measure the system latencies, including serial port, radio modem and UDP connections. + PING sequence + 0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system + 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system + Unix timestamp in microseconds + + + UTC date and time from GPS module + GPS UTC date ddmmyy + GPS UTC time hhmmss + + + Request to control this MAV + System the GCS requests control for + 0: request control of this MAV, 1: Release control of this MAV + 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. + Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" + + + Accept / deny control of this MAV + ID of the GCS this message + 0: request control of this MAV, 1: Release control of this MAV + 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control + + + Emit an encrypted signature / key identifying this system. PLEASE NOTE: This protocol has been kept simple, so transmitting the key requires an encrypted channel for true safety. + key + + + This message acknowledges an action. IMPORTANT: The acknowledgement can be also negative, e.g. the MAV rejects a reset message because it is in-flight. The action ids are defined in ENUM MAV_ACTION in mavlink/include/mavlink_types.h + The action id + 0: Action DENIED, 1: Action executed + + + An action message allows to execute a certain onboard action. These include liftoff, land, storing parameters too EEPROM, shutddown, etc. The action ids are defined in ENUM MAV_ACTION in mavlink/include/mavlink_types.h + The system executing the action + The component executing the action + The action id + + + Set the system mode, as defined by enum MAV_MODE in mavlink/include/mavlink_types.h. There is no target component id as the mode is by definition for the overall aircraft, not only for one component. + The system setting the mode + The new mode + + + Set the system navigation mode, as defined by enum MAV_NAV_MODE in mavlink/include/mavlink_types.h. The navigation mode applies to the whole aircraft and thus all components. + The system setting the mode + The new navigation mode + + + Request to read the onboard parameter with the param_id string id. Onboard parameters are stored as key[const char*] -> value[float]. This allows to send a parameter to any other component (such as the GCS) without the need of previous knowledge of possible parameter names. Thus the same GCS can store different parameters for different autopilots. See also http://qgroundcontrol.org/parameter_interface for a full documentation of QGroundControl and IMU code. + System ID + Component ID + Onboard parameter id + Parameter index. Send -1 to use the param ID field as identifier + + + Request all parameters of this component. After his request, all parameters are emitted. + System ID + Component ID + + + Emit the value of a onboard parameter. The inclusion of param_count and param_index in the message allows the recipient to keep track of received parameters and allows him to re-request missing parameters after a loss or timeout. + Onboard parameter id + Onboard parameter value + Total number of onboard parameters + Index of this onboard parameter + + + Set a parameter value TEMPORARILY to RAM. It will be reset to default on system reboot. Send the ACTION MAV_ACTION_STORAGE_WRITE to PERMANENTLY write the RAM contents to EEPROM. IMPORTANT: The receiving component should acknowledge the new parameter value by sending a param_value message to all communication partners. This will also ensure that multiple GCS all have an up-to-date list of all parameters. If the sending GCS did not receive a PARAM_VALUE message within its timeout time, it should re-send the PARAM_SET message. + System ID + Component ID + Onboard parameter id + Onboard parameter value + + + The global position, as returned by the Global Positioning System (GPS). This is NOT the global position estimate of the sytem, but rather a RAW sensor value. See message GLOBAL_POSITION for the global position estimate. Coordinate frame is right-handed, Z-axis up (GPS frame) - Timestamp (microseconds since UNIX epoch or microseconds since system boot) - 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. - Latitude in 1E7 degrees - Longitude in 1E7 degrees - Altitude in 1E3 meters (millimeters) - GPS HDOP - GPS VDOP - GPS ground speed (m/s) - Compass heading in degrees, 0..360 degrees - - - - The RAW IMU readings for the usual 9DOF sensor setup. This message should contain the scaled values to the described units - Timestamp (microseconds since UNIX epoch or microseconds since system boot) - X acceleration (mg) - Y acceleration (mg) - Z acceleration (mg) - Angular speed around X axis (millirad /sec) - Angular speed around Y axis (millirad /sec) - Angular speed around Z axis (millirad /sec) - X Magnetic field (milli tesla) - Y Magnetic field (milli tesla) - Z Magnetic field (milli tesla) - - - - The positioning status, as reported by GPS. This message is intended to display status information about each satellite visible to the receiver. See message GLOBAL_POSITION for the global position estimate. This message can contain information for up to 20 satellites. - Number of satellites visible - Global satellite ID - 0: Satellite not used, 1: used for localization - Elevation (0: right on top of receiver, 90: on the horizon) of satellite - Direction of satellite, 0: 0 deg, 255: 360 deg. - Signal to noise ratio of satellite - - - - The RAW IMU readings for the usual 9DOF sensor setup. This message should always contain the true raw values without any scaling to allow data capture and system debugging. - Timestamp (microseconds since UNIX epoch or microseconds since system boot) - X acceleration (raw) - Y acceleration (raw) - Z acceleration (raw) - Angular speed around X axis (raw) - Angular speed around Y axis (raw) - Angular speed around Z axis (raw) - X Magnetic field (raw) - Y Magnetic field (raw) - Z Magnetic field (raw) - - - - The RAW pressure readings for the typical setup of one absolute pressure and one differential pressure sensor. The sensor values should be the raw, UNSCALED ADC values. - Timestamp (microseconds since UNIX epoch or microseconds since system boot) - Absolute pressure (raw) - Differential pressure 1 (raw) - Differential pressure 2 (raw) - Raw Temperature measurement (raw) - - - - The pressure readings for the typical setup of one absolute and differential pressure sensor. The units are as specified in each field. - Timestamp (microseconds since UNIX epoch or microseconds since system boot) - Absolute pressure (hectopascal) - Differential pressure 1 (hectopascal) - Temperature measurement (0.01 degrees celsius) - - - - - The attitude in the aeronautical frame (right-handed, Z-down, X-front, Y-right). - Timestamp (microseconds since UNIX epoch or microseconds since system boot) - Roll angle (rad) - Pitch angle (rad) - Yaw angle (rad) - Roll angular speed (rad/s) - Pitch angular speed (rad/s) - Yaw angular speed (rad/s) - - - - The filtered local position (e.g. fused computer vision and accelerometers). Coordinate frame is right-handed, Z-axis down (aeronautical frame) - Timestamp (microseconds since UNIX epoch or microseconds since system boot) - X Position - Y Position - Z Position - X Speed - Y Speed - Z Speed - - - - - The filtered global position (e.g. fused GPS and accelerometers). Coordinate frame is right-handed, Z-axis up (GPS frame) - Timestamp (microseconds since unix epoch) - Latitude, in degrees - Longitude, in degrees - Absolute altitude, in meters - X Speed (in Latitude direction, positive: going north) - Y Speed (in Longitude direction, positive: going east) - Z Speed (in Altitude direction, positive: going up) - - - - The global position, as returned by the Global Positioning System (GPS). This is + Timestamp (microseconds since UNIX epoch or microseconds since system boot) + 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + Latitude in 1E7 degrees + Longitude in 1E7 degrees + Altitude in 1E3 meters (millimeters) + GPS HDOP + GPS VDOP + GPS ground speed (m/s) + Compass heading in degrees, 0..360 degrees + + + The RAW IMU readings for the usual 9DOF sensor setup. This message should contain the scaled values to the described units + Timestamp (microseconds since UNIX epoch or microseconds since system boot) + X acceleration (mg) + Y acceleration (mg) + Z acceleration (mg) + Angular speed around X axis (millirad /sec) + Angular speed around Y axis (millirad /sec) + Angular speed around Z axis (millirad /sec) + X Magnetic field (milli tesla) + Y Magnetic field (milli tesla) + Z Magnetic field (milli tesla) + + + The positioning status, as reported by GPS. This message is intended to display status information about each satellite visible to the receiver. See message GLOBAL_POSITION for the global position estimate. This message can contain information for up to 20 satellites. + Number of satellites visible + Global satellite ID + 0: Satellite not used, 1: used for localization + Elevation (0: right on top of receiver, 90: on the horizon) of satellite + Direction of satellite, 0: 0 deg, 255: 360 deg. + Signal to noise ratio of satellite + + + The RAW IMU readings for the usual 9DOF sensor setup. This message should always contain the true raw values without any scaling to allow data capture and system debugging. + Timestamp (microseconds since UNIX epoch or microseconds since system boot) + X acceleration (raw) + Y acceleration (raw) + Z acceleration (raw) + Angular speed around X axis (raw) + Angular speed around Y axis (raw) + Angular speed around Z axis (raw) + X Magnetic field (raw) + Y Magnetic field (raw) + Z Magnetic field (raw) + + + The RAW pressure readings for the typical setup of one absolute pressure and one differential pressure sensor. The sensor values should be the raw, UNSCALED ADC values. + Timestamp (microseconds since UNIX epoch or microseconds since system boot) + Absolute pressure (raw) + Differential pressure 1 (raw) + Differential pressure 2 (raw) + Raw Temperature measurement (raw) + + + The pressure readings for the typical setup of one absolute and differential pressure sensor. The units are as specified in each field. + Timestamp (microseconds since UNIX epoch or microseconds since system boot) + Absolute pressure (hectopascal) + Differential pressure 1 (hectopascal) + Temperature measurement (0.01 degrees celsius) + + + The attitude in the aeronautical frame (right-handed, Z-down, X-front, Y-right). + Timestamp (microseconds since UNIX epoch or microseconds since system boot) + Roll angle (rad) + Pitch angle (rad) + Yaw angle (rad) + Roll angular speed (rad/s) + Pitch angular speed (rad/s) + Yaw angular speed (rad/s) + + + The filtered local position (e.g. fused computer vision and accelerometers). Coordinate frame is right-handed, Z-axis down (aeronautical frame) + Timestamp (microseconds since UNIX epoch or microseconds since system boot) + X Position + Y Position + Z Position + X Speed + Y Speed + Z Speed + + + The filtered global position (e.g. fused GPS and accelerometers). Coordinate frame is right-handed, Z-axis up (GPS frame) + Timestamp (microseconds since unix epoch) + Latitude, in degrees + Longitude, in degrees + Absolute altitude, in meters + X Speed (in Latitude direction, positive: going north) + Y Speed (in Longitude direction, positive: going east) + Z Speed (in Altitude direction, positive: going up) + + + The global position, as returned by the Global Positioning System (GPS). This is NOT the global position estimate of the sytem, but rather a RAW sensor value. See message GLOBAL_POSITION for the global position estimate. Coordinate frame is right-handed, Z-axis up (GPS frame) - Timestamp (microseconds since UNIX epoch or microseconds since system boot) - 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. - Latitude in degrees - Longitude in degrees - Altitude in meters - GPS HDOP - GPS VDOP - GPS ground speed - Compass heading in degrees, 0..360 degrees - - - - The general system state. If the system is following the MAVLink standard, the system state is mainly defined by three orthogonal states/modes: The system mode, which is either LOCKED (motors shut down and locked), MANUAL (system under RC control), GUIDED (system with autonomous position control, position setpoint controlled manually) or AUTO (system guided by path/waypoint planner). The NAV_MODE defined the current flight state: LIFTOFF (often an open-loop maneuver), LANDING, WAYPOINTS or VECTOR. This represents the internal navigation state machine. The system status shows wether the system is currently active or not and if an emergency occured. During the CRITICAL and EMERGENCY states the MAV is still considered to be active, but should start emergency procedures autonomously. After a failure occured it should first move from active to critical to allow manual intervention and then move to emergency after a certain timeout. - System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h - Navigation mode, see MAV_NAV_MODE ENUM - System status flag, see MAV_STATUS ENUM - Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - Battery voltage, in millivolts (1 = 1 millivolt) - Remaining battery energy: (0%: 0, 100%: 1000) - Dropped packets (packets that were corrupted on reception on the MAV) - - - - The RAW values of the RC channels received. The standard PPM modulation is as follows: 1000 microseconds: 0%, 2000 microseconds: 100%. Individual receivers/transmitters might violate this specification. - RC channel 1 value, in microseconds - RC channel 2 value, in microseconds - RC channel 3 value, in microseconds - RC channel 4 value, in microseconds - RC channel 5 value, in microseconds - RC channel 6 value, in microseconds - RC channel 7 value, in microseconds - RC channel 8 value, in microseconds - Receive signal strength indicator, 0: 0%, 255: 100% - - - - The scaled values of the RC channels received. (-100%) -10000, (0%) 0, (100%) 10000 - RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - Receive signal strength indicator, 0: 0%, 255: 100% - - - - The RAW values of the servo outputs (for RC input from the remote, use the RC_CHANNELS messages). The standard PPM modulation is as follows: 1000 microseconds: 0%, 2000 microseconds: 100%. - Servo output 1 value, in microseconds - Servo output 2 value, in microseconds - Servo output 3 value, in microseconds - Servo output 4 value, in microseconds - Servo output 5 value, in microseconds - Servo output 6 value, in microseconds - Servo output 7 value, in microseconds - Servo output 8 value, in microseconds - - - - Message encoding a waypoint. This message is emitted to announce + Timestamp (microseconds since UNIX epoch or microseconds since system boot) + 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + Latitude in degrees + Longitude in degrees + Altitude in meters + GPS HDOP + GPS VDOP + GPS ground speed + Compass heading in degrees, 0..360 degrees + + + The general system state. If the system is following the MAVLink standard, the system state is mainly defined by three orthogonal states/modes: The system mode, which is either LOCKED (motors shut down and locked), MANUAL (system under RC control), GUIDED (system with autonomous position control, position setpoint controlled manually) or AUTO (system guided by path/waypoint planner). The NAV_MODE defined the current flight state: LIFTOFF (often an open-loop maneuver), LANDING, WAYPOINTS or VECTOR. This represents the internal navigation state machine. The system status shows wether the system is currently active or not and if an emergency occured. During the CRITICAL and EMERGENCY states the MAV is still considered to be active, but should start emergency procedures autonomously. After a failure occured it should first move from active to critical to allow manual intervention and then move to emergency after a certain timeout. + System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h + Navigation mode, see MAV_NAV_MODE ENUM + System status flag, see MAV_STATUS ENUM + Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + Battery voltage, in millivolts (1 = 1 millivolt) + Remaining battery energy: (0%: 0, 100%: 1000) + Dropped packets (packets that were corrupted on reception on the MAV) + + + The RAW values of the RC channels received. The standard PPM modulation is as follows: 1000 microseconds: 0%, 2000 microseconds: 100%. Individual receivers/transmitters might violate this specification. + RC channel 1 value, in microseconds + RC channel 2 value, in microseconds + RC channel 3 value, in microseconds + RC channel 4 value, in microseconds + RC channel 5 value, in microseconds + RC channel 6 value, in microseconds + RC channel 7 value, in microseconds + RC channel 8 value, in microseconds + Receive signal strength indicator, 0: 0%, 255: 100% + + + The scaled values of the RC channels received. (-100%) -10000, (0%) 0, (100%) 10000 + RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + Receive signal strength indicator, 0: 0%, 255: 100% + + + The RAW values of the servo outputs (for RC input from the remote, use the RC_CHANNELS messages). The standard PPM modulation is as follows: 1000 microseconds: 0%, 2000 microseconds: 100%. + Servo output 1 value, in microseconds + Servo output 2 value, in microseconds + Servo output 3 value, in microseconds + Servo output 4 value, in microseconds + Servo output 5 value, in microseconds + Servo output 6 value, in microseconds + Servo output 7 value, in microseconds + Servo output 8 value, in microseconds + + + Message encoding a waypoint. This message is emitted to announce the presence of a waypoint and to set a waypoint on the system. The waypoint can be either in x, y, z meters (type: LOCAL) or x:lat, y:lon, z:altitude. Local frame is Z-down, right handed, global frame is Z-up, right handed - System ID - Component ID - Sequence - The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h - The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs - false:0, true:1 - autocontinue to next wp - PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters - PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds - PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. - PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH - PARAM5 / local: x position, global: latitude - PARAM6 / y position: global: longitude - PARAM7 / z position: global: altitude - - - - Request the information of the waypoint with the sequence number seq. The response of the system to this message should be a WAYPOINT message. - System ID - Component ID - Sequence - - - - Set the waypoint with sequence number seq as current waypoint. This means that the MAV will continue to this waypoint on the shortest path (not following the waypoints in-between). - System ID - Component ID - Sequence - - - - Message that announces the sequence number of the current active waypoint. The MAV will fly towards this waypoint. - Sequence - - - - Request the overall list of waypoints from the system/component. - System ID - Component ID - - - - This message is emitted as response to WAYPOINT_REQUEST_LIST by the MAV. The GCS can then request the individual waypoints based on the knowledge of the total number of waypoints. - System ID - Component ID - Number of Waypoints in the Sequence - - - - Delete all waypoints at once. - System ID - Component ID - - - - A certain waypoint has been reached. The system will either hold this position (or circle on the orbit) or (if the autocontinue on the WP was set) continue to the next waypoint. - Sequence - - - - Ack message during waypoint handling. The type field states if this message is a positive ack (type=0) or if an error happened (type=non-zero). - System ID - Component ID - 0: OK, 1: Error - - - - As local waypoints exist, the global waypoint reference allows to transform between the local coordinate frame and the global (GPS) coordinate frame. This can be necessary when e.g. in- and outdoor settings are connected and the MAV should move from in- to outdoor. - System ID - Component ID - global position * 1E7 - global position * 1E7 - global position * 1000 - - - - Once the MAV sets a new GPS-Local correspondence, this message announces the origin (0,0,0) position - Latitude (WGS84), expressed as * 1E7 - Longitude (WGS84), expressed as * 1E7 - Altitude(WGS84), expressed as * 1000 - - - - Set the setpoint for a local position controller. This is the position in local coordinates the MAV should fly to. This message is sent by the path/waypoint planner to the onboard position controller. As some MAVs have a degree of freedom in yaw (e.g. all helicopters/quadrotors), the desired yaw angle is part of the message. - System ID - Component ID - x position - y position - z position - Desired yaw angle - - - - Transmit the current local setpoint of the controller to other MAVs (collision avoidance) and to the GCS. - x position - y position - z position - Desired yaw angle - - - - Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix - Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix - GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix - Attitude estimation health: 0: poor, 255: excellent - 0: Attitude control disabled, 1: enabled - 0: X, Y position control disabled, 1: enabled - 0: Z position control disabled, 1: enabled - 0: Yaw angle control disabled, 1: enabled - - - - Set a safety zone (volume), which is defined by two corners of a cube. This message can be used to tell the MAV which setpoints/waypoints to accept and which to reject. Safety areas are often enforced by national or competition regulations. - System ID - Component ID - Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. - x position 1 / Latitude 1 - y position 1 / Longitude 1 - z position 1 / Altitude 1 - x position 2 / Latitude 2 - y position 2 / Longitude 2 - z position 2 / Altitude 2 - - - - Read out the safety zone the MAV currently assumes. - Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. - x position 1 / Latitude 1 - y position 1 / Longitude 1 - z position 1 / Altitude 1 - x position 2 / Latitude 2 - y position 2 / Longitude 2 - z position 2 / Altitude 2 - - - - The output of the attitude controller. This output is the control response the controller currently generates and the attitude the MAV would take if it is under control of the attitude controller. The primary use of this message is to check the response and signs of the controller before the actual flight. - 1: enabled, 0: disabled - Attitude roll: -128: -100%, 127: +100% - Attitude pitch: -128: -100%, 127: +100% - Attitude yaw: -128: -100%, 127: +100% - Attitude thrust: -128: -100%, 127: +100% - - - - The output of the position controller. The primary use of this message is to check the response and signs of the controller before the actual flight. - 1: enabled, 0: disabled - Position x: -128: -100%, 127: +100% - Position y: -128: -100%, 127: +100% - Position z: -128: -100%, 127: +100% - Position yaw: -128: -100%, 127: +100% - - - - Outputs of the APM navigation controller. The primary use of this message is to check the response and signs + System ID + Component ID + Sequence + The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h + The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs + false:0, true:1 + autocontinue to next wp + PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters + PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds + PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. + PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH + PARAM5 / local: x position, global: latitude + PARAM6 / y position: global: longitude + PARAM7 / z position: global: altitude + + + Request the information of the waypoint with the sequence number seq. The response of the system to this message should be a WAYPOINT message. + System ID + Component ID + Sequence + + + Set the waypoint with sequence number seq as current waypoint. This means that the MAV will continue to this waypoint on the shortest path (not following the waypoints in-between). + System ID + Component ID + Sequence + + + Message that announces the sequence number of the current active waypoint. The MAV will fly towards this waypoint. + Sequence + + + Request the overall list of waypoints from the system/component. + System ID + Component ID + + + This message is emitted as response to WAYPOINT_REQUEST_LIST by the MAV. The GCS can then request the individual waypoints based on the knowledge of the total number of waypoints. + System ID + Component ID + Number of Waypoints in the Sequence + + + Delete all waypoints at once. + System ID + Component ID + + + A certain waypoint has been reached. The system will either hold this position (or circle on the orbit) or (if the autocontinue on the WP was set) continue to the next waypoint. + Sequence + + + Ack message during waypoint handling. The type field states if this message is a positive ack (type=0) or if an error happened (type=non-zero). + System ID + Component ID + 0: OK, 1: Error + + + As local waypoints exist, the global waypoint reference allows to transform between the local coordinate frame and the global (GPS) coordinate frame. This can be necessary when e.g. in- and outdoor settings are connected and the MAV should move from in- to outdoor. + System ID + Component ID + global position * 1E7 + global position * 1E7 + global position * 1000 + + + Once the MAV sets a new GPS-Local correspondence, this message announces the origin (0,0,0) position + Latitude (WGS84), expressed as * 1E7 + Longitude (WGS84), expressed as * 1E7 + Altitude(WGS84), expressed as * 1000 + + + Set the setpoint for a local position controller. This is the position in local coordinates the MAV should fly to. This message is sent by the path/waypoint planner to the onboard position controller. As some MAVs have a degree of freedom in yaw (e.g. all helicopters/quadrotors), the desired yaw angle is part of the message. + System ID + Component ID + x position + y position + z position + Desired yaw angle + + + Transmit the current local setpoint of the controller to other MAVs (collision avoidance) and to the GCS. + x position + y position + z position + Desired yaw angle + + + Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix + Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix + GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix + Attitude estimation health: 0: poor, 255: excellent + 0: Attitude control disabled, 1: enabled + 0: X, Y position control disabled, 1: enabled + 0: Z position control disabled, 1: enabled + 0: Yaw angle control disabled, 1: enabled + + + Set a safety zone (volume), which is defined by two corners of a cube. This message can be used to tell the MAV which setpoints/waypoints to accept and which to reject. Safety areas are often enforced by national or competition regulations. + System ID + Component ID + Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + x position 1 / Latitude 1 + y position 1 / Longitude 1 + z position 1 / Altitude 1 + x position 2 / Latitude 2 + y position 2 / Longitude 2 + z position 2 / Altitude 2 + + + Read out the safety zone the MAV currently assumes. + Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + x position 1 / Latitude 1 + y position 1 / Longitude 1 + z position 1 / Altitude 1 + x position 2 / Latitude 2 + y position 2 / Longitude 2 + z position 2 / Altitude 2 + + + Set roll, pitch and yaw. + System ID + Component ID + Desired roll angle in radians + Desired pitch angle in radians + Desired yaw angle in radians + Collective thrust, normalized to 0 .. 1 + + + Set roll, pitch and yaw. + System ID + Component ID + Desired roll angular speed in rad/s + Desired pitch angular speed in rad/s + Desired yaw angular speed in rad/s + Collective thrust, normalized to 0 .. 1 + + + Setpoint in roll, pitch, yaw currently active on the system. + Timestamp in micro seconds since unix epoch + Desired roll angle in radians + Desired pitch angle in radians + Desired yaw angle in radians + Collective thrust, normalized to 0 .. 1 + + + Setpoint in rollspeed, pitchspeed, yawspeed currently active on the system. + Timestamp in micro seconds since unix epoch + Desired roll angular speed in rad/s + Desired pitch angular speed in rad/s + Desired yaw angular speed in rad/s + Collective thrust, normalized to 0 .. 1 + + + Outputs of the APM navigation controller. The primary use of this message is to check the response and signs of the controller before actual flight and to assist with tuning controller parameters - Current desired roll in degrees - Current desired pitch in degrees - Current desired heading in degrees - Bearing to current waypoint/target in degrees - Distance to active waypoint in meters - Current altitude error in meters - Current airspeed error in meters/second - Current crosstrack error on x-y plane in meters - - - - The goal position of the system. This position is the input to any navigation or path planning algorithm and does NOT represent the current controller setpoint. - x position - y position - z position - yaw orientation in radians, 0 = NORTH - - - - Corrects the systems state by adding an error correction term to the position and velocity, and by rotating the attitude by a correction angle. - x position error - y position error - z position error - roll error (radians) - pitch error (radians) - yaw error (radians) - x velocity - y velocity - z velocity - - - - The system setting the altitude - The new altitude in meters - - - - The target requested to send the message stream. - The target requested to send the message stream. - The ID of the requested message type - The requested interval between two messages of this type - 1 to start sending, 0 to stop sending. - - - - The system to be controlled - roll - pitch - yaw - thrust - roll control enabled auto:0, manual:1 - pitch auto:0, manual:1 - yaw auto:0, manual:1 - thrust auto:0, manual:1 - - - - The filtered global position (e.g. fused GPS and accelerometers). The position is in GPS-frame (right-handed, Z-up) - Latitude, expressed as * 1E7 - Longitude, expressed as * 1E7 - Altitude in meters, expressed as * 1000 (millimeters) - Ground X Speed (Latitude), expressed as m/s * 100 - Ground Y Speed (Longitude), expressed as m/s * 100 - Ground Z Speed (Altitude), expressed as m/s * 100 - - - - Metrics typically displayed on a HUD for fixed wing aircraft - Current airspeed in m/s - Current ground speed in m/s - Current heading in degrees, in compass units (0..360, 0=north) - Current throttle setting in integer percent, 0 to 100 - Current altitude (MSL), in meters - Current climb rate in meters/second - - - - Send a command with up to four parameters to the MAV - System which should execute the command - Component which should execute the command, 0 for all components - Command ID, as defined by MAV_CMD enum. - 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) - Parameter 1, as defined by MAV_CMD enum. - Parameter 2, as defined by MAV_CMD enum. - Parameter 3, as defined by MAV_CMD enum. - Parameter 4, as defined by MAV_CMD enum. - - - - Report status of a command. Includes feedback wether the command was executed - Current airspeed in m/s - 1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION - - - - - - - - - Name - Timestamp - x - y - z - - - - Send a key-value pair as float. The use of this message is discouraged for normal packets, but a quite efficient way for testing new messages and getting experimental debug output. - Name of the debug variable - Floating point value - - - - Send a key-value pair as integer. The use of this message is discouraged for normal packets, but a quite efficient way for testing new messages and getting experimental debug output. - Name of the debug variable - Signed integer value - - - - Status text message. These messages are printed in yellow in the COMM console of QGroundControl. WARNING: They consume quite some bandwidth, so use only for important status and error messages. If implemented wisely, these messages are buffered on the MCU and sent only at a limited rate (e.g. 10 Hz). - Severity of status, 0 = info message, 255 = critical fault - Status text message, without null termination character - - - - Send a debug value. The index is used to discriminate between values. These values show up in the plot of QGroundControl as DEBUG N. - index of debug variable - DEBUG value - - - + Current desired roll in degrees + Current desired pitch in degrees + Current desired heading in degrees + Bearing to current waypoint/target in degrees + Distance to active waypoint in meters + Current altitude error in meters + Current airspeed error in meters/second + Current crosstrack error on x-y plane in meters + + + The goal position of the system. This position is the input to any navigation or path planning algorithm and does NOT represent the current controller setpoint. + x position + y position + z position + yaw orientation in radians, 0 = NORTH + + + Corrects the systems state by adding an error correction term to the position and velocity, and by rotating the attitude by a correction angle. + x position error + y position error + z position error + roll error (radians) + pitch error (radians) + yaw error (radians) + x velocity + y velocity + z velocity + + + The system setting the altitude + The new altitude in meters + + + The target requested to send the message stream. + The target requested to send the message stream. + The ID of the requested message type + Update rate in Hertz + 1 to start sending, 0 to stop sending. + + + This packet is useful for high throughput + applications such as hardware in the loop simulations. + + Timestamp (microseconds since UNIX epoch or microseconds since system boot) + Roll angle (rad) + Pitch angle (rad) + Yaw angle (rad) + Roll angular speed (rad/s) + Pitch angular speed (rad/s) + Yaw angular speed (rad/s) + Latitude, expressed as * 1E7 + Longitude, expressed as * 1E7 + Altitude in meters, expressed as * 1000 (millimeters) + Ground X Speed (Latitude), expressed as m/s * 100 + Ground Y Speed (Longitude), expressed as m/s * 100 + Ground Z Speed (Altitude), expressed as m/s * 100 + X acceleration (mg) + Y acceleration (mg) + Z acceleration (mg) + + + Hardware in the loop control outputs + Timestamp (microseconds since UNIX epoch or microseconds since system boot) + Control output -3 .. 1 + Control output -1 .. 1 + Control output -1 .. 1 + Throttle 0 .. 1 + System mode (MAV_MODE) + Navigation mode (MAV_NAV_MODE) + + + The system to be controlled + roll + pitch + yaw + thrust + roll control enabled auto:0, manual:1 + pitch auto:0, manual:1 + yaw auto:0, manual:1 + thrust auto:0, manual:1 + + + The RAW values of the RC channels sent to the MAV to override info received from the RC radio. A value of -1 means no change to that channel. A value of 0 means control of that channel should be released back to the RC radio. The standard PPM modulation is as follows: 1000 microseconds: 0%, 2000 microseconds: 100%. Individual receivers/transmitters might violate this specification. + System ID + Component ID + RC channel 1 value, in microseconds + RC channel 2 value, in microseconds + RC channel 3 value, in microseconds + RC channel 4 value, in microseconds + RC channel 5 value, in microseconds + RC channel 6 value, in microseconds + RC channel 7 value, in microseconds + RC channel 8 value, in microseconds + + + The filtered global position (e.g. fused GPS and accelerometers). The position is in GPS-frame (right-handed, Z-up) + Latitude, expressed as * 1E7 + Longitude, expressed as * 1E7 + Altitude in meters, expressed as * 1000 (millimeters) + Ground X Speed (Latitude), expressed as m/s * 100 + Ground Y Speed (Longitude), expressed as m/s * 100 + Ground Z Speed (Altitude), expressed as m/s * 100 + + + Metrics typically displayed on a HUD for fixed wing aircraft + Current airspeed in m/s + Current ground speed in m/s + Current heading in degrees, in compass units (0..360, 0=north) + Current throttle setting in integer percent, 0 to 100 + Current altitude (MSL), in meters + Current climb rate in meters/second + + + Send a command with up to four parameters to the MAV + System which should execute the command + Component which should execute the command, 0 for all components + Command ID, as defined by MAV_CMD enum. + 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) + Parameter 1, as defined by MAV_CMD enum. + Parameter 2, as defined by MAV_CMD enum. + Parameter 3, as defined by MAV_CMD enum. + Parameter 4, as defined by MAV_CMD enum. + + + Report status of a command. Includes feedback wether the command was executed + Current airspeed in m/s + 1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION + + + Optical flow from a flow sensor (e.g. optical mouse sensor) + Timestamp (UNIX) + Sensor ID + Flow in pixels in x-sensor direction + Flow in pixels in y-sensor direction + Optical flow quality / confidence. 0: bad, 255: maximum quality + Ground distance in meters + + + Object has been detected + Timestamp in milliseconds since system boot + Object ID + Object type: 0: image, 1: letter, 2: ground vehicle, 3: air vehicle, 4: surface vehicle, 5: sub-surface vehicle, 6: human, 7: animal + Name of the object as defined by the detector + Detection quality / confidence. 0: bad, 255: maximum confidence + Angle of the object with respect to the body frame in NED coordinates in radians. 0: front + Ground distance in meters + + + + Name + Timestamp + x + y + z + + + Send a key-value pair as float. The use of this message is discouraged for normal packets, but a quite efficient way for testing new messages and getting experimental debug output. + Name of the debug variable + Floating point value + + + Send a key-value pair as integer. The use of this message is discouraged for normal packets, but a quite efficient way for testing new messages and getting experimental debug output. + Name of the debug variable + Signed integer value + + + Status text message. These messages are printed in yellow in the COMM console of QGroundControl. WARNING: They consume quite some bandwidth, so use only for important status and error messages. If implemented wisely, these messages are buffered on the MCU and sent only at a limited rate (e.g. 10 Hz). + Severity of status, 0 = info message, 255 = critical fault + Status text message, without null termination character + + + Send a debug value. The index is used to discriminate between values. These values show up in the plot of QGroundControl as DEBUG N. + index of debug variable + DEBUG value + + diff --git a/thirdParty/mavlink/message_definitions/minimal.xml b/thirdParty/mavlink/message_definitions/minimal.xml index 16d26831e165d7c8ff3410cbab22accfd34311ca..5b41a4909090112233c5b91d1fa3067d7ffbdcbf 100644 --- a/thirdParty/mavlink/message_definitions/minimal.xml +++ b/thirdParty/mavlink/message_definitions/minimal.xml @@ -1,13 +1,13 @@ - 2 - - - - The heartbeat message shows that a system is present and responding. The type of the MAV and Autopilot hardware allow the receiving system to treat further messages from this system appropriate (e.g. by laying out the user interface based on the autopilot). - Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) - Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM - MAVLink version - - + 2 + + + + The heartbeat message shows that a system is present and responding. The type of the MAV and Autopilot hardware allow the receiving system to treat further messages from this system appropriate (e.g. by laying out the user interface based on the autopilot). + Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM + MAVLink version + + diff --git a/thirdParty/mavlink/message_definitions/pixhawk.xml b/thirdParty/mavlink/message_definitions/pixhawk.xml index e2a99a738b666758d65670e47de52f181422b5ac..7bed667831bbd9a119ceda537f25dfdd12d7db33 100644 --- a/thirdParty/mavlink/message_definitions/pixhawk.xml +++ b/thirdParty/mavlink/message_definitions/pixhawk.xml @@ -1,262 +1,276 @@ -common.xml + common.xml + + + + Content Types for data transmission handshake + + + + + + + + + + + + The system to be controlled + roll + pitch + yaw + thrust + roll control enabled auto:0, manual:1 + pitch auto:0, manual:1 + yaw auto:0, manual:1 + thrust auto:0, manual:1 + + + + Camera id + Camera mode: 0 = auto, 1 = manual + Trigger pin, 0-3 for PtGrey FireFly + Shutter interval, in microseconds + Exposure time, in microseconds + Camera gain + + + + Timestamp + IMU seq + Roll angle in rad + Pitch angle in rad + Yaw angle in rad + Local frame Z coordinate (height over ground) + GPS X coordinate + GPS Y coordinate + Global frame altitude + Ground truth X + Ground truth Y + Ground truth Z + + + + 0 to disable, 1 to enable + + + + Camera id + Camera # (starts with 0) + Timestamp + Until which timestamp this buffer will stay valid + The image sequence number + Position of the image in the buffer, starts with 0 + Image width + Image height + Image depth + Image channels + Shared memory area key + Exposure time, in microseconds + Camera gain + Roll angle in rad + Pitch angle in rad + Yaw angle in rad + Local frame Z coordinate (height over ground) + GPS X coordinate + GPS Y coordinate + Global frame altitude + Ground truth X + Ground truth Y + Ground truth Z + + + + Timestamp (milliseconds) + Global X position + Global Y position + Global Z position + Roll angle in rad + Pitch angle in rad + Yaw angle in rad + + + + Timestamp (milliseconds) + Global X position + Global Y position + Global Z position + Roll angle in rad + Pitch angle in rad + Yaw angle in rad + + + + Timestamp (milliseconds) + Global X speed + Global Y speed + Global Z speed + + + + Message sent to the MAV to set a new position as reference for the controller + System ID + Component ID + ID of waypoint, 0 for plain position + x position + y position + z position + yaw orientation in radians, 0 = NORTH + + + + Message sent to the MAV to set a new offset from the currently controlled position + System ID + Component ID + x position offset + y position offset + z position offset + yaw orientation offset in radians, 0 = NORTH + + + + + ID of waypoint, 0 for plain position + x position + y position + z position + yaw orientation in radians, 0 = NORTH + + + + ID + x position + y position + z position + roll orientation + pitch orientation + yaw orientation + + + + ADC1 (J405 ADC3, LPC2148 AD0.6) + ADC2 (J405 ADC5, LPC2148 AD0.2) + ADC3 (J405 ADC6, LPC2148 AD0.1) + ADC4 (J405 ADC7, LPC2148 AD1.3) + Battery voltage + Temperature (degrees celcius) + Barometric pressure (hecto Pascal) + + + + Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + Number of I2C errors since startup + Number of I2C errors since startup + Number of I2C errors since startup + Number of I2C errors since startup + Number of I2C errors since startup + + + + Watchdog ID + Number of processes + + + + Watchdog ID + Process ID + Process name + Process arguments + Timeout (seconds) + + + + Watchdog ID + Process ID + Is running / finished / suspended / crashed + Is muted + PID + Number of crashes + + + + Target system ID + Watchdog ID + Process ID + Command ID + + + + 0: Pattern, 1: Letter + Confidence of detection + Pattern file name + Accepted as true detection, 0 no, 1 yes + + + + Notifies the operator about a point of interest (POI). This can be anything detected by the + system. This generic message is intented to help interfacing to generic visualizations and to display + the POI on a map. + + 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug + 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta + 0: global, 1:local + 0: no timeout, >1: timeout in seconds + X Position + Y Position + Z Position + POI name + + + + Notifies the operator about the connection of two point of interests (POI). This can be anything detected by the + system. This generic message is intented to help interfacing to generic visualizations and to display + the POI on a map. + + 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug + 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta + 0: global, 1:local + 0: no timeout, >1: timeout in seconds + X1 Position + Y1 Position + Z1 Position + X2 Position + Y2 Position + Z2 Position + POI connection name + + + + type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) + total data size in bytes (set on ACK only) + number of packets beeing sent (set on ACK only) + payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) + JPEG quality out of [1,100] + + + + sequence number (starting with 0 on every transmission) + image data bytes + + + + x position in m + y position in m + z position in m + Orientation assignment 0: false, 1:true + Size in pixels + Orientation + Descriptor + Harris operator response at this location + - - - Content Types for data transmission handshake - - - - - - - - - - - - The system to be controlled - roll - pitch - yaw - thrust - roll control enabled auto:0, manual:1 - pitch auto:0, manual:1 - yaw auto:0, manual:1 - thrust auto:0, manual:1 - - - - Camera id - Camera mode: 0 = auto, 1 = manual - Trigger pin, 0-3 for PtGrey FireFly - Shutter interval, in microseconds - Exposure time, in microseconds - Camera gain - - - - Timestamp - IMU seq - Roll angle in rad - Pitch angle in rad - Yaw angle in rad - Local frame Z coordinate (height over ground) - GPS X coordinate - GPS Y coordinate - Global frame altitude - Ground truth X - Ground truth Y - Ground truth Z - - - - 0 to disable, 1 to enable - - - - Camera id - Camera # (starts with 0) - Timestamp - Until which timestamp this buffer will stay valid - The image sequence number - Position of the image in the buffer, starts with 0 - Image width - Image height - Image depth - Image channels - Shared memory area key - Exposure time, in microseconds - Camera gain - Roll angle in rad - Pitch angle in rad - Yaw angle in rad - Local frame Z coordinate (height over ground) - GPS X coordinate - GPS Y coordinate - Global frame altitude - Ground truth X - Ground truth Y - Ground truth Z - - - - Timestamp (milliseconds) - Global X position - Global Y position - Global Z position - Roll angle in rad - Pitch angle in rad - Yaw angle in rad - - - - Timestamp (milliseconds) - Global X position - Global Y position - Global Z position - Roll angle in rad - Pitch angle in rad - Yaw angle in rad - - - - Timestamp (milliseconds) - Global X speed - Global Y speed - Global Z speed - - - - Message sent to the MAV to set a new position as reference for the controller - System ID - Component ID - ID of waypoint, 0 for plain position - x position - y position - z position - yaw orientation in radians, 0 = NORTH - - - - Message sent to the MAV to set a new offset from the currently controlled position - System ID - Component ID - x position offset - y position offset - z position offset - yaw orientation offset in radians, 0 = NORTH - - - - - ID of waypoint, 0 for plain position - x position - y position - z position - yaw orientation in radians, 0 = NORTH - - - - ID - x position - y position - z position - roll orientation - pitch orientation - yaw orientation - - - - ADC1 (J405 ADC3, LPC2148 AD0.6) - ADC2 (J405 ADC5, LPC2148 AD0.2) - ADC3 (J405 ADC6, LPC2148 AD0.1) - ADC4 (J405 ADC7, LPC2148 AD1.3) - Battery voltage - Temperature (degrees celcius) - Barometric pressure (hecto Pascal) - - - - Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - Number of I2C errors since startup -Number of I2C errors since startup - Number of I2C errors since startup -Number of I2C errors since startup - Number of I2C errors since startup - - - - Watchdog ID - Number of processes - - - - Watchdog ID - Process ID - Process name - Process arguments - Timeout (seconds) - - - - Watchdog ID - Process ID - Is running / finished / suspended / crashed - Is muted - PID - Number of crashes - - - - Target system ID - Watchdog ID - Process ID - Command ID - - - - 0: Pattern, 1: Letter - Confidence of detection - Pattern file name - Accepted as true detection, 0 no, 1 yes - - - - Notifies the operator about a point of interest (POI). This can be anything detected by the - system. This generic message is intented to help interfacing to generic visualizations and to display - the POI on a map. - 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - 0: global, 1:local - 0: no timeout, >1: timeout in seconds - X Position - Y Position - Z Position - POI name - - - - Notifies the operator about the connection of two point of interests (POI). This can be anything detected by the - system. This generic message is intented to help interfacing to generic visualizations and to display - the POI on a map. - 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - 0: global, 1:local - 0: no timeout, >1: timeout in seconds - X1 Position - Y1 Position - Z1 Position - X2 Position - Y2 Position - Z2 Position - POI connection name - - - - type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) - total data size in bytes (set on ACK only) - number of packets beeing sent (set on ACK only) - payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) - JPEG quality out of [1,100] - - - - sequence number (starting with 0 on every transmission) - image data bytes - - - - x position in m - y position in m - z position in m - Orientation assignment 0: false, 1:true - Size in pixels - Orientation - Descriptor - Harris operator response at this location - - - + + Visual odometry estimate describing relative pose between two frames + Time at which frame 1 was captured (in microseconds since unix epoch) + Time at which frame 2 was captured (in microseconds since unix epoch) + Relative X position + Relative Y position + Relative Z position + Relative roll angle in rad + Relative pitch angle in rad + Relative yaw angle in rad + + + diff --git a/thirdParty/mavlink/message_definitions/slugs.xml b/thirdParty/mavlink/message_definitions/slugs.xml index 6de0697fef69a78cafeb856f9b4569bb4ae14925..f8644c5c4be7e497178d892e9c15c25ba3c5131c 100644 --- a/thirdParty/mavlink/message_definitions/slugs.xml +++ b/thirdParty/mavlink/message_definitions/slugs.xml @@ -1,8 +1,8 @@ -common.xml - - - - - - - Sensor and DSC control loads. - Sensor DSC Load - Control DSC Load - Battery Voltage in millivolts - - - - Air data for altitude and airspeed computation. - Dynamic pressure (Pa) - Static pressure (Pa) - Board temperature - - - - Accelerometer and gyro biases. - Accelerometer X bias (m/s) - Accelerometer Y bias (m/s) - Accelerometer Z bias (m/s) - Gyro X bias (rad/s) - Gyro Y bias (rad/s) - Gyro Z bias (rad/s) - - - - Configurable diagnostic messages. - Diagnostic float 1 - Diagnostic float 2 - Diagnostic float 3 - Diagnostic short 1 - Diagnostic short 2 - Diagnostic short 3 - - - - Data used in the navigation algorithm. - Measured Airspeed prior to the Nav Filter - Commanded Roll - Commanded Pitch - Commanded Turn rate - Y component of the body acceleration - Total Distance to Run on this leg of Navigation - Remaining distance to Run on this leg of Navigation - Origin WP - Destination WP - - - - Configurable data log probes to be used inside Simulink - Log value 1 - Log value 2 - Log value 3 - Log value 4 - Log value 5 - Log value 6 - - - - Pilot console PWM messges. - Year reported by Gps - Month reported by Gps - Day reported by Gps - Hour reported by Gps - Min reported by Gps - Sec reported by Gps - Visible sattelites reported by Gps - - - Mid Level commands sent from the GS to the autopilot. These are only sent when being opperated in mid-level commands mode from the ground; for periodic report of these commands generated from the autopilot see message XXXX. - The system setting the commands - Commanded Airspeed - Log value 2 - Log value 3 - - - -This message configures the Selective Passthrough mode. it allows to select which control surfaces the Pilot can control from his console. It is implemented as a bitfield as follows: - Position Bit Code - ================================= - 15-8 Reserved - 7 dt_pass 128 - 6 dla_pass 64 - 5 dra_pass 32 - 4 dr_pass 16 - 3 dle_pass 8 - 2 dre_pass 4 - 1 dlf_pass 2 - 0 drf_pass 1 - Where Bit 15 is the MSb. 0 = AP has control of the surface; 1 = Pilot Console has control of the surface. - The system setting the commands - Bitfield containing the PT configuration - - - - - Action messages focused on the SLUGS AP. - The system reporting the action - Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names - Value associated with the action - - - - \ No newline at end of file + + + + Sensor and DSC control loads. + Sensor DSC Load + Control DSC Load + Battery Voltage in millivolts + + + + Air data for altitude and airspeed computation. + Dynamic pressure (Pa) + Static pressure (Pa) + Board temperature + + + + Accelerometer and gyro biases. + Accelerometer X bias (m/s) + Accelerometer Y bias (m/s) + Accelerometer Z bias (m/s) + Gyro X bias (rad/s) + Gyro Y bias (rad/s) + Gyro Z bias (rad/s) + + + + Configurable diagnostic messages. + Diagnostic float 1 + Diagnostic float 2 + Diagnostic float 3 + Diagnostic short 1 + Diagnostic short 2 + Diagnostic short 3 + + + + Data used in the navigation algorithm. + Measured Airspeed prior to the Nav Filter + Commanded Roll + Commanded Pitch + Commanded Turn rate + Y component of the body acceleration + Total Distance to Run on this leg of Navigation + Remaining distance to Run on this leg of Navigation + Origin WP + Destination WP + + + + Configurable data log probes to be used inside Simulink + Log value 1 + Log value 2 + Log value 3 + Log value 4 + Log value 5 + Log value 6 + + + + Pilot console PWM messges. + Year reported by Gps + Month reported by Gps + Day reported by Gps + Hour reported by Gps + Min reported by Gps + Sec reported by Gps + Visible sattelites reported by Gps + + + + Mid Level commands sent from the GS to the autopilot. These are only sent when being opperated in mid-level commands mode from the ground; for periodic report of these commands generated from the autopilot see message XXXX. + The system setting the commands + Commanded Airspeed + Log value 2 + Log value 3 + + + + + This message configures the Selective Passthrough mode. it allows to select which control surfaces the Pilot can control from his console. It is implemented as a bitfield as follows: + Position Bit Code + ================================= + 15-8 Reserved + 7 dt_pass 128 + 6 dla_pass 64 + 5 dra_pass 32 + 4 dr_pass 16 + 3 dle_pass 8 + 2 dre_pass 4 + 1 dlf_pass 2 + 0 drf_pass 1 + Where Bit 15 is the MSb. 0 = AP has control of the surface; 1 = Pilot Console has control of the surface. + The system setting the commands + Bitfield containing the PT configuration + + + + + + Action messages focused on the SLUGS AP. + The system reporting the action + Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names + Value associated with the action + + + + diff --git a/thirdParty/mavlink/message_definitions/ualberta.xml b/thirdParty/mavlink/message_definitions/ualberta.xml index eaa9d99844835df716665ef893d0bbcac5d47445..5e53e141e9f018e943abfd25bc1f58c0a0091574 100644 --- a/thirdParty/mavlink/message_definitions/ualberta.xml +++ b/thirdParty/mavlink/message_definitions/ualberta.xml @@ -1,54 +1,54 @@ - common.xml - - - Available autopilot modes for ualberta uav - Raw input pulse widts sent to output - Inputs are normalized using calibration, the converted back to raw pulse widths for output - dfsdfs - dfsfds - dfsdfsdfs - - - Navigation filter mode - - AHRS mode - INS/GPS initialization mode - INS/GPS mode - - - Mode currently commanded by pilot - sdf - dfs - Rotomotion mode - - - - - Accelerometer and Gyro biases from the navigation filter - Timestamp (microseconds) - b_f[0] - b_f[1] - b_f[2] - b_f[0] - b_f[1] - b_f[2] - - - Complete set of calibration parameters for the radio - Aileron setpoints: left, center, right - Elevator setpoints: nose down, center, nose up - Rudder setpoints: nose left, center, nose right - Tail gyro mode/gain setpoints: heading hold, rate mode - Pitch curve setpoints (every 25%) - Throttle curve setpoints (every 25%) - - - System status specific to ualberta uav - System mode, see UALBERTA_AUTOPILOT_MODE ENUM - Navigation mode, see UALBERTA_NAV_MODE ENUM - Pilot mode, see UALBERTA_PILOT_MODE - - + common.xml + + + Available autopilot modes for ualberta uav + Raw input pulse widts sent to output + Inputs are normalized using calibration, the converted back to raw pulse widths for output + dfsdfs + dfsfds + dfsdfsdfs + + + Navigation filter mode + + AHRS mode + INS/GPS initialization mode + INS/GPS mode + + + Mode currently commanded by pilot + sdf + dfs + Rotomotion mode + + + + + Accelerometer and Gyro biases from the navigation filter + Timestamp (microseconds) + b_f[0] + b_f[1] + b_f[2] + b_f[0] + b_f[1] + b_f[2] + + + Complete set of calibration parameters for the radio + Aileron setpoints: left, center, right + Elevator setpoints: nose down, center, nose up + Rudder setpoints: nose left, center, nose right + Tail gyro mode/gain setpoints: heading hold, rate mode + Pitch curve setpoints (every 25%) + Throttle curve setpoints (every 25%) + + + System status specific to ualberta uav + System mode, see UALBERTA_AUTOPILOT_MODE ENUM + Navigation mode, see UALBERTA_NAV_MODE ENUM + Pilot mode, see UALBERTA_PILOT_MODE + + diff --git a/thirdParty/mavlink/missionlib/testing/.gitignore b/thirdParty/mavlink/missionlib/testing/.gitignore new file mode 100644 index 0000000000000000000000000000000000000000..0776965a63b239ff52dbd7c2afe5aaa7db6c2262 --- /dev/null +++ b/thirdParty/mavlink/missionlib/testing/.gitignore @@ -0,0 +1,3 @@ +*.o +udptest +build diff --git a/thirdParty/mavlink/missionlib/testing/main.c b/thirdParty/mavlink/missionlib/testing/main.c new file mode 100644 index 0000000000000000000000000000000000000000..0bec1155ad5e73a5bdd765f71399e88bc0c59754 --- /dev/null +++ b/thirdParty/mavlink/missionlib/testing/main.c @@ -0,0 +1,276 @@ +/******************************************************************************* + + Copyright (C) 2011 Lorenz Meier lm ( a t ) inf.ethz.ch + and Bryan Godbolt godbolt ( a t ) ualberta.ca + + adapted from example written by Bryan Godbolt godbolt ( a t ) ualberta.ca + + This program 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. + + This program 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 this program. If not, see . + + ****************************************************************************/ +/* + This program sends some data to qgroundcontrol using the mavlink protocol. The sent packets + cause qgroundcontrol to respond with heartbeats. Any settings or custom commands sent from + qgroundcontrol are printed by this program along with the heartbeats. + + + I compiled this program sucessfully on Ubuntu 10.04 with the following command + + gcc -I ../../pixhawk/mavlink/include -o udp-server udp.c + + the rt library is needed for the clock_gettime on linux + */ +/* These headers are for QNX, but should all be standard on unix/linux */ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#if (defined __QNX__) | (defined __QNXNTO__) +/* QNX specific headers */ +#include +#else +/* Linux / MacOS POSIX timer headers */ +#include +#include +#include +#endif + +/* 0: Include MAVLink types */ +#include <../mavlink_types.h> + +/* 1: Define mavlink system storage */ +mavlink_system_t mavlink_system; + +/* 2: Include actual protocol, REQUIRES mavlink_system */ +#include + +/* 3: Define waypoint helper functions */ +void mavlink_wpm_send_message(mavlink_message_t* msg); +void mavlink_wpm_send_gcs_string(const char* string); +uint64_t mavlink_wpm_get_system_timestamp(); + +/* 4: Include waypoint protocol */ +#include "waypoints.h" +mavlink_wpm_storage wpm; + + +#define BUFFER_LENGTH 2041 // minimum buffer size that can be used with qnx (I don't know why) + +char help[] = "--help"; + + +char target_ip[100]; + +float position[6] = {}; +int sock; +struct sockaddr_in gcAddr; +struct sockaddr_in locAddr; +uint8_t buf[BUFFER_LENGTH]; +ssize_t recsize; +socklen_t fromlen; +int bytes_sent; +mavlink_message_t msg; +uint16_t len; +int i = 0; +unsigned int temp = 0; + +uint64_t microsSinceEpoch(); + + + + +/* Provide the interface functions for the waypoint manager */ + +/* + * @brief Sends a MAVLink message over UDP + */ +void mavlink_wpm_send_message(mavlink_message_t* msg) +{ + uint8_t buf[MAVLINK_MAX_PACKET_LEN]; + uint16_t len = mavlink_msg_to_send_buffer(buf, msg); + uint16_t bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof (struct sockaddr_in)); + + printf("SENT %d bytes", bytes_sent); +} + +void mavlink_wpm_send_gcs_string(const char* string) +{ + printf("%s",string); +} + +uint64_t mavlink_wpm_get_system_timestamp() +{ + struct timeval tv; + gettimeofday(&tv, NULL); + return ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec; +} + + + +int main(int argc, char* argv[]) +{ + // Initialize MAVLink + mavlink_wpm_init(&wpm); + mavlink_system.sysid = 1; + mavlink_system.compid = MAV_COMP_ID_WAYPOINTPLANNER; + + + + // Create socket + sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP); + + // Check if --help flag was used + if ((argc == 2) && (strcmp(argv[1], help) == 0)) + { + printf("\n"); + printf("\tUsage:\n\n"); + printf("\t"); + printf("%s", argv[0]); + printf(" \n"); + printf("\tDefault for localhost: udp-server 127.0.0.1\n\n"); + exit(EXIT_FAILURE); + } + + + // Change the target ip if parameter was given + strcpy(target_ip, "127.0.0.1"); + if (argc == 2) + { + strcpy(target_ip, argv[1]); + } + + + memset(&locAddr, 0, sizeof(locAddr)); + locAddr.sin_family = AF_INET; + locAddr.sin_addr.s_addr = INADDR_ANY; + locAddr.sin_port = htons(14551); + + /* Bind the socket to port 14551 - necessary to receive packets from qgroundcontrol */ + if (-1 == bind(sock,(struct sockaddr *)&locAddr, sizeof(struct sockaddr))) + { + perror("error bind failed"); + close(sock); + exit(EXIT_FAILURE); + } + + /* Attempt to make it non blocking */ + if (fcntl(sock, F_SETFL, O_NONBLOCK | FASYNC) < 0) + { + fprintf(stderr, "error setting nonblocking: %s\n", strerror(errno)); + close(sock); + exit(EXIT_FAILURE); + } + + + memset(&gcAddr, 0, sizeof(gcAddr)); + gcAddr.sin_family = AF_INET; + gcAddr.sin_addr.s_addr = inet_addr(target_ip); + gcAddr.sin_port = htons(14550); + + + printf("MAVLINK MISSION LIBRARY EXAMPLE PROCESS INITIALIZATION DONE, RUNNING..\n"); + + + for (;;) + { + + /*Send Heartbeat */ + mavlink_msg_heartbeat_pack(mavlink_system.sysid, 200, &msg, MAV_HELICOPTER, MAV_CLASS_GENERIC); + len = mavlink_msg_to_send_buffer(buf, &msg); + bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); + + /* Send Status */ + mavlink_msg_sys_status_pack(1, 200, &msg, MAV_MODE_GUIDED, MAV_NAV_HOLD, MAV_STATE_ACTIVE, 500, 7500, 0, 0); + len = mavlink_msg_to_send_buffer(buf, &msg); + bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof (struct sockaddr_in)); + + /* Send Local Position */ + mavlink_msg_local_position_pack(mavlink_system.sysid, 200, &msg, microsSinceEpoch(), + position[0], position[1], position[2], + position[3], position[4], position[5]); + len = mavlink_msg_to_send_buffer(buf, &msg); + bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); + + /* Send attitude */ + mavlink_msg_attitude_pack(mavlink_system.sysid, 200, &msg, microsSinceEpoch(), 1.2, 1.7, 3.14, 0.01, 0.02, 0.03); + len = mavlink_msg_to_send_buffer(buf, &msg); + bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); + + + memset(buf, 0, BUFFER_LENGTH); + recsize = recvfrom(sock, (void *)buf, BUFFER_LENGTH, 0, (struct sockaddr *)&gcAddr, &fromlen); + if (recsize > 0) + { + // Something received - print out all bytes and parse packet + mavlink_message_t msg; + mavlink_status_t status; + + printf("Bytes Received: %d\nDatagram: ", (int)recsize); + for (i = 0; i < recsize; ++i) + { + temp = buf[i]; + printf("%02x ", (unsigned char)temp); + if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &msg, &status)) + { + // Packet received + printf("\nReceived packet: SYS: %d, COMP: %d, LEN: %d, MSG ID: %d\n", msg.sysid, msg.compid, msg.len, msg.msgid); + + // Handle packet with waypoint component + mavlink_wpm_message_handler(&msg); + + // Handle packet with parameter component + } + } + printf("\n"); + } + memset(buf, 0, BUFFER_LENGTH); + usleep(50000); // Sleep one second + } +} + + +/* QNX timer version */ +#if (defined __QNX__) | (defined __QNXNTO__) +uint64_t microsSinceEpoch() +{ + + struct timespec time; + + uint64_t micros = 0; + + clock_gettime(CLOCK_REALTIME, &time); + micros = (uint64_t)time.tv_sec * 100000 + time.tv_nsec/1000; + + return micros; +} +#else +uint64_t microsSinceEpoch() +{ + + struct timeval tv; + + uint64_t micros = 0; + + gettimeofday(&tv, NULL); + micros = ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec; + + return micros; +} +#endif \ No newline at end of file diff --git a/thirdParty/mavlink/missionlib/testing/udptest.xcodeproj/.gitignore b/thirdParty/mavlink/missionlib/testing/udptest.xcodeproj/.gitignore new file mode 100644 index 0000000000000000000000000000000000000000..d82a5e47def52cce2467649612ca36ad79f40a63 --- /dev/null +++ b/thirdParty/mavlink/missionlib/testing/udptest.xcodeproj/.gitignore @@ -0,0 +1,2 @@ +*.mode1v3 +*.pbxuser diff --git a/thirdParty/mavlink/missionlib/testing/udptest.xcodeproj/project.pbxproj b/thirdParty/mavlink/missionlib/testing/udptest.xcodeproj/project.pbxproj new file mode 100644 index 0000000000000000000000000000000000000000..ba27d2f32eed92413f59df2f43dc17a6830706f7 --- /dev/null +++ b/thirdParty/mavlink/missionlib/testing/udptest.xcodeproj/project.pbxproj @@ -0,0 +1,217 @@ +// !$*UTF8*$! +{ + archiveVersion = 1; + classes = { + }; + objectVersion = 45; + objects = { + +/* Begin PBXBuildFile section */ + 34E8AFDB13F5064C001100AA /* waypoints.c in Sources */ = {isa = PBXBuildFile; fileRef = 34E8AFDA13F5064C001100AA /* waypoints.c */; }; + 8DD76FAC0486AB0100D96B5E /* main.c in Sources */ = {isa = PBXBuildFile; fileRef = 08FB7796FE84155DC02AAC07 /* main.c */; settings = {ATTRIBUTES = (); }; }; + 8DD76FB00486AB0100D96B5E /* udptest.1 in CopyFiles */ = {isa = PBXBuildFile; fileRef = C6A0FF2C0290799A04C91782 /* udptest.1 */; }; +/* End PBXBuildFile section */ + +/* Begin PBXCopyFilesBuildPhase section */ + 8DD76FAF0486AB0100D96B5E /* CopyFiles */ = { + isa = PBXCopyFilesBuildPhase; + buildActionMask = 8; + dstPath = /usr/share/man/man1/; + dstSubfolderSpec = 0; + files = ( + 8DD76FB00486AB0100D96B5E /* udptest.1 in CopyFiles */, + ); + runOnlyForDeploymentPostprocessing = 1; + }; +/* End PBXCopyFilesBuildPhase section */ + +/* Begin PBXFileReference section */ + 08FB7796FE84155DC02AAC07 /* main.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = main.c; sourceTree = ""; }; + 34E8AFDA13F5064C001100AA /* waypoints.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = waypoints.c; path = ../waypoints.c; sourceTree = SOURCE_ROOT; }; + 34E8AFDC13F50659001100AA /* waypoints.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; name = waypoints.h; path = ../waypoints.h; sourceTree = SOURCE_ROOT; }; + 8DD76FB20486AB0100D96B5E /* udptest */ = {isa = PBXFileReference; explicitFileType = "compiled.mach-o.executable"; includeInIndex = 0; path = udptest; sourceTree = BUILT_PRODUCTS_DIR; }; + C6A0FF2C0290799A04C91782 /* udptest.1 */ = {isa = PBXFileReference; lastKnownFileType = text.man; path = udptest.1; sourceTree = ""; }; +/* End PBXFileReference section */ + +/* Begin PBXFrameworksBuildPhase section */ + 8DD76FAD0486AB0100D96B5E /* Frameworks */ = { + isa = PBXFrameworksBuildPhase; + buildActionMask = 2147483647; + files = ( + ); + runOnlyForDeploymentPostprocessing = 0; + }; +/* End PBXFrameworksBuildPhase section */ + +/* Begin PBXGroup section */ + 08FB7794FE84155DC02AAC07 /* udptest */ = { + isa = PBXGroup; + children = ( + 34E8AFDC13F50659001100AA /* waypoints.h */, + 08FB7795FE84155DC02AAC07 /* Source */, + C6A0FF2B0290797F04C91782 /* Documentation */, + 1AB674ADFE9D54B511CA2CBB /* Products */, + ); + name = udptest; + sourceTree = ""; + }; + 08FB7795FE84155DC02AAC07 /* Source */ = { + isa = PBXGroup; + children = ( + 34E8AFDA13F5064C001100AA /* waypoints.c */, + 08FB7796FE84155DC02AAC07 /* main.c */, + ); + name = Source; + sourceTree = ""; + }; + 1AB674ADFE9D54B511CA2CBB /* Products */ = { + isa = PBXGroup; + children = ( + 8DD76FB20486AB0100D96B5E /* udptest */, + ); + name = Products; + sourceTree = ""; + }; + C6A0FF2B0290797F04C91782 /* Documentation */ = { + isa = PBXGroup; + children = ( + C6A0FF2C0290799A04C91782 /* udptest.1 */, + ); + name = Documentation; + sourceTree = ""; + }; +/* End PBXGroup section */ + +/* Begin PBXNativeTarget section */ + 8DD76FA90486AB0100D96B5E /* udptest */ = { + isa = PBXNativeTarget; + buildConfigurationList = 1DEB928508733DD80010E9CD /* Build configuration list for PBXNativeTarget "udptest" */; + buildPhases = ( + 8DD76FAB0486AB0100D96B5E /* Sources */, + 8DD76FAD0486AB0100D96B5E /* Frameworks */, + 8DD76FAF0486AB0100D96B5E /* CopyFiles */, + ); + buildRules = ( + ); + dependencies = ( + ); + name = udptest; + productInstallPath = "$(HOME)/bin"; + productName = udptest; + productReference = 8DD76FB20486AB0100D96B5E /* udptest */; + productType = "com.apple.product-type.tool"; + }; +/* End PBXNativeTarget section */ + +/* Begin PBXProject section */ + 08FB7793FE84155DC02AAC07 /* Project object */ = { + isa = PBXProject; + buildConfigurationList = 1DEB928908733DD80010E9CD /* Build configuration list for PBXProject "udptest" */; + compatibilityVersion = "Xcode 3.1"; + developmentRegion = English; + hasScannedForEncodings = 1; + knownRegions = ( + English, + Japanese, + French, + German, + ); + mainGroup = 08FB7794FE84155DC02AAC07 /* udptest */; + projectDirPath = ""; + projectRoot = ""; + targets = ( + 8DD76FA90486AB0100D96B5E /* udptest */, + ); + }; +/* End PBXProject section */ + +/* Begin PBXSourcesBuildPhase section */ + 8DD76FAB0486AB0100D96B5E /* Sources */ = { + isa = PBXSourcesBuildPhase; + buildActionMask = 2147483647; + files = ( + 8DD76FAC0486AB0100D96B5E /* main.c in Sources */, + 34E8AFDB13F5064C001100AA /* waypoints.c in Sources */, + ); + runOnlyForDeploymentPostprocessing = 0; + }; +/* End PBXSourcesBuildPhase section */ + +/* Begin XCBuildConfiguration section */ + 1DEB928608733DD80010E9CD /* Debug */ = { + isa = XCBuildConfiguration; + buildSettings = { + ALWAYS_SEARCH_USER_PATHS = NO; + COPY_PHASE_STRIP = NO; + GCC_DYNAMIC_NO_PIC = NO; + GCC_ENABLE_FIX_AND_CONTINUE = YES; + GCC_MODEL_TUNING = G5; + GCC_OPTIMIZATION_LEVEL = 0; + INSTALL_PATH = /usr/local/bin; + PRODUCT_NAME = udptest; + }; + name = Debug; + }; + 1DEB928708733DD80010E9CD /* Release */ = { + isa = XCBuildConfiguration; + buildSettings = { + ALWAYS_SEARCH_USER_PATHS = NO; + DEBUG_INFORMATION_FORMAT = "dwarf-with-dsym"; + GCC_MODEL_TUNING = G5; + INSTALL_PATH = /usr/local/bin; + PRODUCT_NAME = udptest; + }; + name = Release; + }; + 1DEB928A08733DD80010E9CD /* Debug */ = { + isa = XCBuildConfiguration; + buildSettings = { + ARCHS = "$(ARCHS_STANDARD_32_64_BIT)"; + GCC_C_LANGUAGE_STANDARD = gnu99; + GCC_OPTIMIZATION_LEVEL = 0; + GCC_WARN_ABOUT_RETURN_TYPE = YES; + GCC_WARN_UNUSED_VARIABLE = YES; + HEADER_SEARCH_PATHS = ../../include/common/; + ONLY_ACTIVE_ARCH = YES; + PREBINDING = NO; + SDKROOT = macosx10.6; + }; + name = Debug; + }; + 1DEB928B08733DD80010E9CD /* Release */ = { + isa = XCBuildConfiguration; + buildSettings = { + ARCHS = "$(ARCHS_STANDARD_32_64_BIT)"; + GCC_C_LANGUAGE_STANDARD = gnu99; + GCC_WARN_ABOUT_RETURN_TYPE = YES; + GCC_WARN_UNUSED_VARIABLE = YES; + PREBINDING = NO; + SDKROOT = macosx10.6; + }; + name = Release; + }; +/* End XCBuildConfiguration section */ + +/* Begin XCConfigurationList section */ + 1DEB928508733DD80010E9CD /* Build configuration list for PBXNativeTarget "udptest" */ = { + isa = XCConfigurationList; + buildConfigurations = ( + 1DEB928608733DD80010E9CD /* Debug */, + 1DEB928708733DD80010E9CD /* Release */, + ); + defaultConfigurationIsVisible = 0; + defaultConfigurationName = Release; + }; + 1DEB928908733DD80010E9CD /* Build configuration list for PBXProject "udptest" */ = { + isa = XCConfigurationList; + buildConfigurations = ( + 1DEB928A08733DD80010E9CD /* Debug */, + 1DEB928B08733DD80010E9CD /* Release */, + ); + defaultConfigurationIsVisible = 0; + defaultConfigurationName = Release; + }; +/* End XCConfigurationList section */ + }; + rootObject = 08FB7793FE84155DC02AAC07 /* Project object */; +} diff --git a/thirdParty/mavlink/missionlib/waypoints.c b/thirdParty/mavlink/missionlib/waypoints.c new file mode 100644 index 0000000000000000000000000000000000000000..a7b8ee5f5ca0aa43be983ab1f1c1eaadaa616ee4 --- /dev/null +++ b/thirdParty/mavlink/missionlib/waypoints.c @@ -0,0 +1,1044 @@ +/******************************************************************************* + + Copyright (C) 2011 Lorenz Meier lm ( a t ) inf.ethz.ch + + This program 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. + + This program 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 this program. If not, see . + + ****************************************************************************/ + +#include "waypoints.h" +#include + +bool debug = true; +bool verbose = true; + +extern mavlink_system_t mavlink_system; +extern mavlink_wpm_storage wpm; + +extern void mavlink_wpm_send_message(mavlink_message_t* msg); +extern void mavlink_wpm_send_gcs_string(const char* string); +extern uint64_t mavlink_wpm_get_system_timestamp(); + + +#define MAVLINK_WPM_NO_PRINTF + +void mavlink_wpm_init(mavlink_wpm_storage* state) +{ + // Set all waypoints to zero + + // Set count to zero + state->size = 0; + state->max_size = MAVLINK_WPM_MAX_WP_COUNT; + state->current_state = MAVLINK_WPM_STATE_IDLE; + state->current_partner_sysid = 0; + state->current_partner_compid = 0; + state->timestamp_lastaction = 0; + state->timestamp_last_send_setpoint = 0; + state->timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT; + state->delay_setpoint = MAVLINK_WPM_SETPOINT_DELAY_DEFAULT; + state->idle = false; ///< indicates if the system is following the waypoints or is waiting + state->current_active_wp_id = -1; ///< id of current waypoint + state->yaw_reached = false; ///< boolean for yaw attitude reached + state->pos_reached = false; ///< boolean for position reached + state->timestamp_lastoutside_orbit = 0;///< timestamp when the MAV was last outside the orbit or had the wrong yaw value + state->timestamp_firstinside_orbit = 0;///< timestamp when the MAV was the first time after a waypoint change inside the orbit and had the correct yaw value + +} + +/* + * @brief Sends an waypoint ack message + */ +void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type) +{ + mavlink_message_t msg; + mavlink_waypoint_ack_t wpa; + + wpa.target_system = wpm.current_partner_sysid; + wpa.target_component = wpm.current_partner_compid; + wpa.type = type; + + mavlink_msg_waypoint_ack_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wpa); + mavlink_wpm_send_message(&msg); + + // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); + + if (MAVLINK_WPM_TEXT_FEEDBACK) + { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("Sent waypoint ACK"); +#else + if (MAVLINK_WPM_VERBOSE) printf("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system); +#endif + mavlink_wpm_send_gcs_string("Sent waypoint ACK"); + } +} + +/* + * @brief Broadcasts the new target waypoint and directs the MAV to fly there + * + * This function broadcasts its new active waypoint sequence number and + * sends a message to the controller, advising it to fly to the coordinates + * of the waypoint with a given orientation + * + * @param seq The waypoint sequence number the MAV should fly to. + */ +void mavlink_wpm_send_waypoint_current(uint16_t seq) +{ + if(seq < wpm.size) + { + mavlink_waypoint_t *cur = &(wpm.waypoints[seq]); + + mavlink_message_t msg; + mavlink_waypoint_current_t wpc; + + wpc.seq = cur->seq; + + mavlink_msg_waypoint_current_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wpc); + mavlink_wpm_send_message(&msg); + + // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); + + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Broadcasted new current waypoint\n"); //// printf("Broadcasted new current waypoint %u\n", wpc.seq); + } + else + { + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("ERROR: index out of bounds\n"); + } +} + +/* + * @brief Directs the MAV to fly to a position + * + * Sends a message to the controller, advising it to fly to the coordinates + * of the waypoint with a given orientation + * + * @param seq The waypoint sequence number the MAV should fly to. + */ +void mavlink_wpm_send_setpoint(uint16_t seq) +{ + if(seq < wpm.size) + { + mavlink_waypoint_t *cur = &(wpm.waypoints[seq]); + + mavlink_message_t msg; + mavlink_local_position_setpoint_set_t position_control_set_point; + + // Send new NED or ENU setpoint to onbaord autopilot + if (cur->frame == MAV_FRAME_LOCAL_NED || cur->frame == MAV_FRAME_LOCAL_ENU) + { + position_control_set_point.target_system = mavlink_system.sysid; + position_control_set_point.target_component = MAV_COMP_ID_IMU; + position_control_set_point.x = cur->x; + position_control_set_point.y = cur->y; + position_control_set_point.z = cur->z; + position_control_set_point.yaw = cur->param4; + + mavlink_msg_local_position_setpoint_set_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &position_control_set_point); + mavlink_wpm_send_message(&msg); + + // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); + } + else + { + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("No new setpoint set because of invalid coordinate frame of waypoint");//// if (verbose) // printf("No new set point sent to IMU because the new waypoint %u had no local coordinates\n", cur->seq); + } + + wpm.timestamp_last_send_setpoint = mavlink_wpm_get_system_timestamp(); + } + else + { + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("ERROR: Waypoint index out of bounds\n"); //// if (verbose) // printf("ERROR: index out of bounds\n"); + } +} + +void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count) +{ + mavlink_message_t msg; + mavlink_waypoint_count_t wpc; + + wpc.target_system = wpm.current_partner_sysid; + wpc.target_component = wpm.current_partner_compid; + wpc.count = count; + + mavlink_msg_waypoint_count_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wpc); + mavlink_wpm_send_message(&msg); + + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint count"); //// if (verbose) // printf("Sent waypoint count (%u) to ID %u\n", wpc.count, wpc.target_system); + + // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); +} + +void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq) +{ + if (seq < wpm.size) + { + mavlink_message_t msg; + mavlink_waypoint_t *wp = &(wpm.waypoints[seq]); + wp->target_system = wpm.current_partner_sysid; + wp->target_component = wpm.current_partner_compid; + mavlink_msg_waypoint_encode(mavlink_system.sysid, mavlink_system.compid, &msg, wp); + mavlink_wpm_send_message(&msg); + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint"); //// if (verbose) // printf("Sent waypoint %u to ID %u\n", wp->seq, wp->target_system); + + // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); + } + else + { + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("ERROR: Waypoint index out of bounds\n"); + } +} + +void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq) +{ + if (seq < wpm.max_size) + { + mavlink_message_t msg; + mavlink_waypoint_request_t wpr; + wpr.target_system = wpm.current_partner_sysid; + wpr.target_component = wpm.current_partner_compid; + wpr.seq = seq; + mavlink_msg_waypoint_request_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wpr); + mavlink_wpm_send_message(&msg); + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint request"); //// if (verbose) // printf("Sent waypoint request %u to ID %u\n", wpr.seq, wpr.target_system); + + // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); + } + else + { + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("ERROR: Waypoint index exceeds list capacity\n"); + } +} + +/* + * @brief emits a message that a waypoint reached + * + * This function broadcasts a message that a waypoint is reached. + * + * @param seq The waypoint sequence number the MAV has reached. + */ +void mavlink_wpm_send_waypoint_reached(uint16_t seq) +{ + mavlink_message_t msg; + mavlink_waypoint_reached_t wp_reached; + + wp_reached.seq = seq; + + mavlink_msg_waypoint_reached_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wp_reached); + mavlink_wpm_send_message(&msg); + + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint reached message"); //// if (verbose) // printf("Sent waypoint %u reached message\n", wp_reached.seq); + + // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); +} + +//float mavlink_wpm_distance_to_segment(uint16_t seq, float x, float y, float z) +//{ +// if (seq < wpm.size) +// { +// mavlink_waypoint_t *cur = waypoints->at(seq); +// +// const PxVector3 A(cur->x, cur->y, cur->z); +// const PxVector3 C(x, y, z); +// +// // seq not the second last waypoint +// if ((uint16_t)(seq+1) < wpm.size) +// { +// mavlink_waypoint_t *next = waypoints->at(seq+1); +// const PxVector3 B(next->x, next->y, next->z); +// const float r = (B-A).dot(C-A) / (B-A).lengthSquared(); +// if (r >= 0 && r <= 1) +// { +// const PxVector3 P(A + r*(B-A)); +// return (P-C).length(); +// } +// else if (r < 0.f) +// { +// return (C-A).length(); +// } +// else +// { +// return (C-B).length(); +// } +// } +// else +// { +// return (C-A).length(); +// } +// } +// else +// { +// // if (verbose) // printf("ERROR: index out of bounds\n"); +// } +// return -1.f; +//} + +float mavlink_wpm_distance_to_point(uint16_t seq, float x, float y, float z) +{ + // if (seq < wpm.size) + // { + // mavlink_waypoint_t *cur = waypoints->at(seq); + // + // const PxVector3 A(cur->x, cur->y, cur->z); + // const PxVector3 C(x, y, z); + // + // return (C-A).length(); + // } + // else + // { + // // if (verbose) // printf("ERROR: index out of bounds\n"); + // } + return -1.f; +} + + +void mavlink_wpm_message_handler(const mavlink_message_t* msg) +{ + // Handle param messages + //paramClient->handleMAVLinkPacket(msg); + + //check for timed-out operations + uint64_t now = mavlink_wpm_get_system_timestamp(); + if (now-wpm.timestamp_lastaction > wpm.timeout && wpm.current_state != MAVLINK_WPM_STATE_IDLE) + { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("Operation timeout switching -> IDLE"); +#else + if (MAVLINK_WPM_VERBOSE) printf("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm.current_state); +#endif + wpm.current_state = MAVLINK_WPM_STATE_IDLE; + wpm.current_count = 0; + wpm.current_partner_sysid = 0; + wpm.current_partner_compid = 0; + wpm.current_wp_id = -1; + + if(wpm.size == 0) + { + wpm.current_active_wp_id = -1; + } + } + + if(now-wpm.timestamp_last_send_setpoint > wpm.delay_setpoint && wpm.current_active_wp_id < wpm.size) + { + mavlink_wpm_send_setpoint(wpm.current_active_wp_id); + } + + switch(msg->msgid) + { + case MAVLINK_MSG_ID_ATTITUDE: + { + if(msg->sysid == mavlink_system.sysid && wpm.current_active_wp_id < wpm.size) + { + mavlink_waypoint_t *wp = &(wpm.waypoints[wpm.current_active_wp_id]); + if(wp->frame == MAV_FRAME_LOCAL_ENU || wp->frame == MAV_FRAME_LOCAL_NED) + { + mavlink_attitude_t att; + mavlink_msg_attitude_decode(msg, &att); + float yaw_tolerance = wpm.accept_range_yaw; + //compare current yaw + if (att.yaw - yaw_tolerance >= 0.0f && att.yaw + yaw_tolerance < 2.f*M_PI) + { + if (att.yaw - yaw_tolerance <= wp->param4 && att.yaw + yaw_tolerance >= wp->param4) + wpm.yaw_reached = true; + } + else if(att.yaw - yaw_tolerance < 0.0f) + { + float lowerBound = 360.0f + att.yaw - yaw_tolerance; + if (lowerBound < wp->param4 || wp->param4 < att.yaw + yaw_tolerance) + wpm.yaw_reached = true; + } + else + { + float upperBound = att.yaw + yaw_tolerance - 2.f*M_PI; + if (att.yaw - yaw_tolerance < wp->param4 || wp->param4 < upperBound) + wpm.yaw_reached = true; + } + } + } + break; + } + + case MAVLINK_MSG_ID_LOCAL_POSITION: + { + if(msg->sysid == mavlink_system.sysid && wpm.current_active_wp_id < wpm.size) + { + mavlink_waypoint_t *wp = &(wpm.waypoints[wpm.current_active_wp_id]); + + if(wp->frame == MAV_FRAME_LOCAL_ENU || MAV_FRAME_LOCAL_NED) + { + mavlink_local_position_t pos; + mavlink_msg_local_position_decode(msg, &pos); + //// if (debug) // printf("Received new position: x: %f | y: %f | z: %f\n", pos.x, pos.y, pos.z); + + wpm.pos_reached = false; + + // compare current position (given in message) with current waypoint + float orbit = wp->param1; + + float dist; + if (wp->param2 == 0) + { + // FIXME segment distance + //dist = mavlink_wpm_distance_to_segment(current_active_wp_id, pos.x, pos.y, pos.z); + } + else + { + dist = mavlink_wpm_distance_to_point(wpm.current_active_wp_id, pos.x, pos.y, pos.z); + } + + if (dist >= 0.f && dist <= orbit && wpm.yaw_reached) + { + wpm.pos_reached = true; + } + } + } + break; + } + + // case MAVLINK_MSG_ID_CMD: // special action from ground station + // { + // mavlink_cmd_t action; + // mavlink_msg_cmd_decode(msg, &action); + // if(action.target == mavlink_system.sysid) + // { + // // if (verbose) std::cerr << "Waypoint: received message with action " << action.action << std::endl; + // switch (action.action) + // { + // // case MAV_ACTION_LAUNCH: + // // // if (verbose) std::cerr << "Launch received" << std::endl; + // // current_active_wp_id = 0; + // // if (wpm.size>0) + // // { + // // setActive(waypoints[current_active_wp_id]); + // // } + // // else + // // // if (verbose) std::cerr << "No launch, waypointList empty" << std::endl; + // // break; + // + // // case MAV_ACTION_CONTINUE: + // // // if (verbose) std::c + // // err << "Continue received" << std::endl; + // // idle = false; + // // setActive(waypoints[current_active_wp_id]); + // // break; + // + // // case MAV_ACTION_HALT: + // // // if (verbose) std::cerr << "Halt received" << std::endl; + // // idle = true; + // // break; + // + // // default: + // // // if (verbose) std::cerr << "Unknown action received with id " << action.action << ", no action taken" << std::endl; + // // break; + // } + // } + // break; + // } + + case MAVLINK_MSG_ID_WAYPOINT_ACK: + { + mavlink_waypoint_ack_t wpa; + mavlink_msg_waypoint_ack_decode(msg, &wpa); + + if((msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_system.compid*/)) + { + wpm.timestamp_lastaction = now; + + if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST || wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) + { + if (wpm.current_wp_id == wpm.size-1) + { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("Got last WP ACK state -> IDLE"); +#else + if (MAVLINK_WPM_VERBOSE) printf("Received ACK after having sent last waypoint, going to state MAVLINK_WPM_STATE_IDLE\n"); +#endif + wpm.current_state = MAVLINK_WPM_STATE_IDLE; + wpm.current_wp_id = 0; + } + } + } + else + { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("REJ. WP CMD: curr partner id mismatch"); +#else + if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); +#endif + } + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: + { + mavlink_waypoint_set_current_t wpc; + mavlink_msg_waypoint_set_current_decode(msg, &wpc); + + if(wpc.target_system == mavlink_system.sysid /*&& wpc.target_component == mavlink_system.compid*/) + { + wpm.timestamp_lastaction = now; + + if (wpm.current_state == MAVLINK_WPM_STATE_IDLE) + { + if (wpc.seq < wpm.size) + { + // if (verbose) // printf("Received MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT\n"); + wpm.current_active_wp_id = wpc.seq; + uint32_t i; + for(i = 0; i < wpm.size; i++) + { + if (i == wpm.current_active_wp_id) + { + wpm.waypoints[i].current = true; + } + else + { + wpm.waypoints[i].current = false; + } + } +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("NEW WP SET"); +#else + if (MAVLINK_WPM_VERBOSE) printf("New current waypoint %u\n", wpm.current_active_wp_id); +#endif + wpm.yaw_reached = false; + wpm.pos_reached = false; + mavlink_wpm_send_waypoint_current(wpm.current_active_wp_id); + mavlink_wpm_send_setpoint(wpm.current_active_wp_id); + wpm.timestamp_firstinside_orbit = 0; + } + else + { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("IGN WP CURR CMD: Not in list"); +#else + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: Index out of bounds\n"); +#endif + } + } + else + { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("IGN WP CURR CMD: Busy"); +#else + if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE NOT IN IDLE STATE\n"); +#endif + } + } + else + { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("REJ. WP CMD: target id mismatch"); +#else + if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); +#endif + } + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST: + { + mavlink_waypoint_request_list_t wprl; + mavlink_msg_waypoint_request_list_decode(msg, &wprl); + if(wprl.target_system == mavlink_system.sysid /*&& wprl.target_component == mavlink_system.compid*/) + { + wpm.timestamp_lastaction = now; + + if (wpm.current_state == MAVLINK_WPM_STATE_IDLE || wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) + { + if (wpm.size > 0) + { + if (verbose && wpm.current_state == MAVLINK_WPM_STATE_IDLE) // printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u changing state to MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid); + if (verbose && wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) // printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST again from %u staying in state MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid); + wpm.current_state = MAVLINK_WPM_STATE_SENDLIST; + wpm.current_wp_id = 0; + wpm.current_partner_sysid = msg->sysid; + wpm.current_partner_compid = msg->compid; + } + else + { + // if (verbose) // printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u but have no waypoints, staying in \n", msg->sysid); + } + wpm.current_count = wpm.size; + mavlink_wpm_send_waypoint_count(msg->sysid,msg->compid, wpm.current_count); + } + else + { + // if (verbose) // printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST because i'm doing something else already (state=%i).\n", wpm.current_state); + } + } + else + { + // if (verbose) // printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT MISMATCH\n"); + } + + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_REQUEST: + { + mavlink_waypoint_request_t wpr; + mavlink_msg_waypoint_request_decode(msg, &wpr); + if(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid && wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_system.compid*/) + { + wpm.timestamp_lastaction = now; + + //ensure that we are in the correct state and that the first request has id 0 and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint) + if ((wpm.current_state == MAVLINK_WPM_STATE_SENDLIST && wpr.seq == 0) || (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && (wpr.seq == wpm.current_wp_id || wpr.seq == wpm.current_wp_id + 1) && wpr.seq < wpm.size)) + { + if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) + { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("GOT WP REQ, state -> SEND"); +#else + if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); +#endif + } + if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm.current_wp_id + 1) + { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("GOT 2nd WP REQ"); +#else + if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); +#endif + } + if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm.current_wp_id) + { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("GOT 2nd WP REQ"); +#else + if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); +#endif + } + + wpm.current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; + wpm.current_wp_id = wpr.seq; + mavlink_wpm_send_waypoint(wpm.current_partner_sysid, wpm.current_partner_compid, wpr.seq); + } + else + { + // if (verbose) + { + if (!(wpm.current_state == MAVLINK_WPM_STATE_SENDLIST || wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS)) + { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("REJ. WP CMD: Busy"); +#else + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because i'm doing something else already (state=%i).\n", wpm.current_state); +#endif + break; + } + else if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) + { + if (wpr.seq != 0) + { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("REJ. WP CMD: First id != 0"); +#else + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the first requested waypoint ID (%u) was not 0.\n", wpr.seq); +#endif + } + } + else if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) + { + if (wpr.seq != wpm.current_wp_id && wpr.seq != wpm.current_wp_id + 1) + { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("REJ. WP CMD: Req. WP was unexpected"); +#else + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).\n", wpr.seq, wpm.current_wp_id, wpm.current_wp_id+1); +#endif + } + else if (wpr.seq >= wpm.size) + { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("REJ. WP CMD: Req. WP not in list"); +#else + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was out of bounds.\n", wpr.seq); +#endif + } + } + else + { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("REJ. WP CMD: ?"); +#else + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST - FIXME: missed error description\n"); +#endif + } + } + } + } + else + { + //we we're target but already communicating with someone else + if((wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_system.compid*/) && !(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid)) + { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("REJ. WP CMD: Busy"); +#else + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST from ID %u because i'm already talking to ID %u.\n", msg->sysid, wpm.current_partner_sysid); +#endif + } + else + { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("REJ. WP CMD: target id mismatch"); +#else + if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); +#endif + } + + } + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_COUNT: + { + mavlink_waypoint_count_t wpc; + mavlink_msg_waypoint_count_decode(msg, &wpc); + if(wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_system.compid*/) + { + wpm.timestamp_lastaction = now; + + if (wpm.current_state == MAVLINK_WPM_STATE_IDLE || (wpm.current_state == MAVLINK_WPM_STATE_GETLIST && wpm.current_wp_id == 0)) + { + if (wpc.count > 0) + { + if (wpm.current_state == MAVLINK_WPM_STATE_IDLE) + { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("WP CMD OK: state -> GETLIST"); +#else + if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST\n", wpc.count, msg->sysid); +#endif + } + if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST) + { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("WP CMD OK AGAIN"); +#else + if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) again from %u\n", wpc.count, msg->sysid); +#endif + } + + wpm.current_state = MAVLINK_WPM_STATE_GETLIST; + wpm.current_wp_id = 0; + wpm.current_partner_sysid = msg->sysid; + wpm.current_partner_compid = msg->compid; + wpm.current_count = wpc.count; + +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("CLR RCV BUF: READY"); +#else + if (MAVLINK_WPM_VERBOSE) printf("clearing receive buffer and readying for receiving waypoints\n"); +#endif + wpm.rcv_size = 0; + //while(waypoints_receive_buffer->size() > 0) + // { + // delete waypoints_receive_buffer->back(); + // waypoints_receive_buffer->pop_back(); + // } + + mavlink_wpm_send_waypoint_request(wpm.current_partner_sysid, wpm.current_partner_compid, wpm.current_wp_id); + } + else if (wpc.count == 0) + { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("COUNT 0"); +#else + if (MAVLINK_WPM_VERBOSE) printf("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE\n"); +#endif + wpm.rcv_size = 0; + //while(waypoints_receive_buffer->size() > 0) + // { + // delete waypoints->back(); + // waypoints->pop_back(); + // } + wpm.current_active_wp_id = -1; + wpm.yaw_reached = false; + wpm.pos_reached = false; + break; + + } + else + { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("IGN WP CMD"); +#else + if (MAVLINK_WPM_VERBOSE) printf("Ignoring MAVLINK_MSG_ID_WAYPOINT_COUNT from %u with count of %u\n", msg->sysid, wpc.count); +#endif + } + } + else + { + if (!(wpm.current_state == MAVLINK_WPM_STATE_IDLE || wpm.current_state == MAVLINK_WPM_STATE_GETLIST)) + { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("REJ. WP CMD: Busy"); +#else + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm doing something else already (state=%i).\n", wpm.current_state); +#endif + } + else if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST && wpm.current_wp_id != 0) + { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("REJ. WP CMD: Busy"); +#else + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm already receiving waypoint %u.\n", wpm.current_wp_id); +#endif + } + else + { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("REJ. WP CMD: ?"); +#else + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT - FIXME: missed error description\n"); +#endif + } + } + } + else + { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("REJ. WP CMD: target id mismatch"); +#else + if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); +#endif + } + + } + break; + + case MAVLINK_MSG_ID_WAYPOINT: + { + mavlink_waypoint_t wp; + mavlink_msg_waypoint_decode(msg, &wp); + + // if (verbose) // printf("GOT WAYPOINT!"); + + if((msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && (wp.target_system == mavlink_system.sysid /*&& wp.target_component == mavlink_system.compid*/)) + { + wpm.timestamp_lastaction = now; + + //ensure that we are in the correct state and that the first waypoint has id 0 and the following waypoints have the correct ids + if ((wpm.current_state == MAVLINK_WPM_STATE_GETLIST && wp.seq == 0) || (wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm.current_wp_id && wp.seq < wpm.current_count)) + { + if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST) // printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u changing state to MAVLINK_WPM_STATE_GETLIST_GETWPS\n", wp.seq, msg->sysid); + if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm.current_wp_id) // printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u\n", wp.seq, msg->sysid); + if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq-1 == wpm.current_wp_id) // printf("Got MAVLINK_MSG_ID_WAYPOINT %u (again) from %u\n", wp.seq, msg->sysid); + + wpm.current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS; + mavlink_waypoint_t* newwp = &(wpm.rcv_waypoints[wp.seq]); + memcpy(newwp, &wp, sizeof(mavlink_waypoint_t)); + + wpm.current_wp_id = wp.seq + 1; + + // if (verbose) // printf ("Added new waypoint to list. X= %f\t Y= %f\t Z= %f\t Yaw= %f\n", newwp->x, newwp->y, newwp->z, newwp->param4); + + if(wpm.current_wp_id == wpm.current_count && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) + { + // if (verbose) // printf("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm.current_count); + + mavlink_wpm_send_waypoint_ack(wpm.current_partner_sysid, wpm.current_partner_compid, 0); + + if (wpm.current_active_wp_id > wpm.rcv_size-1) + { + wpm.current_active_wp_id = wpm.rcv_size-1; + } + + // switch the waypoints list + // FIXME CHECK!!! + for (int i = 0; i < wpm.current_count; ++i) + { + wpm.waypoints[i] = wpm.rcv_waypoints[i]; + } + wpm.size = wpm.current_count; + + //get the new current waypoint + uint32_t i; + for(i = 0; i < wpm.size; i++) + { + if (wpm.waypoints[i].current == 1) + { + wpm.current_active_wp_id = i; + //// if (verbose) // printf("New current waypoint %u\n", current_active_wp_id); + wpm.yaw_reached = false; + wpm.pos_reached = false; + mavlink_wpm_send_waypoint_current(wpm.current_active_wp_id); + mavlink_wpm_send_setpoint(wpm.current_active_wp_id); + wpm.timestamp_firstinside_orbit = 0; + break; + } + } + + if (i == wpm.size) + { + wpm.current_active_wp_id = -1; + wpm.yaw_reached = false; + wpm.pos_reached = false; + wpm.timestamp_firstinside_orbit = 0; + } + + wpm.current_state = MAVLINK_WPM_STATE_IDLE; + } + else + { + mavlink_wpm_send_waypoint_request(wpm.current_partner_sysid, wpm.current_partner_compid, wpm.current_wp_id); + } + } + else + { + if (wpm.current_state == MAVLINK_WPM_STATE_IDLE) + { + //we're done receiving waypoints, answer with ack. + mavlink_wpm_send_waypoint_ack(wpm.current_partner_sysid, wpm.current_partner_compid, 0); + // printf("Received MAVLINK_MSG_ID_WAYPOINT while state=MAVLINK_WPM_STATE_IDLE, answered with WAYPOINT_ACK.\n"); + } + // if (verbose) + { + if (!(wpm.current_state == MAVLINK_WPM_STATE_GETLIST || wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS)) + { + // printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u because i'm doing something else already (state=%i).\n", wp.seq, wpm.current_state); + break; + } + else if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST) + { + if(!(wp.seq == 0)) + { + // printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the first waypoint ID (%u) was not 0.\n", wp.seq); + } + else + { + // printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); + } + } + else if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) + { + if (!(wp.seq == wpm.current_wp_id)) + { + // printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was not the expected %u.\n", wp.seq, wpm.current_wp_id); + } + else if (!(wp.seq < wpm.current_count)) + { + // printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was out of bounds.\n", wp.seq); + } + else + { + // printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); + } + } + else + { + // printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); + } + } + } + } + else + { + //we we're target but already communicating with someone else + if((wp.target_system == mavlink_system.sysid /*&& wp.target_component == mavlink_system.compid*/) && !(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && wpm.current_state != MAVLINK_WPM_STATE_IDLE) + { + // if (verbose) // printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i'm already talking to ID %u.\n", wp.seq, msg->sysid, wpm.current_partner_sysid); + } + else if(wp.target_system == mavlink_system.sysid /* && wp.target_component == mavlink_system.compid*/) + { + // if (verbose) // printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i have no idea what to do with it\n", wp.seq, msg->sysid); + } + } + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL: + { + mavlink_waypoint_clear_all_t wpca; + mavlink_msg_waypoint_clear_all_decode(msg, &wpca); + + if(wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_system.compid */ && wpm.current_state == MAVLINK_WPM_STATE_IDLE) + { + wpm.timestamp_lastaction = now; + + // if (verbose) // printf("Got MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u deleting all waypoints\n", msg->sysid); + // Delete all waypoints + wpm.size = 0; + wpm.current_active_wp_id = -1; + wpm.yaw_reached = false; + wpm.pos_reached = false; + } + else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_system.compid */ && wpm.current_state != MAVLINK_WPM_STATE_IDLE) + { + // if (verbose) // printf("Ignored MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, wpm.current_state); + } + break; + } + + default: + { + // if (debug) // printf("Waypoint: received message of unknown type"); + break; + } + } + + //check if the current waypoint was reached + if (wpm.pos_reached /*wpm.yaw_reached &&*/ && !wpm.idle) + { + if (wpm.current_active_wp_id < wpm.size) + { + mavlink_waypoint_t *cur_wp = &(wpm.waypoints[wpm.current_active_wp_id]); + + if (wpm.timestamp_firstinside_orbit == 0) + { + // Announce that last waypoint was reached + // if (verbose) // printf("*** Reached waypoint %u ***\n", cur_wp->seq); + mavlink_wpm_send_waypoint_reached(cur_wp->seq); + wpm.timestamp_firstinside_orbit = now; + } + + // check if the MAV was long enough inside the waypoint orbit + //if (now-timestamp_lastoutside_orbit > (cur_wp->hold_time*1000)) + if(now-wpm.timestamp_firstinside_orbit >= cur_wp->param2*1000) + { + if (cur_wp->autocontinue) + { + cur_wp->current = 0; + if (wpm.current_active_wp_id == wpm.size - 1 && wpm.size > 1) + { + //the last waypoint was reached, if auto continue is + //activated restart the waypoint list from the beginning + wpm.current_active_wp_id = 1; + } + else + { + if ((uint16_t)(wpm.current_active_wp_id + 1) < wpm.size) + wpm.current_active_wp_id++; + } + + // Fly to next waypoint + wpm.timestamp_firstinside_orbit = 0; + mavlink_wpm_send_waypoint_current(wpm.current_active_wp_id); + mavlink_wpm_send_setpoint(wpm.current_active_wp_id); + wpm.waypoints[wpm.current_active_wp_id].current = true; + wpm.pos_reached = false; + wpm.yaw_reached = false; + // if (verbose) // printf("Set new waypoint (%u)\n", wpm.current_active_wp_id); + } + } + } + } + else + { + wpm.timestamp_lastoutside_orbit = now; + } +} + diff --git a/thirdParty/mavlink/missionlib/waypoints.h b/thirdParty/mavlink/missionlib/waypoints.h new file mode 100644 index 0000000000000000000000000000000000000000..4bc32a14a4368cf68e2dd69b60b027c8a93f4e65 --- /dev/null +++ b/thirdParty/mavlink/missionlib/waypoints.h @@ -0,0 +1,91 @@ +/******************************************************************************* + + Copyright (C) 2011 Lorenz Meier lm ( a t ) inf.ethz.ch + + This program 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. + + This program 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 this program. If not, see . + + ****************************************************************************/ + +/* This assumes you have the mavlink headers on your include path + or in the same folder as this source file */ +#include +#include + +// FIXME XXX - TO BE MOVED TO XML +enum MAVLINK_WPM_STATES +{ + MAVLINK_WPM_STATE_IDLE = 0, + MAVLINK_WPM_STATE_SENDLIST, + MAVLINK_WPM_STATE_SENDLIST_SENDWPS, + MAVLINK_WPM_STATE_GETLIST, + MAVLINK_WPM_STATE_GETLIST_GETWPS, + MAVLINK_WPM_STATE_GETLIST_GOTALL, + MAVLINK_WPM_STATE_ENUM_END +}; + +enum MAVLINK_WPM_CODES +{ + MAVLINK_WPM_CODE_OK = 0, + MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED, + MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED, + MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS, + MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED, + MAVLINK_WPM_CODE_ENUM_END +}; + + +/* WAYPOINT MANAGER - MISSION LIB */ + +#define MAVLINK_WPM_MAX_WP_COUNT 100 +#define MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE ///< Enable double buffer and in-flight updates +#define MAVLINK_WPM_TEXT_FEEDBACK 1 ///< Report back status information as text +#define MAVLINK_WPM_SYSTEM_ID 1 +#define MAVLINK_WPM_COMPONENT_ID 1 +#define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 2000000 +#define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000 +#define MAVLINK_WPM_PROTOCOL_DELAY_DEFAULT 40 + + +struct mavlink_wpm_storage { + mavlink_waypoint_t waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Currently active waypoints +#ifdef MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE + mavlink_waypoint_t rcv_waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Receive buffer for next waypoints +#endif + uint16_t size; + uint16_t max_size; + uint16_t rcv_size; + enum MAVLINK_WPM_STATES current_state; + uint16_t current_wp_id; ///< Waypoint in current transmission + uint16_t current_active_wp_id; ///< Waypoint the system is currently heading towards + uint16_t current_count; + uint8_t current_partner_sysid; + uint8_t current_partner_compid; + uint64_t timestamp_lastaction; + uint64_t timestamp_last_send_setpoint; + uint64_t timestamp_firstinside_orbit; + uint64_t timestamp_lastoutside_orbit; + uint32_t timeout; + uint32_t delay_setpoint; + float accept_range_yaw; + float accept_range_distance; + bool yaw_reached; + bool pos_reached; + bool idle; +}; + +typedef struct mavlink_wpm_storage mavlink_wpm_storage; + +void mavlink_wpm_init(mavlink_wpm_storage* state); + +void mavlink_wpm_message_handler(const mavlink_message_t* msg); \ No newline at end of file