diff --git a/.gitignore b/.gitignore index f37b7d808cb527fe1d9835c534ac1d461c698363..dd317da215b5c04f614c853d05299f8d2fe90f8e 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/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 e6250e7258110308690efdbf6e7e8243123d0327..3ce864cecf3fe1070c750d3b79ccb0f949547219 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -38,6 +38,7 @@ MOC_DIR = $$BUILDDIR/moc UI_HEADERS_DIR = $$BUILDDIR/ui RCC_DIR = $$BUILDDIR/rcc MAVLINK_CONF = "" +DEFINES += MAVLINK_NO_DATA ################################################################# @@ -235,7 +236,7 @@ HEADERS += src/MG.h \ src/comm/SerialSimulationLink.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 \ @@ -299,7 +300,6 @@ HEADERS += src/MG.h \ src/ui/uas/QGCUnconnectedInfoWidget.h \ src/ui/designer/QGCToolWidget.h \ src/ui/designer/QGCParamSlider.h \ - src/ui/designer/QGCActionButton.h \ src/ui/designer/QGCCommandButton.h \ src/ui/designer/QGCToolWidgetItem.h \ src/ui/QGCMAVLinkLogPlayer.h \ @@ -364,7 +364,7 @@ SOURCES += src/main.cc \ src/comm/SerialLink.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 \ @@ -426,7 +426,6 @@ SOURCES += src/main.cc \ src/ui/uas/QGCUnconnectedInfoWidget.cc \ src/ui/designer/QGCToolWidget.cc \ src/ui/designer/QGCParamSlider.cc \ - src/ui/designer/QGCActionButton.cc \ src/ui/designer/QGCCommandButton.cc \ src/ui/designer/QGCToolWidgetItem.cc \ src/ui/QGCMAVLinkLogPlayer.cc \ diff --git a/src/QGCCore.cc b/src/QGCCore.cc index ca6bf0c9af2014a1c7ea6413005845c2b1eca77a..c1f460a0c75b0df5e0d1eff122da698a8f1a48f6 100644 --- a/src/QGCCore.cc +++ b/src/QGCCore.cc @@ -44,7 +44,6 @@ This file is part of the QGROUNDCONTROL project #include "configuration.h" #include "QGC.h" #include "QGCCore.h" -#include "MG.h" #include "MainWindow.h" #include "GAudioOutput.h" diff --git a/src/Waypoint.cc b/src/Waypoint.cc index 2fd5a7099fd5c1ff1c793d72c90c712fbdeba3e6..bfcde0c4c194806facc8fc51af292dd21f98dbf6 100644 --- a/src/Waypoint.cc +++ b/src/Waypoint.cc @@ -109,7 +109,8 @@ void Waypoint::setId(quint16 id) void Waypoint::setX(double x) { - if (this->x != x && (this->frame == MAV_FRAME_LOCAL)) { + if (!isinf(x) && !isnan(x) && ((this->frame == MAV_FRAME_LOCAL_NED) || (this->frame == MAV_FRAME_LOCAL_ENU))) + { this->x = x; emit changed(this); } @@ -117,7 +118,8 @@ void Waypoint::setX(double x) void Waypoint::setY(double y) { - if (this->y != y && (this->frame == MAV_FRAME_LOCAL)) { + if (!isinf(y) && !isnan(y) && ((this->frame == MAV_FRAME_LOCAL_NED) || (this->frame == MAV_FRAME_LOCAL_ENU))) + { this->y = y; emit changed(this); } @@ -125,7 +127,8 @@ void Waypoint::setY(double y) void Waypoint::setZ(double z) { - if (this->z != z && (this->frame == MAV_FRAME_LOCAL)) { + if (!isinf(z) && !isnan(z) && ((this->frame == MAV_FRAME_LOCAL_NED) || (this->frame == MAV_FRAME_LOCAL_ENU))) + { this->z = z; emit changed(this); } @@ -133,7 +136,8 @@ void Waypoint::setZ(double z) void Waypoint::setLatitude(double lat) { - if (this->x != lat && ((this->frame == MAV_FRAME_GLOBAL) || (this->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT))) { + if (this->x != lat && ((this->frame == MAV_FRAME_GLOBAL) || (this->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT))) + { this->x = lat; emit changed(this); } @@ -141,7 +145,8 @@ void Waypoint::setLatitude(double lat) void Waypoint::setLongitude(double lon) { - if (this->y != lon && ((this->frame == MAV_FRAME_GLOBAL) || (this->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT))) { + if (this->y != lon && ((this->frame == MAV_FRAME_GLOBAL) || (this->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT))) + { this->y = lon; emit changed(this); } @@ -149,7 +154,8 @@ void Waypoint::setLongitude(double lon) void Waypoint::setAltitude(double altitude) { - if (this->z != altitude && ((this->frame == MAV_FRAME_GLOBAL) || (this->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT))) { + if (this->z != altitude && ((this->frame == MAV_FRAME_GLOBAL) || (this->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT))) + { this->z = altitude; emit changed(this); } @@ -157,7 +163,8 @@ void Waypoint::setAltitude(double altitude) void Waypoint::setYaw(int yaw) { - if (this->yaw != yaw) { + if (this->yaw != yaw) + { this->yaw = yaw; emit changed(this); } @@ -165,7 +172,8 @@ void Waypoint::setYaw(int yaw) void Waypoint::setYaw(double yaw) { - if (this->yaw != yaw) { + if (this->yaw != yaw) + { this->yaw = yaw; emit changed(this); } @@ -173,7 +181,8 @@ void Waypoint::setYaw(double yaw) void Waypoint::setAction(int action) { - if (this->action != (MAV_CMD)action) { + if (this->action != (MAV_CMD)action) + { this->action = (MAV_CMD)action; emit changed(this); } @@ -205,7 +214,8 @@ void Waypoint::setAutocontinue(bool autoContinue) void Waypoint::setCurrent(bool current) { - if (this->current != current) { + if (this->current != current) + { this->current = current; emit changed(this); } @@ -213,7 +223,8 @@ void Waypoint::setCurrent(bool current) void Waypoint::setAcceptanceRadius(double radius) { - if (this->param2 != radius) { + if (this->param2 != radius) + { this->param2 = radius; emit changed(this); } @@ -223,7 +234,8 @@ void Waypoint::setParam1(double param1) { //qDebug() << "SENDER:" << QObject::sender(); //qDebug() << "PARAM1 SET REQ:" << param1; - if (this->param1 != param1) { + if (this->param1 != param1) + { this->param1 = param1; emit changed(this); } @@ -231,7 +243,8 @@ void Waypoint::setParam1(double param1) void Waypoint::setParam2(double param2) { - if (this->param2 != param2) { + if (this->param2 != param2) + { this->param2 = param2; emit changed(this); } 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() +{ +} + +/** + * + * CALCULATE THE CHECKSUM + * + */ + +#define X25_INIT_CRC 0xffff +#define X25_VALIDATE_CRC 0xf0b8 + +/** + * The checksum function adds the hash of one char at a time to the + * 16 bit checksum (uint16_t). + * + * @param data new char to hash + * @param crcAccum the already accumulated checksum + **/ +void MAVLinkXMLParserV10::crcAccumulate(uint8_t data, uint16_t *crcAccum) +{ + /*Accumulate one byte of data into the CRC*/ + uint8_t tmp; + + tmp=data ^ (uint8_t)(*crcAccum &0xff); + tmp^= (tmp<<4); + *crcAccum = (*crcAccum>>8) ^ (tmp<<8) ^ (tmp <<3) ^ (tmp>>4); +} + +/** + * @param crcAccum the 16 bit X.25 CRC + */ +void MAVLinkXMLParserV10::crcInit(uint16_t* crcAccum) +{ + *crcAccum = X25_INIT_CRC; +} + + +const struct { + const char *prefix; + unsigned length; +} length_map[] = { + { "array", 1 }, + { "char", 1 }, + { "uint8", 1 }, + { "int8", 1 }, + { "uint16", 2 }, + { "int16", 2 }, + { "uint32", 4 }, + { "int32", 4 }, + { "uint64", 8 }, + { "int64", 8 }, + { "float", 4 }, + { "double", 8 }, +}; + +unsigned itemLength( const QString &s1 ) +{ + unsigned el1, i1, i2; + QString Ss1 = s1; + + Ss1 = Ss1.replace("_"," "); + Ss1 = Ss1.simplified(); + Ss1 = Ss1.section(" ",0,0); + + el1 = i1 = 0; + i2 = sizeof(length_map)/sizeof(length_map[0]); + + do { + if (Ss1.startsWith(length_map[i1].prefix)) + el1 = length_map[i1].length; + else ; + i1++; + } while ( (el1 == 0) && (i1 < i2)); + return el1; +} + +bool structSort(const QString &s1, const QString &s2) +{ + unsigned el1, el2; + + el1 = itemLength( s1 ); + el2 = itemLength( s2 ); + return el2 < el1; +} + +/** + * Generate C-code (C-89 compliant) out of the XML protocol specs. + */ +bool MAVLinkXMLParserV10::generate() +{ + uint16_t crc_key = X25_INIT_CRC; + // 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; + static unsigned message_lengths[256]; + static unsigned message_key[256]; + static int highest_message_id; + static int recursion_level; + + if (recursion_level == 0) { + highest_message_id = 0; + memset(message_lengths, 0, sizeof(message_lengths)); + } + + + // 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"; + + 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()) + { + // If this is the only field, don't add the separator + if (pp.nextSibling().isNull()) + { + fieldComment += pp2.nodeValue(); + } + else + { + fieldComment += pp2.nodeValue() + sep; + } + } + else if (pp2.isElement()) + { + // If this is the only field, don't add the separator + if (pp.nextSibling().isNull()) + { + fieldComment += pp2.text(); + } + else + { + 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; + unsigned message_length = 0; + + + // Get the message fields + QDomNode f = e.firstChild(); + + // The field types and order are hashed with a checksum + + // Initialize CRC + uint16_t fieldHash; + crcInit(&fieldHash); + + 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); + 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); + 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); + decodeLines = itemList.join("\n") + "\n"; + } + } + + // ARRAYS are not longer supported - leave error message in here to inform users! + else if (fieldType.startsWith("array")) + { + emit parseState(tr("ERROR: In message %1 deprecated type used near line %2 of file %3. Please change from array[size] to uint8_t[size] to get the same behaviour.\nAbort.").arg(messageName, QString::number(e.lineNumber()), fileName)); + return false; + } + else if (fieldType.startsWith("string")) + { + int arrayLength = QString(fieldType.split("[").at(1).split("]").first()).toInt(); + // String array is always unsigned char, so bytes + calculatedLength += arrayLength; + 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("\n\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); + 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); + 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); + 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 invalid 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); + 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); + 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); + 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); + 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); + 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); + decodeLines = itemList.join("\n") + "\n"; + } + 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; + } + } + + // message length calculation + unsigned element_multiplier = 1; + unsigned element_length = 0; + + if (fieldType.contains("[")) { + element_multiplier = fieldType.split("[").at(1).split("]").first().toInt(); + } + for (unsigned i=0; iERROR: Unable to calculate length for %2 near line %1\nAbort.").arg(QString::number(e.lineNumber()), fieldType)); + return false; + } + message_length += element_length; + + + // + // 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(); + } + + if (messageId > highest_message_id) { + highest_message_id = messageId; + } + message_lengths[messageId] = message_length; + + + // Sort fields to ensure 16bit-boundary aligned data + QStringList fieldList; + // Stable sort fields in C structure + fieldList = cStructLines.split("\n", QString::SkipEmptyParts); + if (fieldList.size() > 1) + { + qStableSort(fieldList.begin(), fieldList.end(), structSort); + } else ; + + // struct now sorted, do crc calc for each field + QString fieldCRCstring; + QByteArray fieldText; + crc_key = X25_INIT_CRC; + + for (int i =0; i < fieldList.size(); i++) + { + fieldCRCstring = fieldList.at(i).simplified(); + fieldCRCstring = fieldCRCstring.section(" ",0,1); // note: this has one space bewteen type and name + fieldText = fieldCRCstring.toAscii(); + for (int i = 0; i < fieldText.size(); ++i) + { + crcAccumulate((uint8_t) fieldText.at(i), &crc_key); + } + } + + // generate the key byte value + QString stringCRC; + message_key[messageId] = (crc_key&0xff)^((crc_key>>8)&0xff); + stringCRC = stringCRC.number( message_key[messageId], 16); + + // create structure + cStructLines = fieldList.join("\n") + "\n"; + + + + // 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 compact2Send("\n\n#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS\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.arg(stringCRC.toUpper()); +// QString cFile = "// MESSAGE " + messageName.toUpper() + " PACKING\n\n" + idDefine.arg(messageName.toUpper(), QString::number(messageId), QString::number(calculatedLength), stringCRC.toUpper() ) + "\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; + QString cFile = "// MESSAGE " + messageName.toUpper() + " PACKING\n\n" + idDefine.arg(messageName.toUpper(), QString::number(messageId), QString::number(calculatedLength), stringCRC.toUpper() ) + "\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) + 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)); + + emit parseState(tr("Compiled message %1 \t(#%3) \tend near line %2, length %4, crc key 0x%5(%6)").arg(messageName, QString::number(n.lineNumber()), QString::number(messageId), QString::number(message_lengths[messageId]), stringCRC.toUpper(), QString::number(message_key[messageId]))); + } // 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); + } + + // CRC seeds + mainHeader += "\n\n// MESSAGE CRC KEYS\n\n"; + mainHeader += "#undef MAVLINK_MESSAGE_KEYS\n"; + mainHeader += "#define MAVLINK_MESSAGE_KEYS { "; + 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 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: + /** @brief Accumulate the X.25 CRC by adding one char at a time. */ + void crcAccumulate(uint8_t data, uint16_t *crcAccum); + + /** @brief Initialize the buffer for the X.25 CRC */ + void crcInit(uint16_t* crcAccum); + + 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/AS4Protocol.cc b/src/comm/AS4Protocol.cc deleted file mode 100644 index 5e0c56002542da02c9adbb0258078f520ff412c6..0000000000000000000000000000000000000000 --- a/src/comm/AS4Protocol.cc +++ /dev/null @@ -1,154 +0,0 @@ -/*===================================================================== - -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 Brief Description - * - * @author Lorenz Meier - * - */ - -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "QGC.h" - - -AS4Protocol::AS4Protocol() -{ - - // Start heartbeat timer, emitting a heartbeat at the configured rate - heartbeatRate = 1; ///< SAE AS-4 has a fixed heartbeat rate of 1 hz. -// heartbeatTimer = new QTimer(this); -// connect(heartbeatTimer, SIGNAL(timeout()), this, SLOT()); -// heartbeatTimer->start(1000/heartbeatRate); - /* - // Start the node manager - configData = new FileLoader("nodeManager.conf"); - handler = new MyHandler(); - - try - { - nodeManager = new NodeManager(configData, handler); - qDebug() << "SAE AS-4 NODE MANAGER constructed"; - } - catch(char *exceptionString) - { - printf("%s", exceptionString); - printf("Terminating Program...\n"); - } - catch(...) - { - printf("Node Manager Construction Failed. Terminating Program...\n"); - } - */ - -} - -AS4Protocol::~AS4Protocol() -{ -// delete nodeManager; -// delete handler; -// delete configData; -} - -void AS4Protocol::run() -{ - forever { - QGC::SLEEP::msleep(5000); - } -} - - -/** - * @brief Receive bytes from a communication interface. - * - * The bytes copied by calling the LinkInterface::readBytes() method. - * - * @param link The interface to read from - * @see LinkInterface - **/ -void AS4Protocol::receiveBytes(LinkInterface* link) -{ -// receiveMutex.lock(); - // Prepare buffer - //static const int maxlen = 4096 * 100; - //static char buffer[maxlen]; - - qint64 bytesToRead = link->bytesAvailable(); - - // Get all data at once, let link read the bytes in the buffer array - //link->readBytes(buffer, maxlen); -// -// /* -// // Debug output -// std::cerr << "receive buffer: "; -// for (int i = 0; i < bytesToRead; i++) -// { -// std::cerr << std::hex << static_cast(buffer[i]); -// } -// std::cerr << std::dec << " length: " << bytesToRead; -// */ -// -// qDebug() << __FILE__ << __LINE__ << ": buffer size:" << maxlen << "bytes:" << bytesToRead; -// - - for (int position = 0; position < bytesToRead; position++) { - } -// receiveMutex.unlock(); - -} - - -/** - * @brief Get the human-readable name of this protocol. - * - * @return The name of this protocol - **/ -QString AS4Protocol::getName() -{ - return QString(tr("SAE AS-4")); -} - -void AS4Protocol::setHeartbeatRate(int rate) -{ - heartbeatRate = rate; - heartbeatTimer->setInterval(1000/heartbeatRate); -} - -int AS4Protocol::getHeartbeatRate() -{ - return heartbeatRate; -} diff --git a/src/comm/AS4Protocol.h b/src/comm/AS4Protocol.h deleted file mode 100644 index 00a42df5bf37551d52d77c7f84210f36b699cec1..0000000000000000000000000000000000000000 --- a/src/comm/AS4Protocol.h +++ /dev/null @@ -1,155 +0,0 @@ -/*===================================================================== - -PIXHAWK Micro Air Vehicle Flying Robotics Toolkit - -(c) 2009 PIXHAWK PROJECT - -This file is part of the PIXHAWK project - - PIXHAWK 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. - - PIXHAWK is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with PIXHAWK. If not, see . - -======================================================================*/ - -/** - * @file - * @brief Brief Description - * - * @author Lorenz Meier - * - */ - -#ifndef AS4PROTOCOL_H_ -#define AS4PROTOCOL_H_ - -#include -#include -#include -#include -#include -#include -#include -#include -/*#include - -class MyHandler : public EventHandler -{ -public: - ~MyHandler() - { - - } - - void handleEvent(NodeManagerEvent *e) - { - SystemTreeEvent *treeEvent; - ErrorEvent *errorEvent; - JausMessageEvent *messageEvent; - DebugEvent *debugEvent; - ConfigurationEvent *configEvent; - - switch(e->getType()) - { - case NodeManagerEvent::SystemTreeEvent: - treeEvent = (SystemTreeEvent *)e; - printf("%s\n", treeEvent->toString().c_str()); - delete e; - break; - - case NodeManagerEvent::ErrorEvent: - errorEvent = (ErrorEvent *)e; - printf("%s\n", errorEvent->toString().c_str()); - delete e; - break; - - case NodeManagerEvent::JausMessageEvent: - messageEvent = (JausMessageEvent *)e; - // If you turn this on, the system gets spam-y this is very useful for debug purposes - if(messageEvent->getJausMessage()->commandCode != JAUS_REPORT_HEARTBEAT_PULSE) - { - //printf("%s\n", messageEvent->toString().c_str()); - } - else - { - //printf("%s\n", messageEvent->toString().c_str()); - } - delete e; - break; - - case NodeManagerEvent::DebugEvent: - debugEvent = (DebugEvent *)e; - //printf("%s\n", debugEvent->toString().c_str()); - delete e; - break; - - case NodeManagerEvent::ConfigurationEvent: - configEvent = (ConfigurationEvent *)e; - printf("%s\n", configEvent->toString().c_str()); - delete e; - break; - - default: - delete e; - break; - } - } -};*/ - -/** - * SAE AS-4 Nodemanager - * - **/ -class AS4Protocol : public ProtocolInterface -{ - Q_OBJECT - -public: - AS4Protocol(); - ~AS4Protocol(); - - void run(); - QString getName(); - int getHeartbeatRate(); - -public slots: - void receiveBytes(LinkInterface* link); - /** - * @brief Set the rate at which heartbeats are emitted - * - * The default rate is 1 Hertz. - * - * @param rate heartbeat rate in hertz (times per second) - */ - void setHeartbeatRate(int rate); - - /** - * @brief Send an extra heartbeat to all connected units - * - * The heartbeat is sent out of order and does not reset the - * periodic heartbeat emission. It will be just sent in addition. - */ -// void sendHeartbeat(); - -protected: - QTimer* heartbeatTimer; - int heartbeatRate; - QMutex receiveMutex; -// NodeManager* nodeManager; -// MyHandler* handler; -// FileLoader* configData; - -signals: - -}; - -#endif // AS4PROTOCOL_H_ diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index 69ee72af4d437d2e56a7eee212d545e22eb9ac3b..ebb6d2eaa0c3d3566d74f98c4da7fe1ed8caf112 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,11 +27,13 @@ #include "ArduPilotMegaMAV.h" #include "configuration.h" #include "LinkManager.h" -//#include "MainWindow.h" #include "QGCMAVLink.h" #include "QGCMAVLinkUASFactory.h" #include "QGC.h" +// Instantiate MAVLink data +#include "mavlink_data.h" + /** * The default constructor will create a new MAVLink object sending heartbeats at * the MAVLINK_HEARTBEAT_DEFAULT_RATE to all connected links. @@ -395,7 +396,7 @@ void MAVLinkProtocol::sendHeartbeat() { if (m_heartbeatsEnabled) { mavlink_message_t beat; - mavlink_msg_heartbeat_pack(getSystemId(), getComponentId(),&beat, OCU, MAV_AUTOPILOT_GENERIC); + mavlink_msg_heartbeat_pack(getSystemId(), getComponentId(),&beat, MAV_TYPE_OCU, MAV_CLASS_INVALID); sendMessage(beat); } if (m_authEnabled) { diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc index 50e87e9ff790391052a43fe341b2341d39ca25df..db29cc0258fa93ff469cfbe65840effd46e02188 100644 --- a/src/comm/MAVLinkSimulationLink.cc +++ b/src/comm/MAVLinkSimulationLink.cc @@ -409,7 +409,7 @@ void MAVLinkSimulationLink::mainloop() // streampointer += bufferlength; // GLOBAL POSITION - mavlink_msg_global_position_int_pack(systemId, componentId, &ret, (473780.28137103+(x))*1E3, (85489.9892510421+(y))*1E3, (z+550.0)*1000.0, xSpeed, ySpeed, zSpeed); + mavlink_msg_global_position_int_pack(systemId, componentId, &ret, (473780.28137103+(x))*1E3, (85489.9892510421+(y))*1E3, (z+550.0)*1000.0, xSpeed, ySpeed, zSpeed, yaw); bufferlength = mavlink_msg_to_send_buffer(buffer, &ret); //add data into datastream memcpy(stream+streampointer,buffer, bufferlength); @@ -546,16 +546,17 @@ void MAVLinkSimulationLink::mainloop() static int typeCounter = 0; uint8_t mavType; if (typeCounter < 10) { - mavType = MAV_QUADROTOR; + mavType = MAV_TYPE_QUADROTOR; } else { - mavType = typeCounter % (OCU); + mavType = typeCounter % (MAV_TYPE_OCU); } typeCounter++; // Pack message and get size of encoded byte string - messageSize = mavlink_msg_heartbeat_pack(systemId, componentId, &msg, mavType, MAV_AUTOPILOT_PIXHAWK); + messageSize = mavlink_msg_heartbeat_pack(systemId, componentId, &msg, mavType, MAV_CLASS_PIXHAWK); // Allocate buffer with packet data bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); + qDebug() << "CRC:" << msg.ck_a << msg.ck_b; //add data into datastream memcpy(stream+streampointer,buffer, bufferlength); streampointer += bufferlength; @@ -716,37 +717,38 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size) } break; // EXECUTE OPERATOR ACTIONS - case MAVLINK_MSG_ID_ACTION: { - mavlink_action_t action; - mavlink_msg_action_decode(&msg, &action); - - qDebug() << "SIM" << "received action" << action.action << "for system" << action.target; - - switch (action.action) { - case MAV_ACTION_LAUNCH: - status.status = MAV_STATE_ACTIVE; - status.mode = MAV_MODE_AUTO; - break; - case MAV_ACTION_RETURN: - status.status = MAV_STATE_ACTIVE; - break; - case MAV_ACTION_MOTORS_START: - status.status = MAV_STATE_ACTIVE; - status.mode = MAV_MODE_LOCKED; - break; - case MAV_ACTION_MOTORS_STOP: - status.status = MAV_STATE_STANDBY; - status.mode = MAV_MODE_LOCKED; - break; - case MAV_ACTION_EMCY_KILL: - status.status = MAV_STATE_EMERGENCY; - status.mode = MAV_MODE_MANUAL; - break; - case MAV_ACTION_SHUTDOWN: - status.status = MAV_STATE_POWEROFF; - status.mode = MAV_MODE_LOCKED; - break; - } + case MAVLINK_MSG_ID_COMMAND: { + mavlink_command_t action; + mavlink_msg_command_decode(&msg, &action); + + qDebug() << "SIM" << "received action" << action.command << "for system" << action.target_system; + + // FIXME MAVLINKV10PORTINGNEEDED +// switch (action.action) { +// case MAV_ACTION_LAUNCH: +// status.status = MAV_STATE_ACTIVE; +// status.mode = MAV_MODE_AUTO; +// break; +// case MAV_ACTION_RETURN: +// status.status = MAV_STATE_ACTIVE; +// break; +// case MAV_ACTION_MOTORS_START: +// status.status = MAV_STATE_ACTIVE; +// status.mode = MAV_MODE_LOCKED; +// break; +// case MAV_ACTION_MOTORS_STOP: +// status.status = MAV_STATE_STANDBY; +// status.mode = MAV_MODE_LOCKED; +// break; +// case MAV_ACTION_EMCY_KILL: +// status.status = MAV_STATE_EMERGENCY; +// status.mode = MAV_MODE_MANUAL; +// break; +// case MAV_ACTION_SHUTDOWN: +// status.status = MAV_STATE_POWEROFF; +// status.mode = MAV_MODE_LOCKED; +// break; +// } } break; #ifdef MAVLINK_ENABLED_PIXHAWK @@ -771,7 +773,7 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size) for (i = onboardParams.begin(); i != onboardParams.end(); ++i) { if (j != 5) { // Pack message and get size of encoded byte string - mavlink_msg_param_value_pack(read.target_system, componentId, &msg, (int8_t*)i.key().toStdString().c_str(), i.value(), onboardParams.size(), j); + mavlink_msg_param_value_pack(read.target_system, componentId, &msg, i.key().toStdString().c_str(), i.value(), onboardParams.size(), j); // Allocate buffer with packet data bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); //add data into datastream @@ -799,7 +801,7 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size) onboardParams.insert(key, set.param_value); // Pack message and get size of encoded byte string - mavlink_msg_param_value_pack(set.target_system, componentId, &msg, (int8_t*)key.toStdString().c_str(), set.param_value, onboardParams.size(), onboardParams.keys().indexOf(key)); + mavlink_msg_param_value_pack(set.target_system, componentId, &msg, key.toStdString().c_str(), set.param_value, onboardParams.size(), onboardParams.keys().indexOf(key)); // Allocate buffer with packet data bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); //add data into datastream @@ -820,7 +822,7 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size) float paramValue = onboardParams.value(key); // Pack message and get size of encoded byte string - mavlink_msg_param_value_pack(read.target_system, componentId, &msg, (int8_t*)key.toStdString().c_str(), paramValue, onboardParams.size(), onboardParams.keys().indexOf(key)); + mavlink_msg_param_value_pack(read.target_system, componentId, &msg, key.toStdString().c_str(), paramValue, onboardParams.size(), onboardParams.keys().indexOf(key)); // Allocate buffer with packet data bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); //add data into datastream @@ -832,7 +834,7 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size) float paramValue = onboardParams.value(key); // Pack message and get size of encoded byte string - mavlink_msg_param_value_pack(read.target_system, componentId, &msg, (int8_t*)key.toStdString().c_str(), paramValue, onboardParams.size(), onboardParams.keys().indexOf(key)); + mavlink_msg_param_value_pack(read.target_system, componentId, &msg, key.toStdString().c_str(), paramValue, onboardParams.size(), onboardParams.keys().indexOf(key)); // Allocate buffer with packet data bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); //add data into datastream diff --git a/src/comm/MAVLinkSimulationMAV.cc b/src/comm/MAVLinkSimulationMAV.cc index 356104601ddf338dbe1c95ad0837dc8a654aaffc..51205297a57781b01da187d4b9e999d9a62237d5 100644 --- a/src/comm/MAVLinkSimulationMAV.cc +++ b/src/comm/MAVLinkSimulationMAV.cc @@ -68,7 +68,7 @@ void MAVLinkSimulationMAV::mainloop() // 1 Hz execution if (timer1Hz <= 0) { mavlink_message_t msg; - mavlink_msg_heartbeat_pack_version_free(systemid, MAV_COMP_ID_IMU, &msg, MAV_FIXED_WING, MAV_AUTOPILOT_PIXHAWK, mavlink_version); + mavlink_msg_heartbeat_pack(systemid, MAV_COMP_ID_IMU, &msg, MAV_TYPE_FIXED_WING, MAV_CLASS_PIXHAWK); link->sendMAVLinkMessage(&msg); planner.handleMessage(msg); @@ -306,30 +306,31 @@ void MAVLinkSimulationMAV::handleMessage(const mavlink_message_t& msg) if (systemid == mode.target) sys_mode = mode.mode; } break; - case MAVLINK_MSG_ID_ACTION: { - mavlink_action_t action; - mavlink_msg_action_decode(&msg, &action); - if (systemid == action.target && (action.target_component == 0 || action.target_component == MAV_COMP_ID_IMU)) { - mavlink_action_ack_t ack; - ack.action = action.action; - switch (action.action) { - case MAV_ACTION_TAKEOFF: - flying = true; - nav_mode = MAV_NAV_LIFTOFF; - ack.result = 1; - break; - default: { - ack.result = 0; - } - break; - } - - // Give feedback about action - mavlink_message_t r_msg; - mavlink_msg_action_ack_encode(systemid, MAV_COMP_ID_IMU, &r_msg, &ack); - link->sendMAVLinkMessage(&r_msg); - } - } + // FIXME MAVLINKV10PORTINGNEEDED +// case MAVLINK_MSG_ID_ACTION: { +// mavlink_action_t action; +// mavlink_msg_action_decode(&msg, &action); +// if (systemid == action.target && (action.target_component == 0 || action.target_component == MAV_COMP_ID_IMU)) { +// mavlink_action_ack_t ack; +// ack.action = action.action; +//// switch (action.action) { +//// case MAV_ACTION_TAKEOFF: +//// flying = true; +//// nav_mode = MAV_NAV_LIFTOFF; +//// ack.result = 1; +//// break; +//// default: { +//// ack.result = 0; +//// } +//// break; +//// } + +// // Give feedback about action +// mavlink_message_t r_msg; +// mavlink_msg_action_ack_encode(systemid, MAV_COMP_ID_IMU, &r_msg, &ack); +// link->sendMAVLinkMessage(&r_msg); +// } +// } break; case MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET: { mavlink_local_position_setpoint_set_t sp; diff --git a/src/comm/MAVLinkSimulationMAV.h b/src/comm/MAVLinkSimulationMAV.h index 4f24eb7c52771434a9b9aff1cbe16d48c3679085..8417ea350846c0bffa95ca5d32b12a31c15fe2b2 100644 --- a/src/comm/MAVLinkSimulationMAV.h +++ b/src/comm/MAVLinkSimulationMAV.h @@ -55,16 +55,17 @@ protected: bool flying; int mavlink_version; - static inline uint16_t mavlink_msg_heartbeat_pack_version_free(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t type, uint8_t autopilot, uint8_t version) { - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_HEARTBEAT; + // FIXME MAVLINKV10PORTINGNEEDED +// static inline uint16_t mavlink_msg_heartbeat_pack_version_free(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t type, uint8_t autopilot, uint8_t version) { +// uint16_t i = 0; +// msg->msgid = MAVLINK_MSG_ID_HEARTBEAT; - i += put_uint8_t_by_index(type, i, msg->payload); // Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) - i += put_uint8_t_by_index(autopilot, i, msg->payload); // Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM - i += put_uint8_t_by_index(version, i, msg->payload); // MAVLink version +// i += put_uint8_t_by_index(type, i, msg->payload); // Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) +// i += put_uint8_t_by_index(autopilot, i, msg->payload); // Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM +// i += put_uint8_t_by_index(version, i, msg->payload); // MAVLink version - return mavlink_finalize_message(msg, system_id, component_id, i); - } +// return mavlink_finalize_message(msg, system_id, component_id, i); +// } }; diff --git a/src/comm/MAVLinkSimulationWaypointPlanner.cc b/src/comm/MAVLinkSimulationWaypointPlanner.cc index d78b4d5bfe0e96d38f1a5fd497b2f83474956eb0..0f229d2f57f5034c692bed1b3965ea152c2c1301 100644 --- a/src/comm/MAVLinkSimulationWaypointPlanner.cc +++ b/src/comm/MAVLinkSimulationWaypointPlanner.cc @@ -815,11 +815,11 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* break; } - case MAVLINK_MSG_ID_ACTION: { // special action from ground station - mavlink_action_t action; - mavlink_msg_action_decode(msg, &action); - if(action.target == systemid) { - if (verbose) qDebug("Waypoint: received message with action %d\n", action.action); + case MAVLINK_MSG_ID_COMMAND: { // special action from ground station + mavlink_command_t action; + mavlink_msg_command_decode(msg, &action); + if(action.target_system == systemid) { + if (verbose) qDebug("Waypoint: received message with action %d\n", action.command); // switch (action.action) { // case MAV_ACTION_LAUNCH: // if (verbose) std::cerr << "Launch received" << std::endl; 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..478d5d87d8186177e53de53924e7408e8dce8601 100644 --- a/src/uas/PxQuadMAV.cc +++ b/src/uas/PxQuadMAV.cc @@ -65,7 +65,7 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) mavlink_msg_pattern_detected_decode(&message, &detected); QByteArray b; b.resize(256); - mavlink_msg_pattern_detected_get_file(&message, (int8_t*)b.data()); + mavlink_msg_pattern_detected_get_file(&message, b.data()); b.append('\0'); QString name = QString(b); if (detected.type == 0) diff --git a/src/uas/QGCMAVLinkUASFactory.cc b/src/uas/QGCMAVLinkUASFactory.cc index e9081d960eae7995ad52bc4c4e11b414a52c0e4e..7b9703d2ac4bf0615af81110cf8144af1b89875c 100644 --- a/src/uas/QGCMAVLinkUASFactory.cc +++ b/src/uas/QGCMAVLinkUASFactory.cc @@ -10,16 +10,21 @@ UASInterface* QGCMAVLinkUASFactory::createUAS(MAVLinkProtocol* mavlink, LinkInte { QPointer p; - if (parent != NULL) { + if (parent != NULL) + { p = parent; - } else { + } + else + { p = mavlink; } UASInterface* uas; - switch (heartbeat->autopilot) { - case MAV_AUTOPILOT_GENERIC: { + switch (heartbeat->autopilot) + { + case MAV_CLASS_GENERIC: + { UAS* mav = new UAS(mavlink, sysid); // Set the system type mav->setSystemType((int)heartbeat->type); @@ -28,7 +33,8 @@ UASInterface* QGCMAVLinkUASFactory::createUAS(MAVLinkProtocol* mavlink, LinkInte uas = mav; } break; - case MAV_AUTOPILOT_PIXHAWK: { + case MAV_CLASS_PIXHAWK: + { PxQuadMAV* mav = new PxQuadMAV(mavlink, sysid); // Set the system type mav->setSystemType((int)heartbeat->type); @@ -40,7 +46,8 @@ UASInterface* QGCMAVLinkUASFactory::createUAS(MAVLinkProtocol* mavlink, LinkInte uas = mav; } break; - case MAV_AUTOPILOT_SLUGS: { + case MAV_CLASS_SLUGS: + { SlugsMAV* mav = new SlugsMAV(mavlink, sysid); // Set the system type mav->setSystemType((int)heartbeat->type); @@ -52,7 +59,8 @@ UASInterface* QGCMAVLinkUASFactory::createUAS(MAVLinkProtocol* mavlink, LinkInte uas = mav; } break; - case MAV_AUTOPILOT_ARDUPILOTMEGA: { + case MAV_CLASS_ARDUPILOTMEGA: + { ArduPilotMegaMAV* mav = new ArduPilotMegaMAV(mavlink, sysid); // Set the system type mav->setSystemType((int)heartbeat->type); @@ -64,7 +72,8 @@ UASInterface* QGCMAVLinkUASFactory::createUAS(MAVLinkProtocol* mavlink, LinkInte uas = mav; } break; - default: { + default: + { UAS* mav = new UAS(mavlink, sysid); mav->setSystemType((int)heartbeat->type); // Connect this robot to the UAS object diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index e62fc127a0b282962946ad4d6a769c163062815f..0b405f608b74cac21ad4263951b6bab1b783cc62 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,23 +189,31 @@ 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) { - case MAV_FIXED_WING: + if (airframe == 0) + { + switch (type) + { + case MAV_TYPE_FIXED_WING: setAirframe(UASInterface::QGC_AIRFRAME_EASYSTAR); break; - case MAV_QUADROTOR: + case MAV_TYPE_QUADROTOR: setAirframe(UASInterface::QGC_AIRFRAME_CHEETAH); break; default: @@ -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,7 +263,8 @@ 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; } @@ -255,7 +272,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) emit loadChanged(this,state.load/10.0f); 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,21 +376,28 @@ 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(); } @@ -376,7 +405,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) break; #ifdef MAVLINK_ENABLED_PIXHAWK - case MAVLINK_MSG_ID_CONTROL_STATUS: { + case MAVLINK_MSG_ID_CONTROL_STATUS: + { mavlink_control_status_t status; mavlink_msg_control_status_decode(&message, &status); // Emit control status vector @@ -392,7 +422,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) } 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 +439,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,9 +451,9 @@ 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: @@ -430,7 +462,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) { 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 +496,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,7 +510,8 @@ 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); } @@ -487,7 +522,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) emit speedChanged(this, hud.airspeed, 0.0f, hud.climb, getUnixTime()); } 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 +599,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 +667,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); @@ -644,7 +682,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) if (pos.fix_type > 0) { emit globalPositionChanged(this, pos.lat/(double)1E7, pos.lon/(double)1E7, pos.alt/1000.0, time); - emit valueChanged(uasId, "gps speed", "m/s", pos.v, time); + emit valueChanged(uasId, "gps speed", "m/s", pos.vel, time); latitude = pos.lat/(double)1E7; longitude = pos.lon/(double)1E7; altitude = pos.alt/1000.0; @@ -658,31 +696,38 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) } emit valueChanged(uasId, "altitude", "m", pos.alt/(double)1E3, time); // Smaller than threshold and not NaN - if (pos.v < 1000000 && pos.v == pos.v) { - emit valueChanged(uasId, "speed", "m/s", pos.v, time); + + float vel = pos.vel/100.0f; + + if (vel < 1000000 && !isnan(vel) && !isinf(vel)) { + emit valueChanged(uasId, "speed", "m/s", vel, time); //qDebug() << "GOT GPS RAW"; // emit speedChanged(this, (double)pos.v, 0.0, 0.0, time); } else { - emit textMessageReceived(uasId, message.compid, 255, QString("GCS ERROR: RECEIVED INVALID SPEED OF %1 m/s").arg(pos.v)); + emit textMessageReceived(uasId, message.compid, 255, QString("GCS ERROR: RECEIVED INVALID SPEED OF %1 m/s").arg(vel)); } } } 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 +738,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 +749,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 +762,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 +788,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 +798,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()); } @@ -753,19 +812,23 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) emit parameterChanged(uasId, message.compid, value.param_count, value.param_index, parameterName, val); } break; - case MAVLINK_MSG_ID_ACTION_ACK: - mavlink_action_ack_t ack; - mavlink_msg_action_ack_decode(&message, &ack); - if (ack.result == 1) { - emit textMessageReceived(uasId, message.compid, 0, tr("SUCCESS: Executed action: %1").arg(ack.action)); - } else { - emit textMessageReceived(uasId, message.compid, 0, tr("FAILURE: Rejected action: %1").arg(ack.action)); + case MAVLINK_MSG_ID_CMD_ACK: + mavlink_cmd_ack_t ack; + mavlink_msg_cmd_ack_decode(&message, &ack); + if (ack.result == 1) + { + emit textMessageReceived(uasId, message.compid, 0, tr("SUCCESS: Executed CMD: %1").arg(ack.cmd)); + } + else + { + emit textMessageReceived(uasId, message.compid, 0, tr("FAILURE: Rejected CMD: %1").arg(ack.cmd)); } 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: { + 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(); @@ -775,7 +838,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) emit valueChanged(uasId, "att control yaw", "raw", out.yaw, time/1000.0f); } break; - case MAVLINK_MSG_ID_POSITION_CONTROLLER_OUTPUT: { + 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(); @@ -785,44 +849,65 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) emit valueChanged(uasId, "pos control z", "raw", out.z, 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); } + else + { + qDebug() << "Got waypoint message, but was not for me"; + } } 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); } + else + { + qDebug() << "Got waypoint message, but was not for me"; + } } 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); } + else + { + qDebug() << "Got waypoint message, but was not for me"; + } } 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 +917,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,10 +947,11 @@ 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_STATUSTEXT: + { QByteArray b; b.resize(MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN); - mavlink_msg_statustext_get_text(&message, (int8_t*)b.data()); + mavlink_msg_statustext_get_text(&message, b.data()); //b.append('\0'); QString text = QString(b); int severity = mavlink_msg_statustext_get_severity(&message); @@ -872,7 +961,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 +975,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 +990,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 +1011,85 @@ 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; + // 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_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 #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 +1101,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 +1135,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,15 +1182,17 @@ void UAS::setLocalOriginAtCurrentGPSPosition() QTimer::singleShot(5000, &msgBox, SLOT(reject())); - 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 - sendMessage(msg); - // Wait 5 ms - MG::SLEEP::usleep(5000); - // Send again - sendMessage(msg); + if (ret == QMessageBox::Yes) + { + // FIXME MAVLINKV10PORTINGNEEDED +// 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 +// sendMessage(msg); +// // Wait 5 ms +// MG::SLEEP::usleep(5000); +// // Send again +// sendMessage(msg); result = true; } } @@ -1078,57 +1226,121 @@ void UAS::setLocalPositionOffset(float x, float y, float z, float yaw) } void UAS::startRadioControlCalibration() -{ - mavlink_message_t msg; - mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_CALIBRATE_RC); - sendMessage(msg); +{// FIXME MAVLINKV10PORTINGNEEDED +// mavlink_message_t msg; +// mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_CMD_DO_RC_CALIB; +// sendMessage(msg); } void UAS::startDataRecording() { - mavlink_message_t msg; - mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_REC_START); - sendMessage(msg); + // FIXME MAVLINKV10PORTINGNEEDED +// mavlink_message_t msg; +// mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_REC_START); +// sendMessage(msg); } void UAS::pauseDataRecording() { - mavlink_message_t msg; - mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_REC_PAUSE); - sendMessage(msg); + // FIXME MAVLINKV10PORTINGNEEDED +// mavlink_message_t msg; +// mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_REC_PAUSE); +// sendMessage(msg); } void UAS::stopDataRecording() { - mavlink_message_t msg; - mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_REC_STOP); - sendMessage(msg); + // FIXME MAVLINKV10PORTINGNEEDED +// mavlink_message_t msg; +// mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_REC_STOP); +// sendMessage(msg); } void UAS::startMagnetometerCalibration() { - mavlink_message_t msg; - mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_CALIBRATE_MAG); - sendMessage(msg); + // FIXME MAVLINKV10PORTINGNEEDED +// mavlink_message_t msg; +// mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_CALIBRATE_MAG); +// sendMessage(msg); } void UAS::startGyroscopeCalibration() { - mavlink_message_t msg; - mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_CALIBRATE_GYRO); - sendMessage(msg); + // FIXME MAVLINKV10PORTINGNEEDED +// mavlink_message_t msg; +// mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_CALIBRATE_GYRO); +// sendMessage(msg); } void UAS::startPressureCalibration() { - mavlink_message_t msg; - mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_CALIBRATE_PRESSURE); - sendMessage(msg); + // FIXME MAVLINKV10PORTINGNEEDED +// mavlink_message_t msg; +// mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_CALIBRATE_PRESSURE); +// sendMessage(msg); +} + +quint64 UAS::getUnixReferenceTime(quint64 time) +{ + // 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) { - if (time == 0) { + if (attitudeStamped) + { + return lastAttitude; + } + if (time == 0) + { // qDebug() << "XNEW time:" < has to be // a Unix epoch timestamp. Do nothing. return time/1000; @@ -1168,9 +1383,12 @@ quint64 UAS::getUnixTime(quint64 time) QList UAS::getParameterNames(int component) { - if (parameters.contains(component)) { + if (parameters.contains(component)) + { return parameters.value(component)->keys(); - } else { + } + else + { return QList(); } } @@ -1182,13 +1400,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 +1417,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 +1436,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 +1482,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 +1518,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 +1649,12 @@ void UAS::requestImage() **/ quint64 UAS::getUptime() const { - if(startTime == 0) { + if(startTime == 0) + { return 0; - } else { + } + else + { return MG::TIME::getGroundTimeNow() - startTime; } } @@ -1439,19 +1674,21 @@ void UAS::requestParameters() void UAS::writeParametersToStorage() { - mavlink_message_t msg; - // TODO Replace MG System ID with static function call and allow to change ID in GUI - mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(),MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_STORAGE_WRITE); - //mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(),(uint8_t)MAV_ACTION_STORAGE_WRITE); - sendMessage(msg); + // FIXME MAVLINKV10PORTINGNEEDED +// mavlink_message_t msg; +// // TODO Replace MG System ID with static function call and allow to change ID in GUI +// mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(),MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_STORAGE_WRITE); +// //mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(),(uint8_t)MAV_ACTION_STORAGE_WRITE); +// sendMessage(msg); } void UAS::readParametersFromStorage() { - mavlink_message_t msg; - // TODO Replace MG System ID with static function call and allow to change ID in GUI - mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU,(uint8_t)MAV_ACTION_STORAGE_READ); - sendMessage(msg); + // FIXME MAVLINKV10PORTINGNEEDED +// mavlink_message_t msg; +// // TODO Replace MG System ID with static function call and allow to change ID in GUI +// mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU,(uint8_t)MAV_ACTION_STORAGE_READ); +// sendMessage(msg); } void UAS::enableAllDataTransmission(int rate) @@ -1692,7 +1929,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 +1938,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 +1951,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,12 +1977,14 @@ void UAS::setSystemType(int systemType) { type = systemType; // If the airframe is still generic, change it to a close default type - if (airframe == 0) { - switch (systemType) { - case MAV_FIXED_WING: + if (airframe == 0) + { + switch (systemType) + { + case MAV_TYPE_FIXED_WING: airframe = QGC_AIRFRAME_EASYSTAR; break; - case MAV_QUADROTOR: + case MAV_TYPE_QUADROTOR: airframe = QGC_AIRFRAME_MIKROKOPTER; break; } @@ -1789,31 +2032,19 @@ void UAS::executeCommand(MAV_CMD command, int confirmation, float param1, float sendMessage(msg); } -/** - * Sets an action - * - **/ -void UAS::setAction(MAV_ACTION action) -{ - mavlink_message_t msg; - mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), 0, action); - // Send message twice to increase chance that it reaches its goal - sendMessage(msg); - sendMessage(msg); -} - /** * Launches the system * **/ void UAS::launch() { - mavlink_message_t msg; - // TODO Replace MG System ID with static function call and allow to change ID in GUI - mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_TAKEOFF); - // Send message twice to increase chance of reception - sendMessage(msg); - sendMessage(msg); + // FIXME MAVLINKV10PORTINGNEEDED +// mavlink_message_t msg; +// // TODO Replace MG System ID with static function call and allow to change ID in GUI +// mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_TAKEOFF); +// // Send message twice to increase chance of reception +// sendMessage(msg); +// sendMessage(msg); } /** @@ -1822,12 +2053,12 @@ void UAS::launch() **/ void UAS::enable_motors() { - mavlink_message_t msg; - // TODO Replace MG System ID with static function call and allow to change ID in GUI - mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_MOTORS_START); - // Send message twice to increase chance of reception - sendMessage(msg); - sendMessage(msg); +// mavlink_message_t msg; +// // FIXME MAVLINKV10PORTINGNEEDED +// //mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_MOTORS_START); +// // Send message twice to increase chance of reception +// sendMessage(msg); +// sendMessage(msg); } /** @@ -1836,12 +2067,12 @@ void UAS::enable_motors() **/ void UAS::disable_motors() { - mavlink_message_t msg; - // TODO Replace MG System ID with static function call and allow to change ID in GUI - mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_MOTORS_STOP); - // Send message twice to increase chance of reception - sendMessage(msg); - sendMessage(msg); +// mavlink_message_t msg; +// // FIXME MAVLINKV10PORTINGNEEDED +// mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_MOTORS_STOP); +// // Send message twice to increase chance of reception +// sendMessage(msg); +// sendMessage(msg); } void UAS::setManualControlCommands(double roll, double pitch, double yaw, double thrust) @@ -1856,14 +2087,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 +2109,8 @@ int UAS::getSystemType() void UAS::receiveButton(int buttonIndex) { - switch (buttonIndex) { + switch (buttonIndex) + { case 0: break; @@ -1953,35 +2188,36 @@ 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); - + // FIXME MAVLINKV10PORTINGNEEDED +// 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() { - 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_CONTINUE); - // Send message twice to increase chance of reception - sendMessage(msg); - sendMessage(msg); + // FIXME MAVLINKV10PORTINGNEEDED +// 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_CONTINUE); +// // Send message twice to increase chance of reception +// sendMessage(msg); +// sendMessage(msg); } /** Order the robot to return home / to land on the runway **/ void UAS::home() { - 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_RETURN); - // Send message twice to increase chance of reception - sendMessage(msg); - sendMessage(msg); + // FIXME MAVLINKV10PORTINGNEEDED +// 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_RETURN); +// // Send message twice to increase chance of reception +// sendMessage(msg); +// sendMessage(msg); } /** @@ -1990,100 +2226,102 @@ 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); - // Send message twice to increase chance of reception - sendMessage(msg); - sendMessage(msg); +// mavlink_message_t msg; +// // TODO Replace MG System ID with static function call and allow to change ID in GUI +// // FIXME MAVLINKV10PORTINGNEEDED +// //mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (int)MAV_ACTION_EMCY_LAND); +// // Send message twice to increase chance of reception +// sendMessage(msg); +// sendMessage(msg); } /** - * All systems are immediately shut down (e.g. the main power line is cut). + * Shut down this mav - All onboard systems are immediately shut down (e.g. the main power line is cut). * @warning This might lead to a crash * * The command will not be executed until emergencyKILLConfirm is issues immediately afterwards */ bool UAS::emergencyKILL() { - bool result = false; - QMessageBox msgBox; - msgBox.setIcon(QMessageBox::Critical); - msgBox.setText("EMERGENCY: KILL ALL MOTORS ON UAS"); - msgBox.setInformativeText("Do you want to cut power on all systems?"); - msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::Cancel); - msgBox.setDefaultButton(QMessageBox::Cancel); - int ret = msgBox.exec(); - - // Close the message box shortly after the click to prevent accidental clicks - QTimer::singleShot(5000, &msgBox, SLOT(reject())); - - - 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); - // Send message twice to increase chance of reception - sendMessage(msg); - sendMessage(msg); - result = true; - } - return result; + // FIXME MAVLINKV10PORTINGNEEDED +// bool result = false; +// QMessageBox msgBox; +// msgBox.setIcon(QMessageBox::Critical); +// msgBox.setText("EMERGENCY: KILL ALL MOTORS ON UAS"); +// msgBox.setInformativeText("Do you want to cut power on all systems?"); +// msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::Cancel); +// msgBox.setDefaultButton(QMessageBox::Cancel); +// int ret = msgBox.exec(); + +// // Close the message box shortly after the click to prevent accidental clicks +// QTimer::singleShot(5000, &msgBox, SLOT(reject())); + + +// 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); +// // Send message twice to increase chance of reception +// sendMessage(msg); +// sendMessage(msg); +// result = true; +// } +// return result; + return false; } 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); - + // FIXME MAVLINKV10PORTINGNEEDED +// 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); - + // FIXME MAVLINKV10PORTINGNEEDED +// 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); } void UAS::shutdown() { - bool result = false; - QMessageBox msgBox; - msgBox.setIcon(QMessageBox::Critical); - msgBox.setText("Shutting down the UAS"); - msgBox.setInformativeText("Do you want to shut down the onboard computer?"); + // FIXME MAVLINKV10PORTINGNEEDED +// bool result = false; +// QMessageBox msgBox; +// msgBox.setIcon(QMessageBox::Critical); +// msgBox.setText("Shutting down the UAS"); +// msgBox.setInformativeText("Do you want to shut down the onboard computer?"); - msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::Cancel); - msgBox.setDefaultButton(QMessageBox::Cancel); - int ret = msgBox.exec(); +// msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::Cancel); +// msgBox.setDefaultButton(QMessageBox::Cancel); +// int ret = msgBox.exec(); - // Close the message box shortly after the click to prevent accidental clicks - QTimer::singleShot(5000, &msgBox, SLOT(reject())); +// // Close the message box shortly after the click to prevent accidental clicks +// QTimer::singleShot(5000, &msgBox, SLOT(reject())); +// 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 +// mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU,(int)MAV_ACTION_SHUTDOWN); +// // Send message twice to increase chance of reception +// sendMessage(msg); +// sendMessage(msg); - 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 - mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU,(int)MAV_ACTION_SHUTDOWN); - // Send message twice to increase chance of reception - sendMessage(msg); - sendMessage(msg); - - result = true; - } +// result = true; +// } } void UAS::setTargetPosition(float x, float y, float z, float yaw) @@ -2102,9 +2340,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 +2363,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 +2373,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 +2394,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 +2415,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 +2451,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 +2461,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 +2489,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 +2509,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 +2519,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..32703e8eb2f3212a2951710824b4af5284518585 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 */ @@ -253,8 +255,6 @@ public slots: } /** @brief Set a new name **/ void setUASName(const QString& name); - /** @brief Executes an action **/ - void setAction(MAV_ACTION action); /** @brief Executes a command **/ void executeCommand(MAV_CMD command); /** @brief Executes a command **/ @@ -403,6 +403,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..76707ceb73be6b110d252eb9fc540b87f9a70cd6 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -188,8 +188,6 @@ public slots: /** @brief Set a new name for the system */ virtual void setUASName(const QString& name) = 0; - /** @brief Sets an action **/ - virtual void setAction(MAV_ACTION action) = 0; /** @brief Execute command immediately **/ virtual void executeCommand(MAV_CMD command) = 0; /** @brief Executes a command **/ diff --git a/src/uas/UASWaypointManager.cc b/src/uas/UASWaypointManager.cc index c541cb3f93ce034a3e63adb60a508a386bbcd5ab..340470c2f0a2d9828dfe04be8a5df39085ada172 100644 --- a/src/uas/UASWaypointManager.cc +++ b/src/uas/UASWaypointManager.cc @@ -32,7 +32,6 @@ This file is part of the QGROUNDCONTROL project #include "UASWaypointManager.h" #include "UAS.h" #include "mavlink_types.h" -//#include "MainWindow.h" #define PROTOCOL_TIMEOUT_MS 2000 ///< maximum time to wait for pending messages until timeout #define PROTOCOL_DELAY_MS 40 ///< minimum delay between sent messages @@ -586,8 +585,10 @@ int UASWaypointManager::getLocalFrameCount() // Search through all waypoints, // counting only those in global frame int i = 0; - foreach (Waypoint* p, waypoints) { - if (p->getFrame() == MAV_FRAME_GLOBAL) { + foreach (Waypoint* p, waypoints) + { + if (p->getFrame() == MAV_FRAME_GLOBAL) + { i++; } } @@ -600,8 +601,10 @@ int UASWaypointManager::getLocalFrameIndexOf(Waypoint* wp) // Search through all waypoints, // counting only those in local frame int i = 0; - foreach (Waypoint* p, waypoints) { - if (p->getFrame() == MAV_FRAME_LOCAL) { + foreach (Waypoint* p, waypoints) + { + if (p->getFrame() == MAV_FRAME_LOCAL_NED || p->getFrame() == MAV_FRAME_LOCAL_ENU) + { if (p == wp) { return i; } @@ -617,7 +620,8 @@ int UASWaypointManager::getMissionFrameIndexOf(Waypoint* wp) // Search through all waypoints, // counting only those in mission frame int i = 0; - foreach (Waypoint* p, waypoints) { + foreach (Waypoint* p, waypoints) + { if (p->getFrame() == MAV_FRAME_MISSION) { if (p == wp) { return i; diff --git a/src/ui/DebugConsole.cc b/src/ui/DebugConsole.cc index a65e6de1da58a7124f3eca23e72d1dd113c141a7..6474f324940ab2784bee024cbceaa0474941c370 100644 --- a/src/ui/DebugConsole.cc +++ b/src/ui/DebugConsole.cc @@ -355,7 +355,7 @@ void DebugConsole::receiveBytes(LinkInterface* link, QByteArray bytes) for (int j = 0; j < len; j++) { unsigned char byte = bytes.at(j); - // Filter MAVLink (http://pixhawk.ethz.ch/wiki/mavlink/) messages out of the stream. + // Filter MAVLink (http://qgroundcontrol.org/mavlink/) messages out of the stream. if (filterMAVLINK) { if (this->bytesToIgnore > 0) diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc index 27f40b8498561284b41b4e838c9437ba589b4e92..bc3e193d3a6c28b2745f58c27128471d7f3d1043 100644 --- a/src/ui/HSIDisplay.cc +++ b/src/ui/HSIDisplay.cc @@ -792,7 +792,8 @@ void HSIDisplay::drawWaypoints(QPainter& painter) for (int i = 0; i < list.size(); i++) { QPointF in; - if (list.at(i)->getFrame() == MAV_FRAME_LOCAL) { + if (list.at(i)->getFrame() == MAV_FRAME_LOCAL_NED) + { // Do not transform in = QPointF(list.at(i)->getX(), list.at(i)->getY()); } else { diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index 882a23fca1e940582636539992d6a0f230aa1383..7f135543f51ccb90498db04ef00da4cc8640d7ea 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); @@ -1629,7 +1634,7 @@ void MainWindow::UASCreated(UASInterface* uas) } switch (uas->getAutopilotType()) { - case (MAV_AUTOPILOT_SLUGS): { + case (MAV_CLASS_SLUGS): { // Build Slugs Widgets buildSlugsWidgets(); @@ -1667,9 +1672,9 @@ void MainWindow::UASCreated(UASInterface* uas) } break; default: - case (MAV_AUTOPILOT_GENERIC): - case (MAV_AUTOPILOT_ARDUPILOTMEGA): - case (MAV_AUTOPILOT_PIXHAWK): { + case (MAV_CLASS_GENERIC): + case (MAV_CLASS_ARDUPILOTMEGA): + case (MAV_CLASS_PIXHAWK): { // Build Pixhawk Widgets buildPxWidgets(); @@ -2005,4 +2010,4 @@ void MainWindow::loadDataView(QString fileName) QList MainWindow::listLinkMenuActions(void) { return ui.menuNetwork->actions(); -} \ No newline at end of file +} diff --git a/src/ui/MainWindow.h b/src/ui/MainWindow.h index 94098066b30587449d654a814664c0d3d8a31929..ecac1fec86dea2dad19f9918c2bce56cd79663f1 100644 --- a/src/ui/MainWindow.h +++ b/src/ui/MainWindow.h @@ -50,7 +50,6 @@ This file is part of the QGROUNDCONTROL project #include "UASListWidget.h" #include "MAVLinkProtocol.h" #include "MAVLinkSimulationLink.h" -#include "AS4Protocol.h" #include "ObjectDetectionView.h" #include "HUD.h" #include "JoystickWidget.h" @@ -76,6 +75,7 @@ This file is part of the QGROUNDCONTROL project #include "SlugsPadCameraControl.h" #include "UASControlParameters.h" +#include "QGCFlightGearLink.h" class QGCMapTool; @@ -372,7 +372,6 @@ protected: // TODO Should be moved elsewhere, as the protocol does not belong to the UI MAVLinkProtocol* mavlink; - AS4Protocol* as4link; MAVLinkSimulationLink* simulationLink; LinkInterface* udpLink; @@ -449,6 +448,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/RadioCalibration/RadioCalibrationWindow.cc b/src/ui/RadioCalibration/RadioCalibrationWindow.cc index 21ae680661abd5d9749f5d3d9ec372fa16037fe8..5933f7e3dfc50c5215dbdfdf7ac10faa55f4afd5 100644 --- a/src/ui/RadioCalibration/RadioCalibrationWindow.cc +++ b/src/ui/RadioCalibration/RadioCalibrationWindow.cc @@ -270,13 +270,14 @@ void RadioCalibrationWindow::send() void RadioCalibrationWindow::request() { - qDebug() << __FILE__ << __LINE__ << "READ FROM UAV"; - UAS *uas = dynamic_cast(UASManager::instance()->getUASForId(uasId)); - if (uas) { - mavlink_message_t msg; - mavlink_msg_action_pack(uasId, 0, &msg, 0, 0, ::MAV_ACTION_CALIBRATE_RC); - uas->sendMessage(msg); - } + // FIXME MAVLINKV10PORTINGNEEDED +// qDebug() << __FILE__ << __LINE__ << "READ FROM UAV"; +// UAS *uas = dynamic_cast(UASManager::instance()->getUASForId(uasId)); +// if (uas) { +// mavlink_message_t msg; +// mavlink_msg_action_pack(uasId, 0, &msg, 0, 0, ::MAV_ACTION_CALIBRATE_RC); +// uas->sendMessage(msg); +// } } void RadioCalibrationWindow::receive(const QPointer& radio) diff --git a/src/ui/WaypointView.cc b/src/ui/WaypointView.cc index 9d157ca142c2dd5ee323bb0f2d1ece6323ba4007..516b68e3f56887a261ba2cc41254ebe656828167 100644 --- a/src/ui/WaypointView.cc +++ b/src/ui/WaypointView.cc @@ -52,7 +52,7 @@ WaypointView::WaypointView(Waypoint* wp, QWidget* parent) : // add frames m_ui->comboBox_frame->addItem("Abs. Alt/Global",MAV_FRAME_GLOBAL); m_ui->comboBox_frame->addItem("Rel. Alt/Global", MAV_FRAME_GLOBAL_RELATIVE_ALT); - m_ui->comboBox_frame->addItem("Local/Abs. Alt.",MAV_FRAME_LOCAL); + m_ui->comboBox_frame->addItem("Local/Abs. Alt.",MAV_FRAME_LOCAL_NED); m_ui->comboBox_frame->addItem("Mission",MAV_FRAME_MISSION); // Initialize view correctly @@ -322,7 +322,7 @@ void WaypointView::updateFrameView(int frame) m_ui->comboBox_frame->show(); m_ui->customActionWidget->hide(); break; - case MAV_FRAME_LOCAL: + case MAV_FRAME_LOCAL_NED: m_ui->lonSpinBox->hide(); m_ui->latSpinBox->hide(); m_ui->altSpinBox->hide(); @@ -386,7 +386,7 @@ void WaypointView::updateValues() updateFrameView(frame); } switch(frame) { - case MAV_FRAME_LOCAL: { + case MAV_FRAME_LOCAL_NED: { if (m_ui->posNSpinBox->value() != wp->getX()) { m_ui->posNSpinBox->setValue(wp->getX()); } diff --git a/src/ui/designer/QGCActionButton.cc b/src/ui/designer/QGCActionButton.cc deleted file mode 100644 index 72753e91ff1f804369e3aa76b1ad29a91c3b6ab6..0000000000000000000000000000000000000000 --- a/src/ui/designer/QGCActionButton.cc +++ /dev/null @@ -1,136 +0,0 @@ -#include "QGCActionButton.h" -#include "ui_QGCActionButton.h" - -#include "MAVLinkProtocol.h" -#include "UASManager.h" - -const char* kActionLabels[MAV_ACTION_NB] = { - "HOLD", - "START MOTORS", - "LAUNCH", - "RETURN", - "EMERGENCY LAND", - "EMERGENCY KILL", - "CONFIRM KILL", - "CONTINUE", - "STOP MOTORS", - "HALT", - "SHUTDOWN", - "REBOOT", - "SET MANUAL", - "SET AUTO", - "READ STORAGE", - "WRITE STORAGE", - "CALIBRATE RC", - "CALIBRATE GYRO", - "CALIBRATE MAG", - "CALIBRATE ACC", - "CALIBRATE PRESSURE", - "START REC", - "PAUSE REC", - "STOP REC", - "TAKEOFF", - "NAVIGATE", - "LAND", - "LOITER", - "SET ORIGIN", - "RELAY ON", -//"RELAY OFF", -//"GET IMAGE", -//"START VIDEO", -//"STOP VIDEO", - "RESET MAP", - "RESET PLAN" -}; - -QGCActionButton::QGCActionButton(QWidget *parent) : - QGCToolWidgetItem("Button", parent), - ui(new Ui::QGCActionButton), - uas(NULL) -{ - ui->setupUi(this); - - connect(ui->actionButton, SIGNAL(clicked()), this, SLOT(sendAction())); - connect(ui->editFinishButton, SIGNAL(clicked()), this, SLOT(endEditMode())); - connect(ui->editButtonName, SIGNAL(textChanged(QString)), this, SLOT(setActionButtonName(QString))); - connect(ui->editActionComboBox, SIGNAL(currentIndexChanged(QString)), ui->nameLabel, SLOT(setText(QString))); - - // Hide all edit items - ui->editActionComboBox->hide(); - ui->editActionsRefreshButton->hide(); - ui->editFinishButton->hide(); - ui->editNameLabel->hide(); - ui->editButtonName->hide(); - - // add action labels to combobox - for (int i = 0; i < MAV_ACTION_NB; i++) { - ui->editActionComboBox->addItem(kActionLabels[i]); - } -} - -QGCActionButton::~QGCActionButton() -{ - delete ui; -} - -void QGCActionButton::sendAction() -{ - if (QGCToolWidgetItem::uas) { - MAV_ACTION action = static_cast( - ui->editActionComboBox->currentIndex()); - - QGCToolWidgetItem::uas->setAction(action); - } else { - qDebug() << __FILE__ << __LINE__ << "NO UAS SET, DOING NOTHING"; - } -} - -void QGCActionButton::setActionButtonName(QString text) -{ - ui->actionButton->setText(text); -} - -void QGCActionButton::startEditMode() -{ - ui->editActionComboBox->show(); - ui->editActionsRefreshButton->show(); - ui->editFinishButton->show(); - ui->editNameLabel->show(); - ui->editButtonName->show(); - isInEditMode = true; -} - -void QGCActionButton::endEditMode() -{ - ui->editActionComboBox->hide(); - ui->editActionsRefreshButton->hide(); - ui->editFinishButton->hide(); - ui->editNameLabel->hide(); - ui->editButtonName->hide(); - - // Write to settings - emit editingFinished(); - - isInEditMode = false; -} - -void QGCActionButton::writeSettings(QSettings& settings) -{ - settings.setValue("TYPE", "BUTTON"); - settings.setValue("QGC_ACTION_BUTTON_DESCRIPTION", ui->nameLabel->text()); - settings.setValue("QGC_ACTION_BUTTON_BUTTONTEXT", ui->actionButton->text()); - settings.setValue("QGC_ACTION_BUTTON_ACTIONID", ui->editActionComboBox->currentIndex()); - settings.sync(); -} - -void QGCActionButton::readSettings(const QSettings& settings) -{ - ui->editNameLabel->setText(settings.value("QGC_ACTION_BUTTON_DESCRIPTION", "ERROR LOADING BUTTON").toString()); - ui->editButtonName->setText(settings.value("QGC_ACTION_BUTTON_BUTTONTEXT", "UNKNOWN").toString()); - ui->editActionComboBox->setCurrentIndex(settings.value("QGC_ACTION_BUTTON_ACTIONID", 0).toInt()); - - ui->nameLabel->setText(settings.value("QGC_ACTION_BUTTON_DESCRIPTION", "ERROR LOADING BUTTON").toString()); - ui->actionButton->setText(settings.value("QGC_ACTION_BUTTON_BUTTONTEXT", "UNKNOWN").toString()); - ui->editActionComboBox->setCurrentIndex(settings.value("QGC_ACTION_BUTTON_ACTIONID", 0).toInt()); - qDebug() << "DONE READING SETTINGS"; -} diff --git a/src/ui/designer/QGCActionButton.h b/src/ui/designer/QGCActionButton.h deleted file mode 100644 index a145e9a00be5c3d947eff113c293296a0ccb82ec..0000000000000000000000000000000000000000 --- a/src/ui/designer/QGCActionButton.h +++ /dev/null @@ -1,34 +0,0 @@ -#ifndef QGCACTIONBUTTON_H -#define QGCACTIONBUTTON_H - -#include "QGCToolWidgetItem.h" - -namespace Ui -{ -class QGCActionButton; -} - -class UASInterface; - -class QGCActionButton : public QGCToolWidgetItem -{ - Q_OBJECT - -public: - explicit QGCActionButton(QWidget *parent = 0); - ~QGCActionButton(); - -public slots: - void sendAction(); - void setActionButtonName(QString text); - void startEditMode(); - void endEditMode(); - void writeSettings(QSettings& settings); - void readSettings(const QSettings& settings); - -private: - Ui::QGCActionButton *ui; - UASInterface* uas; -}; - -#endif // QGCACTIONBUTTON_H diff --git a/src/ui/designer/QGCToolWidget.cc b/src/ui/designer/QGCToolWidget.cc index 6a0f7678ca8519bc49c6ece7ee6f84c1c2088ae3..9a3fb8ec26545aac87a753bbb5f2a673209d74e0 100644 --- a/src/ui/designer/QGCToolWidget.cc +++ b/src/ui/designer/QGCToolWidget.cc @@ -11,7 +11,6 @@ #include #include "QGCParamSlider.h" -#include "QGCActionButton.h" #include "QGCCommandButton.h" #include "UASManager.h" @@ -129,10 +128,7 @@ void QGCToolWidget::loadSettings(QSettings& settings) QString type = settings.value("TYPE", "UNKNOWN").toString(); if (type != "UNKNOWN") { QGCToolWidgetItem* item = NULL; - if (type == "BUTTON") { - item = new QGCActionButton(this); - qDebug() << "CREATED BUTTON"; - } else if (type == "COMMANDBUTTON") { + if (type == "COMMANDBUTTON") { item = new QGCCommandButton(this); qDebug() << "CREATED COMMANDBUTTON"; } else if (type == "SLIDER") { @@ -303,17 +299,6 @@ void QGCToolWidget::addParam() slider->startEditMode(); } -void QGCToolWidget::addAction() -{ - QGCActionButton* button = new QGCActionButton(this); - if (ui->hintLabel) { - ui->hintLabel->deleteLater(); - ui->hintLabel = NULL; - } - toolLayout->addWidget(button); - button->startEditMode(); -} - void QGCToolWidget::addCommand() { QGCCommandButton* button = new QGCCommandButton(this); diff --git a/src/ui/designer/QGCToolWidget.h b/src/ui/designer/QGCToolWidget.h index 7d8b74dadc8dad24494ff8d495f81f4706f79270..81c9cf4eb10edaff9bdb5026359aa48892500714 100644 --- a/src/ui/designer/QGCToolWidget.h +++ b/src/ui/designer/QGCToolWidget.h @@ -82,8 +82,6 @@ protected: protected slots: void addParam(); - /** @deprecated */ - void addAction(); void addCommand(); void setTitle(); void setTitle(QString title); 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/.gitignore b/thirdParty/mavlink/.gitignore index d67b9114a8bfd87e59462c81db271ba1cdc24f2f..0da69ed7c3efecc275c1a8123ff3061eee60f487 100644 --- a/thirdParty/mavlink/.gitignore +++ b/thirdParty/mavlink/.gitignore @@ -1,3 +1,4 @@ *~ doc/html doc/*.log +missionlib/testing/*.xcodeproj/* diff --git a/thirdParty/mavlink/include/ardupilotmega/ardupilotmega.h b/thirdParty/mavlink/include/ardupilotmega/ardupilotmega.h index 4d13e0b543c9a021cd9811f4378bb84dfdc47d49..774e3e48dfca526ec1fd9b64a078585272b0bb9d 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 Tuesday, August 9 2011, 16:16 UTC */ #ifndef ARDUPILOTMEGA_H #define ARDUPILOTMEGA_H @@ -35,10 +35,10 @@ extern "C" { -// MESSAGE LENGTHS +// MESSAGE CRC KEYS -#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 } +#undef MAVLINK_MESSAGE_KEYS +#define MAVLINK_MESSAGE_KEYS { 71, 249, 232, 226, 76, 126, 117, 186, 0, 165, 44, 249, 133, 0, 0, 0, 0, 0, 0, 0, 112, 34, 81, 152, 0, 223, 28, 99, 28, 21, 243, 240, 204, 21, 111, 43, 192, 234, 22, 197, 192, 192, 166, 34, 233, 34, 166, 158, 142, 60, 10, 75, 249, 247, 234, 157, 125, 0, 0, 0, 85, 211, 62, 75, 185, 18, 42, 38, 0, 127, 200, 0, 0, 53, 251, 20, 22, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 43, 141, 211, 166 } #ifdef __cplusplus } diff --git a/thirdParty/mavlink/include/ardupilotmega/mavlink.h b/thirdParty/mavlink/include/ardupilotmega/mavlink.h index 8ee2430d74ef6945bd88749e49e4bc4105c40747..933149099f58246694eeb34b4214081f6a7ed829 100644 --- a/thirdParty/mavlink/include/ardupilotmega/mavlink.h +++ b/thirdParty/mavlink/include/ardupilotmega/mavlink.h @@ -1,11 +1,16 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Wednesday, July 27 2011, 14:17 UTC + * Generated on Tuesday, August 9 2011, 16:16 UTC */ #ifndef MAVLINK_H #define MAVLINK_H +#pragma pack(push,1) +#include "mavlink_options.h" #include "ardupilotmega.h" - +#ifdef MAVLINK_DATA +#include "mavlink_data.h" +#endif +#pragma pack(pop) #endif diff --git a/thirdParty/mavlink/include/bittest.c b/thirdParty/mavlink/include/bittest.c deleted file mode 100644 index c966b72afb7676ca72721366f4bcfb9329ffbb4b..0000000000000000000000000000000000000000 --- a/thirdParty/mavlink/include/bittest.c +++ /dev/null @@ -1,39 +0,0 @@ -#include -#include - -int main(int argc, char* argv[]) -{ - - uint8_t bitfield = 254; // 11111110 - - uint8_t mask = 128; // 10000000 - - uint8_t result = (bitfield & mask); - - printf("0x%02x\n", bitfield); - - // Transform into network order - - generic_32bit bin; - bin.i = 1; - printf("First byte in (little endian) 0x%02x\n", bin.b[0]); - generic_32bit bout; - bout.b[0] = bin.b[3]; - bout.b[1] = bin.b[2]; - bout.b[2] = bin.b[1]; - bout.b[3] = bin.b[0]; - printf("Last byte out (big endian) 0x%02x\n", bout.b[3]); - - uint8_t n = 5; - printf("Mask is 0x%02x\n", ((uint32_t)(1 << n)) - 1); // = 2^n - 1 - - int32_t encoded = 2; - uint8_t bits = 2; - - uint8_t packet[MAVLINK_MAX_PACKET_LEN]; - uint8_t packet_index = 0; - uint8_t bit_index = 0; - - put_bitfield_n_by_index(encoded, bits, packet_index, bit_index, &bit_index, packet); - -} diff --git a/thirdParty/mavlink/include/checksum.h b/thirdParty/mavlink/include/checksum.h index c16a06e1b9c6430af8ec637ec04e6d221ee38067..07bab9102ae4947f80f39d56907a7d6a30b96242 100644 --- a/thirdParty/mavlink/include/checksum.h +++ b/thirdParty/mavlink/include/checksum.h @@ -34,6 +34,7 @@ static inline void crc_accumulate(uint8_t data, uint16_t *crcAccum) tmp=data ^ (uint8_t)(*crcAccum &0xff); tmp^= (tmp<<4); *crcAccum = (*crcAccum>>8) ^ (tmp<<8) ^ (tmp <<3) ^ (tmp>>4); +// *crcAccum += data; // super simple to test } /** @@ -86,6 +87,49 @@ static inline uint16_t crc_calculate(uint8_t* pBuffer, int length) } +/** + * @brief Calculates the X.25 checksum on a msg buffer + * + * @param pMSG buffer containing the msg to hash + * @param length number of bytes to hash + * @return the checksum over the buffer bytes + **/ +static inline uint16_t crc_calculate_msg(mavlink_message_t* pMSG, int length) +{ + + // For a "message" of length bytes contained in the unsigned char array + // pointed to by pBuffer, calculate the CRC + // crcCalculate(unsigned char* pBuffer, int length, unsigned short* checkConst) < not needed + + uint16_t crcTmp; + //uint16_t tmp; + uint8_t* pTmp; + int i; + + pTmp=&pMSG->len; + + /* init crcTmp */ + crc_init(&crcTmp); + + for (i = 0; i < 5; i++){ + crc_accumulate(*pTmp++, &crcTmp); + } + + pTmp=&pMSG->payload[0]; + for (; i < length; i++){ + crc_accumulate(*pTmp++, &crcTmp); + } + + /* This is currently not needed, as only the checksum over payload should be computed + tmp = crcTmp; + crcAccumulate((unsigned char)(~crcTmp & 0xff),&tmp); + crcAccumulate((unsigned char)((~crcTmp>>8)&0xff),&tmp); + *checkConst = tmp; + */ + return(crcTmp); +} + + #endif /* _CHECKSUM_H_ */ diff --git a/thirdParty/mavlink/include/common/common.h b/thirdParty/mavlink/include/common/common.h index bf99ef000314ed026055dca19e22cc106f0e59fe..8ce7b21f82bfc1209bd6a655d65f369bbc4a9d25 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 Wednesday, July 27 2011, 14:17 UTC + * Generated on Thursday, August 11 2011, 07:25 UTC */ #ifndef COMMON_H #define COMMON_H @@ -18,75 +18,213 @@ extern "C" { // MAVLINK VERSION #ifndef MAVLINK_VERSION -#define MAVLINK_VERSION 2 +#define MAVLINK_VERSION 3 #endif #if (MAVLINK_VERSION == 0) #undef MAVLINK_VERSION -#define MAVLINK_VERSION 2 +#define MAVLINK_VERSION 3 #endif // ENUM DEFINITIONS -/** @brief 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. */ -enum MAV_CMD +/** @brief Micro air vehicle / autopilot classes. This identifies the individual model. */ +enum MAV_CLASS { - MAV_CMD_NAV_WAYPOINT=16, /* 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 | */ - MAV_CMD_NAV_LOITER_UNLIM=17, /* 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 | */ - MAV_CMD_NAV_LOITER_TURNS=18, /* 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 | */ - MAV_CMD_NAV_LOITER_TIME=19, /* 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 | */ - MAV_CMD_NAV_RETURN_TO_LAUNCH=20, /* Return to launch location | Empty | Empty | Empty | Empty | Empty | Empty | Empty | */ - MAV_CMD_NAV_LAND=21, /* Land at location | Empty | Empty | Empty | Desired yaw angle. | Latitude | Longitude | Altitude | */ - MAV_CMD_NAV_TAKEOFF=22, /* 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 | */ - MAV_CMD_NAV_ROI=80, /* 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 | */ - MAV_CMD_NAV_PATHPLANNING=81, /* 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 | */ - MAV_CMD_NAV_LAST=95, /* 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 | */ - MAV_CMD_CONDITION_DELAY=112, /* Delay mission state machine. | Delay in seconds (decimal) | Empty | Empty | Empty | Empty | Empty | Empty | */ - MAV_CMD_CONDITION_CHANGE_ALT=113, /* Ascend/descend at rate. Delay mission state machine until desired altitude reached. | Descent / Ascend rate (m/s) | Empty | Empty | Empty | Empty | Empty | Finish Altitude | */ - MAV_CMD_CONDITION_DISTANCE=114, /* Delay mission state machine until within desired distance of next NAV point. | Distance (meters) | Empty | Empty | Empty | Empty | Empty | Empty | */ - MAV_CMD_CONDITION_YAW=115, /* 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 | */ - MAV_CMD_CONDITION_LAST=159, /* 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 | */ - MAV_CMD_DO_SET_MODE=176, /* Set system mode. | Mode, as defined by ENUM MAV_MODE | Empty | Empty | Empty | Empty | Empty | Empty | */ - MAV_CMD_DO_JUMP=177, /* 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 | */ - MAV_CMD_DO_CHANGE_SPEED=178, /* 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 | */ - MAV_CMD_DO_SET_HOME=179, /* 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 | */ - MAV_CMD_DO_SET_PARAMETER=180, /* 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 | */ - MAV_CMD_DO_SET_RELAY=181, /* Set a relay to a condition. | Relay number | Setting (1=on, 0=off, others possible depending on system hardware) | Empty | Empty | Empty | Empty | Empty | */ - 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_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 | */ - MAV_CMD_ENUM_END + MAV_CLASS_GENERIC=0, /* Generic autopilot, full support for everything */ + MAV_CLASS_PIXHAWK=1, /* PIXHAWK autopilot, http://pixhawk.ethz.ch */ + MAV_CLASS_SLUGS=2, /* SLUGS autopilot, http://slugsuav.soe.ucsc.edu */ + MAV_CLASS_ARDUPILOTMEGA=3, /* ArduPilotMega / ArduCopter, http://diydrones.com */ + MAV_CLASS_OPENPILOT=4, /* OpenPilot, http://openpilot.org */ + MAV_CLASS_GENERIC_MISSION_WAYPOINTS_ONLY=5, /* Generic autopilot only supporting simple waypoints */ + MAV_CLASS_GENERIC_MISSION_NAVIGATION_ONLY=6, /* Generic autopilot supporting waypoints and other simple navigation commands */ + MAV_CLASS_GENERIC_MISSION_FULL=7, /* Generic autopilot supporting the full mission command set */ + MAV_CLASS_INVALID=8, /* No valid autopilot, e.g. a GCS or other MAVLink component */ + MAV_CLASS_PPZ=9, /* PPZ UAV - http://nongnu.org/paparazzi */ + MAV_CLASS_UDB=10, /* UAV Dev Board */ + MAV_CLASS_FP=11, /* FlexiPilot */ + MAV_CLASS_ENUM_END +}; + +/** @brief */ +enum MAV_MODE +{ + MAV_MODE_UNINIT=0, /* System is in undefined state */ + MAV_MODE_LOCKED=1, /* Motors are blocked, system is safe */ + MAV_MODE_MANUAL=2, /* System is allowed to be active, under manual (RC) control */ + MAV_MODE_GUIDED=3, /* System is allowed to be active, under autonomous control, manual setpoint */ + MAV_MODE_AUTO=4, /* System is allowed to be active, under autonomous control and navigation */ + MAV_MODE_TEST1=5, /* Generic test mode, for custom use */ + MAV_MODE_TEST2=6, /* Generic test mode, for custom use */ + MAV_MODE_TEST3=7, /* Generic test mode, for custom use */ + MAV_MODE_READY=8, /* System is ready, motors are unblocked, but controllers are inactive */ + MAV_MODE_RC_TRAINING=9, /* System is blocked, only RC valued are read and reported back */ + MAV_MODE_ENUM_END +}; + +/** @brief */ +enum MAV_NAV +{ + MAV_NAV_GROUNDED=0, /* System is currently on ground. */ + MAV_NAV_LIFTOFF=1, /* System is during liftoff, not in normal navigation mode yet. */ + MAV_NAV_HOLD=2, /* System is holding its current position (rotorcraft or rover / boat). */ + MAV_NAV_WAYPOINT=3, /* System is navigating towards the next waypoint. */ + MAV_NAV_VECTOR=4, /* System is flying at a defined course and speed. */ + MAV_NAV_RETURNING=5, /* System is return straight to home position. */ + MAV_NAV_LANDING=6, /* System is landing. */ + MAV_NAV_LOST=7, /* System lost its position input and is in attitude / flight stabilization only. */ + MAV_NAV_LOITER=8, /* System is loitering in wait position. DO NOT USE THIS FOR WAYPOINT LOITER! */ + MAV_NAV_ENUM_END +}; + +/** @brief */ +enum MAV_STATE +{ + MAV_STATE_UNINIT=0, /* Uninitialized system, state is unknown. */ + MAV_STATE_BOOT=1, /* System is booting up. */ + MAV_STATE_CALIBRATING=2, /* System is calibrating and not flight-ready. */ + MAV_STATE_STANDBY=3, /* System is grounded and on standby. It can be launched any time. */ + MAV_STATE_ACTIVE=4, /* System is active and might be already airborne. Motors are engaged. */ + MAV_STATE_CRITICAL=5, /* System is in a non-normal flight mode. It can however still navigate. */ + MAV_STATE_EMERGENCY=6, /* System is in a non-normal flight mode. It lost control over parts or over the whole airframe. It is in mayday and going down. */ + MAV_STATE_POWEROFF=7, /* System just initialized its power-down sequence, will shut down now. */ + MAV_STATE_ENUM_END }; -/** @brief 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. */ +/** @brief 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. */ enum MAV_DATA_STREAM { - MAV_DATA_STREAM_ALL=0, /* Enable all data streams | */ - MAV_DATA_STREAM_RAW_SENSORS=1, /* Enable IMU_RAW, GPS_RAW, GPS_STATUS packets. | */ - MAV_DATA_STREAM_EXTENDED_STATUS=2, /* Enable GPS_STATUS, CONTROL_STATUS, AUX_STATUS | */ - MAV_DATA_STREAM_RC_CHANNELS=3, /* Enable RC_CHANNELS_SCALED, RC_CHANNELS_RAW, SERVO_OUTPUT_RAW | */ - MAV_DATA_STREAM_RAW_CONTROLLER=4, /* Enable ATTITUDE_CONTROLLER_OUTPUT, POSITION_CONTROLLER_OUTPUT, NAV_CONTROLLER_OUTPUT. | */ - MAV_DATA_STREAM_POSITION=6, /* Enable LOCAL_POSITION, GLOBAL_POSITION/GLOBAL_POSITION_INT messages. | */ - MAV_DATA_STREAM_EXTRA1=10, /* Dependent on the autopilot | */ - MAV_DATA_STREAM_EXTRA2=11, /* Dependent on the autopilot | */ - MAV_DATA_STREAM_EXTRA3=12, /* Dependent on the autopilot | */ + MAV_DATA_STREAM_ALL=0, /* Enable all data streams */ + MAV_DATA_STREAM_RAW_SENSORS=1, /* Enable IMU_RAW, GPS_RAW, GPS_STATUS packets. */ + MAV_DATA_STREAM_EXTENDED_STATUS=2, /* Enable GPS_STATUS, CONTROL_STATUS, AUX_STATUS */ + MAV_DATA_STREAM_RC_CHANNELS=3, /* Enable RC_CHANNELS_SCALED, RC_CHANNELS_RAW, SERVO_OUTPUT_RAW */ + MAV_DATA_STREAM_RAW_CONTROLLER=4, /* Enable ATTITUDE_CONTROLLER_OUTPUT, POSITION_CONTROLLER_OUTPUT, NAV_CONTROLLER_OUTPUT. */ + MAV_DATA_STREAM_POSITION=6, /* Enable LOCAL_POSITION, GLOBAL_POSITION/GLOBAL_POSITION_INT messages. */ + MAV_DATA_STREAM_EXTRA1=10, /* Dependent on the autopilot */ + MAV_DATA_STREAM_EXTRA2=11, /* Dependent on the autopilot */ + MAV_DATA_STREAM_EXTRA3=12, /* Dependent on the autopilot */ 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_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_ENUM_END }; +/** @brief */ +enum MAV_TYPE +{ + MAV_TYPE_GENERIC=0, /* Generic micro air vehicle. */ + MAV_TYPE_FIXED_WING=1, /* Fixed wing aircraft. */ + MAV_TYPE_QUADROTOR=2, /* Quadrotor */ + MAV_TYPE_COAXIAL=3, /* Coaxial helicopter */ + MAV_TYPE_HELICOPTER=4, /* Normal helicopter with tail rotor. */ + MAV_TYPE_GROUND=5, /* Ground installation */ + MAV_TYPE_OCU=6, /* Operator control unit / ground control station */ + MAV_TYPE_AIRSHIP=7, /* Airship, controlled */ + MAV_TYPE_FREE_BALLOON=8, /* Free balloon, uncontrolled */ + MAV_TYPE_ROCKET=9, /* Rocket */ + MAV_TYPE_UGV_GROUND_ROVER=10, /* Ground rover */ + MAV_TYPE_UGV_SURFACE_SHIP=11, /* Surface vessel, boat, ship */ + MAV_TYPE_UGV_SUBMARINE=12, /* Submarine */ + MAV_TYPE_HEXAROTOR=13, /* Hexarotor */ + MAV_TYPE_OCTOROTOR=14, /* Octorotor */ + MAV_TYPE_ENUM_END +}; + +/** @brief */ +enum MAV_COMPONENT +{ + MAV_COMP_ID_GPS=220, + MAV_COMP_ID_WAYPOINTPLANNER=190, + MAV_COMP_ID_PATHPLANNER=195, + MAV_COMP_ID_MAPPER=180, + MAV_COMP_ID_CAMERA=100, + MAV_COMP_ID_IMU=200, + MAV_COMP_ID_IMU_2=201, + MAV_COMP_ID_IMU_3=202, + MAV_COMP_ID_UDP_BRIDGE=240, + MAV_COMP_ID_UART_BRIDGE=241, + MAV_COMP_ID_SYSTEM_CONTROL=250, + MAV_COMP_ID_SERVO1=140, + MAV_COMP_ID_SERVO2=141, + MAV_COMP_ID_SERVO3=142, + MAV_COMP_ID_SERVO4=143, + MAV_COMP_ID_SERVO5=144, + MAV_COMP_ID_SERVO6=145, + MAV_COMP_ID_SERVO7=146, + MAV_COMP_ID_SERVO8=147, + MAV_COMP_ID_SERVO9=148, + MAV_COMP_ID_SERVO10=149, + MAV_COMP_ID_SERVO11=150, + MAV_COMP_ID_SERVO12=151, + MAV_COMP_ID_SERVO13=152, + MAV_COMP_ID_SERVO14=153, + MAV_COMPONENT_ENUM_END +}; + +/** @brief */ +enum MAV_FRAME +{ + MAV_FRAME_GLOBAL=0, /* Global coordinate frame, WGS84 coordinate system. */ + MAV_FRAME_LOCAL_NED=1, /* Local coordinate frame, Z-up (x: north, y: east, z: down). */ + MAV_FRAME_MISSION=2, /* NOT a coordinate frame, indicates a mission command. */ + MAV_FRAME_GLOBAL_RELATIVE_ALT=3, /* Global coordinate frame, WGS84 coordinate system, relative altitude over ground. */ + MAV_FRAME_LOCAL_ENU=4, /* Local coordinate frame, Z-down (x: east, y: north, z: up) */ + MAV_FRAME_ENUM_END +}; + +/** @brief */ +enum MAVLINK_DATA_STREAM_TYPE +{ + MAVLINK_DATA_STREAM_IMG_JPEG=0, + MAVLINK_DATA_STREAM_IMG_BMP=1, + MAVLINK_DATA_STREAM_IMG_RAW8U=2, + MAVLINK_DATA_STREAM_IMG_RAW32U=3, + MAVLINK_DATA_STREAM_IMG_PGM=4, + MAVLINK_DATA_STREAM_IMG_PNG=5, + MAVLINK_DATA_STREAM_TYPE_ENUM_END +}; + +/** @brief 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. */ +enum MAV_CMD +{ + MAV_CMD_NAV_WAYPOINT=16, /* 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 */ + MAV_CMD_NAV_LOITER_UNLIM=17, /* 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 */ + MAV_CMD_NAV_LOITER_TURNS=18, /* 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 */ + MAV_CMD_NAV_LOITER_TIME=19, /* 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 */ + MAV_CMD_NAV_RETURN_TO_LAUNCH=20, /* Return to launch location | Empty | Empty | Empty | Empty | Empty | Empty | Empty */ + MAV_CMD_NAV_LAND=21, /* Land at location | Empty | Empty | Empty | Desired yaw angle. | Latitude | Longitude | Altitude */ + MAV_CMD_NAV_TAKEOFF=22, /* 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 */ + MAV_CMD_NAV_ROI=80, /* 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 */ + MAV_CMD_NAV_PATHPLANNING=81, /* 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 */ + MAV_CMD_NAV_LAST=95, /* 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 */ + MAV_CMD_CONDITION_DELAY=112, /* Delay mission state machine. | Delay in seconds (decimal) | Empty | Empty | Empty | Empty | Empty | Empty */ + MAV_CMD_CONDITION_CHANGE_ALT=113, /* Ascend/descend at rate. Delay mission state machine until desired altitude reached. | Descent / Ascend rate (m/s) | Empty | Empty | Empty | Empty | Empty | Finish Altitude */ + MAV_CMD_CONDITION_DISTANCE=114, /* Delay mission state machine until within desired distance of next NAV point. | Distance (meters) | Empty | Empty | Empty | Empty | Empty | Empty */ + MAV_CMD_CONDITION_YAW=115, /* 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 */ + MAV_CMD_CONDITION_LAST=159, /* 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 */ + MAV_CMD_DO_SET_MODE=176, /* Set system mode. | Mode, as defined by ENUM MAV_MODE | Empty | Empty | Empty | Empty | Empty | Empty */ + MAV_CMD_DO_JUMP=177, /* 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 */ + MAV_CMD_DO_CHANGE_SPEED=178, /* 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 */ + MAV_CMD_DO_SET_HOME=179, /* 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 */ + MAV_CMD_DO_SET_PARAMETER=180, /* 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 */ + MAV_CMD_DO_SET_RELAY=181, /* Set a relay to a condition. | Relay number | Setting (1=on, 0=off, others possible depending on system hardware) | Empty | Empty | Empty | Empty | Empty */ + 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_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 */ + MAV_CMD_ENUM_END +}; + // MESSAGE DEFINITIONS @@ -98,8 +236,7 @@ enum MAV_ROI #include "./mavlink_msg_change_operator_control.h" #include "./mavlink_msg_change_operator_control_ack.h" #include "./mavlink_msg_auth_key.h" -#include "./mavlink_msg_action_ack.h" -#include "./mavlink_msg_action.h" +#include "./mavlink_msg_cmd_ack.h" #include "./mavlink_msg_set_mode.h" #include "./mavlink_msg_set_nav_mode.h" #include "./mavlink_msg_param_request_read.h" @@ -136,6 +273,10 @@ 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_set_roll_pitch_yaw.h" +#include "./mavlink_msg_set_roll_pitch_yaw_speed.h" +#include "./mavlink_msg_roll_pitch_yaw_setpoint.h" +#include "./mavlink_msg_roll_pitch_yaw_speed_setpoint.h" #include "./mavlink_msg_attitude_controller_output.h" #include "./mavlink_msg_position_controller_output.h" #include "./mavlink_msg_nav_controller_output.h" @@ -143,11 +284,14 @@ enum MAV_ROI #include "./mavlink_msg_state_correction.h" #include "./mavlink_msg_set_altitude.h" #include "./mavlink_msg_request_data_stream.h" +#include "./mavlink_msg_full_state.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_memory_vect.h" #include "./mavlink_msg_debug_vect.h" #include "./mavlink_msg_named_value_float.h" #include "./mavlink_msg_named_value_int.h" @@ -155,10 +299,10 @@ enum MAV_ROI #include "./mavlink_msg_debug.h" -// MESSAGE LENGTHS +// MESSAGE CRC KEYS -#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 } +#undef MAVLINK_MESSAGE_KEYS +#define MAVLINK_MESSAGE_KEYS { 71, 249, 232, 226, 76, 126, 117, 186, 0, 144, 44, 249, 133, 0, 0, 0, 0, 0, 0, 0, 33, 34, 163, 45, 0, 166, 28, 99, 28, 21, 243, 240, 91, 21, 111, 43, 192, 234, 22, 197, 192, 192, 166, 34, 233, 34, 166, 158, 142, 60, 10, 75, 249, 247, 234, 157, 125, 79, 252, 0, 85, 211, 62, 75, 185, 18, 42, 38, 0, 127, 200, 0, 0, 212, 251, 20, 22, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 138, 43, 141, 211, 144 } #ifdef __cplusplus } diff --git a/thirdParty/mavlink/include/common/mavlink.h b/thirdParty/mavlink/include/common/mavlink.h index 404b970bffead60f0b273e47333bd8f438f488db..7de59d0c68481b9e0cb1a6d39b4079506ae4852e 100644 --- a/thirdParty/mavlink/include/common/mavlink.h +++ b/thirdParty/mavlink/include/common/mavlink.h @@ -1,11 +1,16 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Wednesday, July 27 2011, 14:17 UTC + * Generated on Thursday, August 11 2011, 07:25 UTC */ #ifndef MAVLINK_H #define MAVLINK_H +#pragma pack(push,1) +#include "mavlink_options.h" #include "common.h" - +#ifdef MAVLINK_DATA +#include "mavlink_data.h" +#endif +#pragma pack(pop) #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_action.h b/thirdParty/mavlink/include/common/mavlink_msg_action.h deleted file mode 100644 index f1de5467547fff8b3c40f00f9da082ed6f54477a..0000000000000000000000000000000000000000 --- a/thirdParty/mavlink/include/common/mavlink_msg_action.h +++ /dev/null @@ -1,135 +0,0 @@ -// MESSAGE ACTION PACKING - -#define MAVLINK_MSG_ID_ACTION 10 - -typedef struct __mavlink_action_t -{ - uint8_t target; ///< The system executing the action - uint8_t target_component; ///< The component executing the action - uint8_t action; ///< The action id - -} mavlink_action_t; - - - -/** - * @brief Pack a action 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 The system executing the action - * @param target_component The component executing the action - * @param action The action id - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_action_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target, uint8_t target_component, uint8_t action) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_ACTION; - - i += put_uint8_t_by_index(target, i, msg->payload); // The system executing the action - i += put_uint8_t_by_index(target_component, i, msg->payload); // The component executing the action - i += put_uint8_t_by_index(action, i, msg->payload); // The action id - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -/** - * @brief Pack a action 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 The system executing the action - * @param target_component The component executing the action - * @param action The action id - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_action_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target, uint8_t target_component, uint8_t action) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_ACTION; - - i += put_uint8_t_by_index(target, i, msg->payload); // The system executing the action - i += put_uint8_t_by_index(target_component, i, msg->payload); // The component executing the action - i += put_uint8_t_by_index(action, i, msg->payload); // The action id - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); -} - -/** - * @brief Encode a action 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 action C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_action_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_action_t* action) -{ - return mavlink_msg_action_pack(system_id, component_id, msg, action->target, action->target_component, action->action); -} - -/** - * @brief Send a action message - * @param chan MAVLink channel to send the message - * - * @param target The system executing the action - * @param target_component The component executing the action - * @param action The action id - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_action_send(mavlink_channel_t chan, uint8_t target, uint8_t target_component, uint8_t action) -{ - mavlink_message_t msg; - mavlink_msg_action_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target, target_component, action); - mavlink_send_uart(chan, &msg); -} - -#endif -// MESSAGE ACTION UNPACKING - -/** - * @brief Get field target from action message - * - * @return The system executing the action - */ -static inline uint8_t mavlink_msg_action_get_target(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload)[0]; -} - -/** - * @brief Get field target_component from action message - * - * @return The component executing the action - */ -static inline uint8_t mavlink_msg_action_get_target_component(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; -} - -/** - * @brief Get field action from action message - * - * @return The action id - */ -static inline uint8_t mavlink_msg_action_get_action(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; -} - -/** - * @brief Decode a action message into a struct - * - * @param msg The message to decode - * @param action C-struct to decode the message contents into - */ -static inline void mavlink_msg_action_decode(const mavlink_message_t* msg, mavlink_action_t* action) -{ - action->target = mavlink_msg_action_get_target(msg); - action->target_component = mavlink_msg_action_get_target_component(msg); - action->action = mavlink_msg_action_get_action(msg); -} diff --git a/thirdParty/mavlink/include/common/mavlink_msg_action_ack.h b/thirdParty/mavlink/include/common/mavlink_msg_action_ack.h deleted file mode 100644 index 6e93449e205003bd383a123cb938e7ad645fd031..0000000000000000000000000000000000000000 --- a/thirdParty/mavlink/include/common/mavlink_msg_action_ack.h +++ /dev/null @@ -1,118 +0,0 @@ -// MESSAGE ACTION_ACK PACKING - -#define MAVLINK_MSG_ID_ACTION_ACK 9 - -typedef struct __mavlink_action_ack_t -{ - uint8_t action; ///< The action id - uint8_t result; ///< 0: Action DENIED, 1: Action executed - -} mavlink_action_ack_t; - - - -/** - * @brief Pack a action_ack 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 action The action id - * @param result 0: Action DENIED, 1: Action executed - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_action_ack_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t action, uint8_t result) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_ACTION_ACK; - - i += put_uint8_t_by_index(action, i, msg->payload); // The action id - i += put_uint8_t_by_index(result, i, msg->payload); // 0: Action DENIED, 1: Action executed - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -/** - * @brief Pack a action_ack 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 action The action id - * @param result 0: Action DENIED, 1: Action executed - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_action_ack_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t action, uint8_t result) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_ACTION_ACK; - - i += put_uint8_t_by_index(action, i, msg->payload); // The action id - i += put_uint8_t_by_index(result, i, msg->payload); // 0: Action DENIED, 1: Action executed - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); -} - -/** - * @brief Encode a action_ack 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 action_ack C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_action_ack_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_action_ack_t* action_ack) -{ - return mavlink_msg_action_ack_pack(system_id, component_id, msg, action_ack->action, action_ack->result); -} - -/** - * @brief Send a action_ack message - * @param chan MAVLink channel to send the message - * - * @param action The action id - * @param result 0: Action DENIED, 1: Action executed - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_action_ack_send(mavlink_channel_t chan, uint8_t action, uint8_t result) -{ - mavlink_message_t msg; - mavlink_msg_action_ack_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, action, result); - mavlink_send_uart(chan, &msg); -} - -#endif -// MESSAGE ACTION_ACK UNPACKING - -/** - * @brief Get field action from action_ack message - * - * @return The action id - */ -static inline uint8_t mavlink_msg_action_ack_get_action(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload)[0]; -} - -/** - * @brief Get field result from action_ack message - * - * @return 0: Action DENIED, 1: Action executed - */ -static inline uint8_t mavlink_msg_action_ack_get_result(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; -} - -/** - * @brief Decode a action_ack message into a struct - * - * @param msg The message to decode - * @param action_ack C-struct to decode the message contents into - */ -static inline void mavlink_msg_action_ack_decode(const mavlink_message_t* msg, mavlink_action_ack_t* action_ack) -{ - action_ack->action = mavlink_msg_action_ack_get_action(msg); - action_ack->result = mavlink_msg_action_ack_get_result(msg); -} diff --git a/thirdParty/mavlink/include/common/mavlink_msg_attitude.h b/thirdParty/mavlink/include/common/mavlink_msg_attitude.h index 6daba3c54b2a8806210871091cfdff74f55808ee..8c32cd48c3760c85754b1d1cf9db14a363591183 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_attitude.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_attitude.h @@ -1,6 +1,8 @@ // MESSAGE ATTITUDE PACKING #define MAVLINK_MSG_ID_ATTITUDE 30 +#define MAVLINK_MSG_ID_ATTITUDE_LEN 32 +#define MAVLINK_MSG_30_LEN 32 typedef struct __mavlink_attitude_t { @@ -14,8 +16,6 @@ typedef struct __mavlink_attitude_t } mavlink_attitude_t; - - /** * @brief Pack a attitude message * @param system_id ID of this system @@ -33,18 +33,18 @@ typedef struct __mavlink_attitude_t */ static inline uint16_t mavlink_msg_attitude_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) { - uint16_t i = 0; + mavlink_attitude_t *p = (mavlink_attitude_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_ATTITUDE; - 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) + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->roll = roll; // float:Roll angle (rad) + p->pitch = pitch; // float:Pitch angle (rad) + p->yaw = yaw; // float:Yaw angle (rad) + p->rollspeed = rollspeed; // float:Roll angular speed (rad/s) + p->pitchspeed = pitchspeed; // float:Pitch angular speed (rad/s) + p->yawspeed = yawspeed; // float:Yaw angular speed (rad/s) - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ATTITUDE_LEN); } /** @@ -64,18 +64,18 @@ static inline uint16_t mavlink_msg_attitude_pack(uint8_t system_id, uint8_t comp */ static inline uint16_t mavlink_msg_attitude_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) { - uint16_t i = 0; + mavlink_attitude_t *p = (mavlink_attitude_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_ATTITUDE; - 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) + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->roll = roll; // float:Roll angle (rad) + p->pitch = pitch; // float:Pitch angle (rad) + p->yaw = yaw; // float:Yaw angle (rad) + p->rollspeed = rollspeed; // float:Roll angular speed (rad/s) + p->pitchspeed = pitchspeed; // float:Pitch angular speed (rad/s) + p->yawspeed = yawspeed; // float:Yaw angular speed (rad/s) - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ATTITUDE_LEN); } /** @@ -103,13 +103,41 @@ static inline uint16_t mavlink_msg_attitude_encode(uint8_t system_id, uint8_t co * @param pitchspeed Pitch angular speed (rad/s) * @param yawspeed Yaw angular speed (rad/s) */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_attitude_send(mavlink_channel_t chan, uint64_t usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed) { - mavlink_message_t msg; - mavlink_msg_attitude_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, usec, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_attitude_t payload; + uint16_t checksum; + mavlink_attitude_t *p = &payload; + + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->roll = roll; // float:Roll angle (rad) + p->pitch = pitch; // float:Pitch angle (rad) + p->yaw = yaw; // float:Yaw angle (rad) + p->rollspeed = rollspeed; // float:Roll angular speed (rad/s) + p->pitchspeed = pitchspeed; // float:Pitch angular speed (rad/s) + p->yawspeed = yawspeed; // float:Yaw angular speed (rad/s) + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_ATTITUDE_LEN; + hdr.msgid = MAVLINK_MSG_ID_ATTITUDE; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -122,16 +150,8 @@ static inline void mavlink_msg_attitude_send(mavlink_channel_t chan, uint64_t us */ static inline uint64_t mavlink_msg_attitude_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; + mavlink_attitude_t *p = (mavlink_attitude_t *)&msg->payload[0]; + return (uint64_t)(p->usec); } /** @@ -141,12 +161,8 @@ static inline uint64_t mavlink_msg_attitude_get_usec(const mavlink_message_t* ms */ static inline float mavlink_msg_attitude_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; + mavlink_attitude_t *p = (mavlink_attitude_t *)&msg->payload[0]; + return (float)(p->roll); } /** @@ -156,12 +172,8 @@ static inline float mavlink_msg_attitude_get_roll(const mavlink_message_t* msg) */ static inline float mavlink_msg_attitude_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; + mavlink_attitude_t *p = (mavlink_attitude_t *)&msg->payload[0]; + return (float)(p->pitch); } /** @@ -171,12 +183,8 @@ static inline float mavlink_msg_attitude_get_pitch(const mavlink_message_t* msg) */ static inline float mavlink_msg_attitude_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; + mavlink_attitude_t *p = (mavlink_attitude_t *)&msg->payload[0]; + return (float)(p->yaw); } /** @@ -186,12 +194,8 @@ static inline float mavlink_msg_attitude_get_yaw(const mavlink_message_t* msg) */ static inline float mavlink_msg_attitude_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; + mavlink_attitude_t *p = (mavlink_attitude_t *)&msg->payload[0]; + return (float)(p->rollspeed); } /** @@ -201,12 +205,8 @@ static inline float mavlink_msg_attitude_get_rollspeed(const mavlink_message_t* */ static inline float mavlink_msg_attitude_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; + mavlink_attitude_t *p = (mavlink_attitude_t *)&msg->payload[0]; + return (float)(p->pitchspeed); } /** @@ -216,12 +216,8 @@ static inline float mavlink_msg_attitude_get_pitchspeed(const mavlink_message_t* */ static inline float mavlink_msg_attitude_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; + mavlink_attitude_t *p = (mavlink_attitude_t *)&msg->payload[0]; + return (float)(p->yawspeed); } /** @@ -232,11 +228,5 @@ static inline float mavlink_msg_attitude_get_yawspeed(const mavlink_message_t* m */ static inline void mavlink_msg_attitude_decode(const mavlink_message_t* msg, mavlink_attitude_t* attitude) { - attitude->usec = mavlink_msg_attitude_get_usec(msg); - attitude->roll = mavlink_msg_attitude_get_roll(msg); - attitude->pitch = mavlink_msg_attitude_get_pitch(msg); - attitude->yaw = mavlink_msg_attitude_get_yaw(msg); - attitude->rollspeed = mavlink_msg_attitude_get_rollspeed(msg); - attitude->pitchspeed = mavlink_msg_attitude_get_pitchspeed(msg); - attitude->yawspeed = mavlink_msg_attitude_get_yawspeed(msg); + memcpy( attitude, msg->payload, sizeof(mavlink_attitude_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_attitude_controller_output.h b/thirdParty/mavlink/include/common/mavlink_msg_attitude_controller_output.h index 60fa775540642cfd8eb876fd33163edb75ab4946..80d73978e61009479a438feb79b58f6d0e35ebb1 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_attitude_controller_output.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_attitude_controller_output.h @@ -1,6 +1,8 @@ // MESSAGE ATTITUDE_CONTROLLER_OUTPUT PACKING #define MAVLINK_MSG_ID_ATTITUDE_CONTROLLER_OUTPUT 60 +#define MAVLINK_MSG_ID_ATTITUDE_CONTROLLER_OUTPUT_LEN 5 +#define MAVLINK_MSG_60_LEN 5 typedef struct __mavlink_attitude_controller_output_t { @@ -12,8 +14,6 @@ typedef struct __mavlink_attitude_controller_output_t } mavlink_attitude_controller_output_t; - - /** * @brief Pack a attitude_controller_output message * @param system_id ID of this system @@ -29,16 +29,16 @@ typedef struct __mavlink_attitude_controller_output_t */ 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; + mavlink_attitude_controller_output_t *p = (mavlink_attitude_controller_output_t *)&msg->payload[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% + p->enabled = enabled; // uint8_t:1: enabled, 0: disabled + p->roll = roll; // int8_t:Attitude roll: -128: -100%, 127: +100% + p->pitch = pitch; // int8_t:Attitude pitch: -128: -100%, 127: +100% + p->yaw = yaw; // int8_t:Attitude yaw: -128: -100%, 127: +100% + p->thrust = thrust; // int8_t:Attitude thrust: -128: -100%, 127: +100% - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ATTITUDE_CONTROLLER_OUTPUT_LEN); } /** @@ -56,16 +56,16 @@ static inline uint16_t mavlink_msg_attitude_controller_output_pack(uint8_t syste */ 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; + mavlink_attitude_controller_output_t *p = (mavlink_attitude_controller_output_t *)&msg->payload[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% + p->enabled = enabled; // uint8_t:1: enabled, 0: disabled + p->roll = roll; // int8_t:Attitude roll: -128: -100%, 127: +100% + p->pitch = pitch; // int8_t:Attitude pitch: -128: -100%, 127: +100% + p->yaw = yaw; // int8_t:Attitude yaw: -128: -100%, 127: +100% + p->thrust = thrust; // int8_t:Attitude thrust: -128: -100%, 127: +100% - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ATTITUDE_CONTROLLER_OUTPUT_LEN); } /** @@ -91,13 +91,39 @@ static inline uint16_t mavlink_msg_attitude_controller_output_encode(uint8_t sys * @param yaw Attitude yaw: -128: -100%, 127: +100% * @param thrust Attitude thrust: -128: -100%, 127: +100% */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#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); + mavlink_header_t hdr; + mavlink_attitude_controller_output_t payload; + uint16_t checksum; + mavlink_attitude_controller_output_t *p = &payload; + + p->enabled = enabled; // uint8_t:1: enabled, 0: disabled + p->roll = roll; // int8_t:Attitude roll: -128: -100%, 127: +100% + p->pitch = pitch; // int8_t:Attitude pitch: -128: -100%, 127: +100% + p->yaw = yaw; // int8_t:Attitude yaw: -128: -100%, 127: +100% + p->thrust = thrust; // int8_t:Attitude thrust: -128: -100%, 127: +100% + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_ATTITUDE_CONTROLLER_OUTPUT_LEN; + hdr.msgid = MAVLINK_MSG_ID_ATTITUDE_CONTROLLER_OUTPUT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -110,7 +136,8 @@ static inline void mavlink_msg_attitude_controller_output_send(mavlink_channel_t */ static inline uint8_t mavlink_msg_attitude_controller_output_get_enabled(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_attitude_controller_output_t *p = (mavlink_attitude_controller_output_t *)&msg->payload[0]; + return (uint8_t)(p->enabled); } /** @@ -120,7 +147,8 @@ static inline uint8_t mavlink_msg_attitude_controller_output_get_enabled(const m */ 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]; + mavlink_attitude_controller_output_t *p = (mavlink_attitude_controller_output_t *)&msg->payload[0]; + return (int8_t)(p->roll); } /** @@ -130,7 +158,8 @@ static inline int8_t mavlink_msg_attitude_controller_output_get_roll(const mavli */ 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]; + mavlink_attitude_controller_output_t *p = (mavlink_attitude_controller_output_t *)&msg->payload[0]; + return (int8_t)(p->pitch); } /** @@ -140,7 +169,8 @@ static inline int8_t mavlink_msg_attitude_controller_output_get_pitch(const mavl */ 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]; + mavlink_attitude_controller_output_t *p = (mavlink_attitude_controller_output_t *)&msg->payload[0]; + return (int8_t)(p->yaw); } /** @@ -150,7 +180,8 @@ static inline int8_t mavlink_msg_attitude_controller_output_get_yaw(const mavlin */ 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]; + mavlink_attitude_controller_output_t *p = (mavlink_attitude_controller_output_t *)&msg->payload[0]; + return (int8_t)(p->thrust); } /** @@ -161,9 +192,5 @@ static inline int8_t mavlink_msg_attitude_controller_output_get_thrust(const mav */ 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); + memcpy( attitude_controller_output, msg->payload, sizeof(mavlink_attitude_controller_output_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_auth_key.h b/thirdParty/mavlink/include/common/mavlink_msg_auth_key.h index 605ba7db61bbd6ef3a93d1b338edd1121789eb45..36366e2dcbc1c891f639daa59ef725d03ace4562 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_auth_key.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_auth_key.h @@ -1,16 +1,16 @@ // MESSAGE AUTH_KEY PACKING #define MAVLINK_MSG_ID_AUTH_KEY 7 +#define MAVLINK_MSG_ID_AUTH_KEY_LEN 32 +#define MAVLINK_MSG_7_LEN 32 typedef struct __mavlink_auth_key_t { char key[32]; ///< key } mavlink_auth_key_t; - #define MAVLINK_MSG_AUTH_KEY_FIELD_KEY_LEN 32 - /** * @brief Pack a auth_key message * @param system_id ID of this system @@ -22,12 +22,12 @@ typedef struct __mavlink_auth_key_t */ static inline uint16_t mavlink_msg_auth_key_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const char* key) { - uint16_t i = 0; + mavlink_auth_key_t *p = (mavlink_auth_key_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_AUTH_KEY; - i += put_array_by_index((const int8_t*)key, sizeof(char)*32, i, msg->payload); // key + memcpy(p->key, key, sizeof(p->key)); // char[32]:key - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AUTH_KEY_LEN); } /** @@ -41,12 +41,12 @@ static inline uint16_t mavlink_msg_auth_key_pack(uint8_t system_id, uint8_t comp */ static inline uint16_t mavlink_msg_auth_key_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const char* key) { - uint16_t i = 0; + mavlink_auth_key_t *p = (mavlink_auth_key_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_AUTH_KEY; - i += put_array_by_index((const int8_t*)key, sizeof(char)*32, i, msg->payload); // key + memcpy(p->key, key, sizeof(p->key)); // char[32]:key - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AUTH_KEY_LEN); } /** @@ -68,13 +68,35 @@ static inline uint16_t mavlink_msg_auth_key_encode(uint8_t system_id, uint8_t co * * @param key key */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_auth_key_send(mavlink_channel_t chan, const char* key) { - mavlink_message_t msg; - mavlink_msg_auth_key_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, key); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_auth_key_t payload; + uint16_t checksum; + mavlink_auth_key_t *p = &payload; + + memcpy(p->key, key, sizeof(p->key)); // char[32]:key + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_AUTH_KEY_LEN; + hdr.msgid = MAVLINK_MSG_ID_AUTH_KEY; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -85,11 +107,12 @@ static inline void mavlink_msg_auth_key_send(mavlink_channel_t chan, const char* * * @return key */ -static inline uint16_t mavlink_msg_auth_key_get_key(const mavlink_message_t* msg, char* r_data) +static inline uint16_t mavlink_msg_auth_key_get_key(const mavlink_message_t* msg, char* key) { + mavlink_auth_key_t *p = (mavlink_auth_key_t *)&msg->payload[0]; - memcpy(r_data, msg->payload, sizeof(char)*32); - return sizeof(char)*32; + memcpy(key, p->key, sizeof(p->key)); + return sizeof(p->key); } /** @@ -100,5 +123,5 @@ static inline uint16_t mavlink_msg_auth_key_get_key(const mavlink_message_t* msg */ static inline void mavlink_msg_auth_key_decode(const mavlink_message_t* msg, mavlink_auth_key_t* auth_key) { - mavlink_msg_auth_key_get_key(msg, auth_key->key); + memcpy( auth_key, msg->payload, sizeof(mavlink_auth_key_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_boot.h b/thirdParty/mavlink/include/common/mavlink_msg_boot.h index 69d8ed64a7d91095e4637d5153dc964857ec9151..9051bb199abe4e1e6a93605bde69908405c16b4e 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_boot.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_boot.h @@ -1,6 +1,8 @@ // MESSAGE BOOT PACKING #define MAVLINK_MSG_ID_BOOT 1 +#define MAVLINK_MSG_ID_BOOT_LEN 4 +#define MAVLINK_MSG_1_LEN 4 typedef struct __mavlink_boot_t { @@ -8,8 +10,6 @@ typedef struct __mavlink_boot_t } mavlink_boot_t; - - /** * @brief Pack a boot message * @param system_id ID of this system @@ -21,12 +21,12 @@ typedef struct __mavlink_boot_t */ static inline uint16_t mavlink_msg_boot_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint32_t version) { - uint16_t i = 0; + mavlink_boot_t *p = (mavlink_boot_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_BOOT; - i += put_uint32_t_by_index(version, i, msg->payload); // The onboard software version + p->version = version; // uint32_t:The onboard software version - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_BOOT_LEN); } /** @@ -40,12 +40,12 @@ static inline uint16_t mavlink_msg_boot_pack(uint8_t system_id, uint8_t componen */ static inline uint16_t mavlink_msg_boot_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint32_t version) { - uint16_t i = 0; + mavlink_boot_t *p = (mavlink_boot_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_BOOT; - i += put_uint32_t_by_index(version, i, msg->payload); // The onboard software version + p->version = version; // uint32_t:The onboard software version - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_BOOT_LEN); } /** @@ -67,13 +67,35 @@ static inline uint16_t mavlink_msg_boot_encode(uint8_t system_id, uint8_t compon * * @param version The onboard software version */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_boot_send(mavlink_channel_t chan, uint32_t version) { - mavlink_message_t msg; - mavlink_msg_boot_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, version); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_boot_t payload; + uint16_t checksum; + mavlink_boot_t *p = &payload; + + p->version = version; // uint32_t:The onboard software version + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_BOOT_LEN; + hdr.msgid = MAVLINK_MSG_ID_BOOT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -86,12 +108,8 @@ static inline void mavlink_msg_boot_send(mavlink_channel_t chan, uint32_t versio */ static inline uint32_t mavlink_msg_boot_get_version(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; + mavlink_boot_t *p = (mavlink_boot_t *)&msg->payload[0]; + return (uint32_t)(p->version); } /** @@ -102,5 +120,5 @@ static inline uint32_t mavlink_msg_boot_get_version(const mavlink_message_t* msg */ static inline void mavlink_msg_boot_decode(const mavlink_message_t* msg, mavlink_boot_t* boot) { - boot->version = mavlink_msg_boot_get_version(msg); + memcpy( boot, msg->payload, sizeof(mavlink_boot_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_change_operator_control.h b/thirdParty/mavlink/include/common/mavlink_msg_change_operator_control.h index 3eab478cbf84010a62c3b8f3b3195a1918ebd4b4..9b1a2e795810504b29b2c28391513b57f7b03362 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_change_operator_control.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_change_operator_control.h @@ -1,6 +1,8 @@ // MESSAGE CHANGE_OPERATOR_CONTROL PACKING #define MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL 5 +#define MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN 28 +#define MAVLINK_MSG_5_LEN 28 typedef struct __mavlink_change_operator_control_t { @@ -10,10 +12,8 @@ typedef struct __mavlink_change_operator_control_t char passkey[25]; ///< 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 "!?,.-" } mavlink_change_operator_control_t; - #define MAVLINK_MSG_CHANGE_OPERATOR_CONTROL_FIELD_PASSKEY_LEN 25 - /** * @brief Pack a change_operator_control message * @param system_id ID of this system @@ -28,15 +28,15 @@ typedef struct __mavlink_change_operator_control_t */ static inline uint16_t mavlink_msg_change_operator_control_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t control_request, uint8_t version, const char* passkey) { - uint16_t i = 0; + mavlink_change_operator_control_t *p = (mavlink_change_operator_control_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL; - i += put_uint8_t_by_index(target_system, i, msg->payload); // System the GCS requests control for - i += put_uint8_t_by_index(control_request, i, msg->payload); // 0: request control of this MAV, 1: Release control of this MAV - i += put_uint8_t_by_index(version, i, msg->payload); // 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. - i += put_array_by_index((const int8_t*)passkey, sizeof(char)*25, i, msg->payload); // 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 "!?,.-" + p->target_system = target_system; // uint8_t:System the GCS requests control for + p->control_request = control_request; // uint8_t:0: request control of this MAV, 1: Release control of this MAV + p->version = version; // uint8_t: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. + memcpy(p->passkey, passkey, sizeof(p->passkey)); // char[25]: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 "!?,.-" - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN); } /** @@ -53,15 +53,15 @@ static inline uint16_t mavlink_msg_change_operator_control_pack(uint8_t system_i */ static inline uint16_t mavlink_msg_change_operator_control_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t control_request, uint8_t version, const char* passkey) { - uint16_t i = 0; + mavlink_change_operator_control_t *p = (mavlink_change_operator_control_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL; - i += put_uint8_t_by_index(target_system, i, msg->payload); // System the GCS requests control for - i += put_uint8_t_by_index(control_request, i, msg->payload); // 0: request control of this MAV, 1: Release control of this MAV - i += put_uint8_t_by_index(version, i, msg->payload); // 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. - i += put_array_by_index((const int8_t*)passkey, sizeof(char)*25, i, msg->payload); // 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 "!?,.-" + p->target_system = target_system; // uint8_t:System the GCS requests control for + p->control_request = control_request; // uint8_t:0: request control of this MAV, 1: Release control of this MAV + p->version = version; // uint8_t: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. + memcpy(p->passkey, passkey, sizeof(p->passkey)); // char[25]: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 "!?,.-" - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN); } /** @@ -86,13 +86,38 @@ static inline uint16_t mavlink_msg_change_operator_control_encode(uint8_t system * @param version 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. * @param passkey 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 "!?,.-" */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_change_operator_control_send(mavlink_channel_t chan, uint8_t target_system, uint8_t control_request, uint8_t version, const char* passkey) { - mavlink_message_t msg; - mavlink_msg_change_operator_control_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, control_request, version, passkey); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_change_operator_control_t payload; + uint16_t checksum; + mavlink_change_operator_control_t *p = &payload; + + p->target_system = target_system; // uint8_t:System the GCS requests control for + p->control_request = control_request; // uint8_t:0: request control of this MAV, 1: Release control of this MAV + p->version = version; // uint8_t: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. + memcpy(p->passkey, passkey, sizeof(p->passkey)); // char[25]: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 "!?,.-" + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN; + hdr.msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -105,7 +130,8 @@ static inline void mavlink_msg_change_operator_control_send(mavlink_channel_t ch */ static inline uint8_t mavlink_msg_change_operator_control_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_change_operator_control_t *p = (mavlink_change_operator_control_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); } /** @@ -115,7 +141,8 @@ static inline uint8_t mavlink_msg_change_operator_control_get_target_system(cons */ static inline uint8_t mavlink_msg_change_operator_control_get_control_request(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_change_operator_control_t *p = (mavlink_change_operator_control_t *)&msg->payload[0]; + return (uint8_t)(p->control_request); } /** @@ -125,7 +152,8 @@ static inline uint8_t mavlink_msg_change_operator_control_get_control_request(co */ static inline uint8_t mavlink_msg_change_operator_control_get_version(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_change_operator_control_t *p = (mavlink_change_operator_control_t *)&msg->payload[0]; + return (uint8_t)(p->version); } /** @@ -133,11 +161,12 @@ static inline uint8_t mavlink_msg_change_operator_control_get_version(const mavl * * @return 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 "!?,.-" */ -static inline uint16_t mavlink_msg_change_operator_control_get_passkey(const mavlink_message_t* msg, char* r_data) +static inline uint16_t mavlink_msg_change_operator_control_get_passkey(const mavlink_message_t* msg, char* passkey) { + mavlink_change_operator_control_t *p = (mavlink_change_operator_control_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t), sizeof(char)*25); - return sizeof(char)*25; + memcpy(passkey, p->passkey, sizeof(p->passkey)); + return sizeof(p->passkey); } /** @@ -148,8 +177,5 @@ static inline uint16_t mavlink_msg_change_operator_control_get_passkey(const mav */ static inline void mavlink_msg_change_operator_control_decode(const mavlink_message_t* msg, mavlink_change_operator_control_t* change_operator_control) { - change_operator_control->target_system = mavlink_msg_change_operator_control_get_target_system(msg); - change_operator_control->control_request = mavlink_msg_change_operator_control_get_control_request(msg); - change_operator_control->version = mavlink_msg_change_operator_control_get_version(msg); - mavlink_msg_change_operator_control_get_passkey(msg, change_operator_control->passkey); + memcpy( change_operator_control, msg->payload, sizeof(mavlink_change_operator_control_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_change_operator_control_ack.h b/thirdParty/mavlink/include/common/mavlink_msg_change_operator_control_ack.h index 3529ae059119053772ca0024d6d7195a1b371eec..8e931567559ecf3cacbd52889d227d6f81bcadc2 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_change_operator_control_ack.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_change_operator_control_ack.h @@ -1,6 +1,8 @@ // MESSAGE CHANGE_OPERATOR_CONTROL_ACK PACKING #define MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK 6 +#define MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN 3 +#define MAVLINK_MSG_6_LEN 3 typedef struct __mavlink_change_operator_control_ack_t { @@ -10,8 +12,6 @@ typedef struct __mavlink_change_operator_control_ack_t } mavlink_change_operator_control_ack_t; - - /** * @brief Pack a change_operator_control_ack message * @param system_id ID of this system @@ -25,14 +25,14 @@ typedef struct __mavlink_change_operator_control_ack_t */ static inline uint16_t mavlink_msg_change_operator_control_ack_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t gcs_system_id, uint8_t control_request, uint8_t ack) { - uint16_t i = 0; + mavlink_change_operator_control_ack_t *p = (mavlink_change_operator_control_ack_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK; - i += put_uint8_t_by_index(gcs_system_id, i, msg->payload); // ID of the GCS this message - i += put_uint8_t_by_index(control_request, i, msg->payload); // 0: request control of this MAV, 1: Release control of this MAV - i += put_uint8_t_by_index(ack, i, msg->payload); // 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control + p->gcs_system_id = gcs_system_id; // uint8_t:ID of the GCS this message + p->control_request = control_request; // uint8_t:0: request control of this MAV, 1: Release control of this MAV + p->ack = ack; // uint8_t:0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN); } /** @@ -48,14 +48,14 @@ static inline uint16_t mavlink_msg_change_operator_control_ack_pack(uint8_t syst */ static inline uint16_t mavlink_msg_change_operator_control_ack_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t gcs_system_id, uint8_t control_request, uint8_t ack) { - uint16_t i = 0; + mavlink_change_operator_control_ack_t *p = (mavlink_change_operator_control_ack_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK; - i += put_uint8_t_by_index(gcs_system_id, i, msg->payload); // ID of the GCS this message - i += put_uint8_t_by_index(control_request, i, msg->payload); // 0: request control of this MAV, 1: Release control of this MAV - i += put_uint8_t_by_index(ack, i, msg->payload); // 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control + p->gcs_system_id = gcs_system_id; // uint8_t:ID of the GCS this message + p->control_request = control_request; // uint8_t:0: request control of this MAV, 1: Release control of this MAV + p->ack = ack; // uint8_t:0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN); } /** @@ -79,13 +79,37 @@ static inline uint16_t mavlink_msg_change_operator_control_ack_encode(uint8_t sy * @param control_request 0: request control of this MAV, 1: Release control of this MAV * @param ack 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_change_operator_control_ack_send(mavlink_channel_t chan, uint8_t gcs_system_id, uint8_t control_request, uint8_t ack) { - mavlink_message_t msg; - mavlink_msg_change_operator_control_ack_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, gcs_system_id, control_request, ack); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_change_operator_control_ack_t payload; + uint16_t checksum; + mavlink_change_operator_control_ack_t *p = &payload; + + p->gcs_system_id = gcs_system_id; // uint8_t:ID of the GCS this message + p->control_request = control_request; // uint8_t:0: request control of this MAV, 1: Release control of this MAV + p->ack = ack; // uint8_t:0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN; + hdr.msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -98,7 +122,8 @@ static inline void mavlink_msg_change_operator_control_ack_send(mavlink_channel_ */ static inline uint8_t mavlink_msg_change_operator_control_ack_get_gcs_system_id(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_change_operator_control_ack_t *p = (mavlink_change_operator_control_ack_t *)&msg->payload[0]; + return (uint8_t)(p->gcs_system_id); } /** @@ -108,7 +133,8 @@ static inline uint8_t mavlink_msg_change_operator_control_ack_get_gcs_system_id( */ static inline uint8_t mavlink_msg_change_operator_control_ack_get_control_request(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_change_operator_control_ack_t *p = (mavlink_change_operator_control_ack_t *)&msg->payload[0]; + return (uint8_t)(p->control_request); } /** @@ -118,7 +144,8 @@ static inline uint8_t mavlink_msg_change_operator_control_ack_get_control_reques */ static inline uint8_t mavlink_msg_change_operator_control_ack_get_ack(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_change_operator_control_ack_t *p = (mavlink_change_operator_control_ack_t *)&msg->payload[0]; + return (uint8_t)(p->ack); } /** @@ -129,7 +156,5 @@ static inline uint8_t mavlink_msg_change_operator_control_ack_get_ack(const mavl */ static inline void mavlink_msg_change_operator_control_ack_decode(const mavlink_message_t* msg, mavlink_change_operator_control_ack_t* change_operator_control_ack) { - change_operator_control_ack->gcs_system_id = mavlink_msg_change_operator_control_ack_get_gcs_system_id(msg); - change_operator_control_ack->control_request = mavlink_msg_change_operator_control_ack_get_control_request(msg); - change_operator_control_ack->ack = mavlink_msg_change_operator_control_ack_get_ack(msg); + memcpy( change_operator_control_ack, msg->payload, sizeof(mavlink_change_operator_control_ack_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_cmd_ack.h b/thirdParty/mavlink/include/common/mavlink_msg_cmd_ack.h new file mode 100644 index 0000000000000000000000000000000000000000..b2e7609656f076f1764573fc874ee698c21bc989 --- /dev/null +++ b/thirdParty/mavlink/include/common/mavlink_msg_cmd_ack.h @@ -0,0 +1,142 @@ +// MESSAGE CMD_ACK PACKING + +#define MAVLINK_MSG_ID_CMD_ACK 9 +#define MAVLINK_MSG_ID_CMD_ACK_LEN 2 +#define MAVLINK_MSG_9_LEN 2 + +typedef struct __mavlink_cmd_ack_t +{ + uint8_t cmd; ///< The MAV_CMD ID + uint8_t result; ///< 0: Action DENIED, 1: Action executed + +} mavlink_cmd_ack_t; + +/** + * @brief Pack a cmd_ack 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 cmd The MAV_CMD ID + * @param result 0: Action DENIED, 1: Action executed + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_cmd_ack_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t cmd, uint8_t result) +{ + mavlink_cmd_ack_t *p = (mavlink_cmd_ack_t *)&msg->payload[0]; + msg->msgid = MAVLINK_MSG_ID_CMD_ACK; + + p->cmd = cmd; // uint8_t:The MAV_CMD ID + p->result = result; // uint8_t:0: Action DENIED, 1: Action executed + + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CMD_ACK_LEN); +} + +/** + * @brief Pack a cmd_ack 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 cmd The MAV_CMD ID + * @param result 0: Action DENIED, 1: Action executed + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_cmd_ack_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t cmd, uint8_t result) +{ + mavlink_cmd_ack_t *p = (mavlink_cmd_ack_t *)&msg->payload[0]; + msg->msgid = MAVLINK_MSG_ID_CMD_ACK; + + p->cmd = cmd; // uint8_t:The MAV_CMD ID + p->result = result; // uint8_t:0: Action DENIED, 1: Action executed + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CMD_ACK_LEN); +} + +/** + * @brief Encode a cmd_ack 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 cmd_ack C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_cmd_ack_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_cmd_ack_t* cmd_ack) +{ + return mavlink_msg_cmd_ack_pack(system_id, component_id, msg, cmd_ack->cmd, cmd_ack->result); +} + +/** + * @brief Send a cmd_ack message + * @param chan MAVLink channel to send the message + * + * @param cmd The MAV_CMD ID + * @param result 0: Action DENIED, 1: Action executed + */ + + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS +static inline void mavlink_msg_cmd_ack_send(mavlink_channel_t chan, uint8_t cmd, uint8_t result) +{ + mavlink_header_t hdr; + mavlink_cmd_ack_t payload; + uint16_t checksum; + mavlink_cmd_ack_t *p = &payload; + + p->cmd = cmd; // uint8_t:The MAV_CMD ID + p->result = result; // uint8_t:0: Action DENIED, 1: Action executed + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_CMD_ACK_LEN; + hdr.msgid = MAVLINK_MSG_ID_CMD_ACK; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); +} + +#endif +// MESSAGE CMD_ACK UNPACKING + +/** + * @brief Get field cmd from cmd_ack message + * + * @return The MAV_CMD ID + */ +static inline uint8_t mavlink_msg_cmd_ack_get_cmd(const mavlink_message_t* msg) +{ + mavlink_cmd_ack_t *p = (mavlink_cmd_ack_t *)&msg->payload[0]; + return (uint8_t)(p->cmd); +} + +/** + * @brief Get field result from cmd_ack message + * + * @return 0: Action DENIED, 1: Action executed + */ +static inline uint8_t mavlink_msg_cmd_ack_get_result(const mavlink_message_t* msg) +{ + mavlink_cmd_ack_t *p = (mavlink_cmd_ack_t *)&msg->payload[0]; + return (uint8_t)(p->result); +} + +/** + * @brief Decode a cmd_ack message into a struct + * + * @param msg The message to decode + * @param cmd_ack C-struct to decode the message contents into + */ +static inline void mavlink_msg_cmd_ack_decode(const mavlink_message_t* msg, mavlink_cmd_ack_t* cmd_ack) +{ + memcpy( cmd_ack, msg->payload, sizeof(mavlink_cmd_ack_t)); +} diff --git a/thirdParty/mavlink/include/common/mavlink_msg_command.h b/thirdParty/mavlink/include/common/mavlink_msg_command.h index dbc123570730ed4501bea2114ff93835eee68797..7b8afd94b5f387c9a76a882b3de3b279c4986cc3 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_command.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_command.h @@ -1,22 +1,22 @@ // MESSAGE COMMAND PACKING #define MAVLINK_MSG_ID_COMMAND 75 +#define MAVLINK_MSG_ID_COMMAND_LEN 20 +#define MAVLINK_MSG_75_LEN 20 typedef struct __mavlink_command_t { - uint8_t target_system; ///< System which should execute the command - uint8_t target_component; ///< Component which should execute the command, 0 for all components - uint8_t command; ///< Command ID, as defined by MAV_CMD enum. - uint8_t confirmation; ///< 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) float param1; ///< Parameter 1, as defined by MAV_CMD enum. float param2; ///< Parameter 2, as defined by MAV_CMD enum. float param3; ///< Parameter 3, as defined by MAV_CMD enum. float param4; ///< Parameter 4, as defined by MAV_CMD enum. + uint8_t target_system; ///< System which should execute the command + uint8_t target_component; ///< Component which should execute the command, 0 for all components + uint8_t command; ///< Command ID, as defined by MAV_CMD enum. + uint8_t confirmation; ///< 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) } mavlink_command_t; - - /** * @brief Pack a command message * @param system_id ID of this system @@ -35,19 +35,19 @@ typedef struct __mavlink_command_t */ static inline uint16_t mavlink_msg_command_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint8_t command, uint8_t confirmation, float param1, float param2, float param3, float param4) { - uint16_t i = 0; + mavlink_command_t *p = (mavlink_command_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_COMMAND; - i += put_uint8_t_by_index(target_system, i, msg->payload); // System which should execute the command - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component which should execute the command, 0 for all components - i += put_uint8_t_by_index(command, i, msg->payload); // Command ID, as defined by MAV_CMD enum. - i += put_uint8_t_by_index(confirmation, i, msg->payload); // 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) - i += put_float_by_index(param1, i, msg->payload); // Parameter 1, as defined by MAV_CMD enum. - i += put_float_by_index(param2, i, msg->payload); // Parameter 2, as defined by MAV_CMD enum. - i += put_float_by_index(param3, i, msg->payload); // Parameter 3, as defined by MAV_CMD enum. - i += put_float_by_index(param4, i, msg->payload); // Parameter 4, as defined by MAV_CMD enum. + p->target_system = target_system; // uint8_t:System which should execute the command + p->target_component = target_component; // uint8_t:Component which should execute the command, 0 for all components + p->command = command; // uint8_t:Command ID, as defined by MAV_CMD enum. + p->confirmation = confirmation; // uint8_t:0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) + p->param1 = param1; // float:Parameter 1, as defined by MAV_CMD enum. + p->param2 = param2; // float:Parameter 2, as defined by MAV_CMD enum. + p->param3 = param3; // float:Parameter 3, as defined by MAV_CMD enum. + p->param4 = param4; // float:Parameter 4, as defined by MAV_CMD enum. - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_COMMAND_LEN); } /** @@ -68,19 +68,19 @@ static inline uint16_t mavlink_msg_command_pack(uint8_t system_id, uint8_t compo */ static inline uint16_t mavlink_msg_command_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, uint8_t command, uint8_t confirmation, float param1, float param2, float param3, float param4) { - uint16_t i = 0; + mavlink_command_t *p = (mavlink_command_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_COMMAND; - i += put_uint8_t_by_index(target_system, i, msg->payload); // System which should execute the command - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component which should execute the command, 0 for all components - i += put_uint8_t_by_index(command, i, msg->payload); // Command ID, as defined by MAV_CMD enum. - i += put_uint8_t_by_index(confirmation, i, msg->payload); // 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) - i += put_float_by_index(param1, i, msg->payload); // Parameter 1, as defined by MAV_CMD enum. - i += put_float_by_index(param2, i, msg->payload); // Parameter 2, as defined by MAV_CMD enum. - i += put_float_by_index(param3, i, msg->payload); // Parameter 3, as defined by MAV_CMD enum. - i += put_float_by_index(param4, i, msg->payload); // Parameter 4, as defined by MAV_CMD enum. + p->target_system = target_system; // uint8_t:System which should execute the command + p->target_component = target_component; // uint8_t:Component which should execute the command, 0 for all components + p->command = command; // uint8_t:Command ID, as defined by MAV_CMD enum. + p->confirmation = confirmation; // uint8_t:0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) + p->param1 = param1; // float:Parameter 1, as defined by MAV_CMD enum. + p->param2 = param2; // float:Parameter 2, as defined by MAV_CMD enum. + p->param3 = param3; // float:Parameter 3, as defined by MAV_CMD enum. + p->param4 = param4; // float:Parameter 4, as defined by MAV_CMD enum. - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_COMMAND_LEN); } /** @@ -109,13 +109,42 @@ static inline uint16_t mavlink_msg_command_encode(uint8_t system_id, uint8_t com * @param param3 Parameter 3, as defined by MAV_CMD enum. * @param param4 Parameter 4, as defined by MAV_CMD enum. */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_command_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t command, uint8_t confirmation, float param1, float param2, float param3, float param4) { - mavlink_message_t msg; - mavlink_msg_command_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, command, confirmation, param1, param2, param3, param4); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_command_t payload; + uint16_t checksum; + mavlink_command_t *p = &payload; + + p->target_system = target_system; // uint8_t:System which should execute the command + p->target_component = target_component; // uint8_t:Component which should execute the command, 0 for all components + p->command = command; // uint8_t:Command ID, as defined by MAV_CMD enum. + p->confirmation = confirmation; // uint8_t:0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) + p->param1 = param1; // float:Parameter 1, as defined by MAV_CMD enum. + p->param2 = param2; // float:Parameter 2, as defined by MAV_CMD enum. + p->param3 = param3; // float:Parameter 3, as defined by MAV_CMD enum. + p->param4 = param4; // float:Parameter 4, as defined by MAV_CMD enum. + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_COMMAND_LEN; + hdr.msgid = MAVLINK_MSG_ID_COMMAND; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -128,7 +157,8 @@ static inline void mavlink_msg_command_send(mavlink_channel_t chan, uint8_t targ */ static inline uint8_t mavlink_msg_command_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_command_t *p = (mavlink_command_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); } /** @@ -138,7 +168,8 @@ static inline uint8_t mavlink_msg_command_get_target_system(const mavlink_messag */ static inline uint8_t mavlink_msg_command_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_command_t *p = (mavlink_command_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); } /** @@ -148,7 +179,8 @@ static inline uint8_t mavlink_msg_command_get_target_component(const mavlink_mes */ static inline uint8_t mavlink_msg_command_get_command(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_command_t *p = (mavlink_command_t *)&msg->payload[0]; + return (uint8_t)(p->command); } /** @@ -158,7 +190,8 @@ static inline uint8_t mavlink_msg_command_get_command(const mavlink_message_t* m */ static inline uint8_t mavlink_msg_command_get_confirmation(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_command_t *p = (mavlink_command_t *)&msg->payload[0]; + return (uint8_t)(p->confirmation); } /** @@ -168,12 +201,8 @@ static inline uint8_t mavlink_msg_command_get_confirmation(const mavlink_message */ static inline float mavlink_msg_command_get_param1(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[3]; - return (float)r.f; + mavlink_command_t *p = (mavlink_command_t *)&msg->payload[0]; + return (float)(p->param1); } /** @@ -183,12 +212,8 @@ static inline float mavlink_msg_command_get_param1(const mavlink_message_t* msg) */ static inline float mavlink_msg_command_get_param2(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[3]; - return (float)r.f; + mavlink_command_t *p = (mavlink_command_t *)&msg->payload[0]; + return (float)(p->param2); } /** @@ -198,12 +223,8 @@ static inline float mavlink_msg_command_get_param2(const mavlink_message_t* msg) */ static inline float mavlink_msg_command_get_param3(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_command_t *p = (mavlink_command_t *)&msg->payload[0]; + return (float)(p->param3); } /** @@ -213,12 +234,8 @@ static inline float mavlink_msg_command_get_param3(const mavlink_message_t* msg) */ static inline float mavlink_msg_command_get_param4(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+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(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+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(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_command_t *p = (mavlink_command_t *)&msg->payload[0]; + return (float)(p->param4); } /** @@ -229,12 +246,5 @@ static inline float mavlink_msg_command_get_param4(const mavlink_message_t* msg) */ static inline void mavlink_msg_command_decode(const mavlink_message_t* msg, mavlink_command_t* command) { - command->target_system = mavlink_msg_command_get_target_system(msg); - command->target_component = mavlink_msg_command_get_target_component(msg); - command->command = mavlink_msg_command_get_command(msg); - command->confirmation = mavlink_msg_command_get_confirmation(msg); - command->param1 = mavlink_msg_command_get_param1(msg); - command->param2 = mavlink_msg_command_get_param2(msg); - command->param3 = mavlink_msg_command_get_param3(msg); - command->param4 = mavlink_msg_command_get_param4(msg); + memcpy( command, msg->payload, sizeof(mavlink_command_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_command_ack.h b/thirdParty/mavlink/include/common/mavlink_msg_command_ack.h index 32b39da3a6550341a6050d8d87bfa9834780fd73..c7e4a4b0ff6fdace6c729312e6984baeb4245e52 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_command_ack.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_command_ack.h @@ -1,6 +1,8 @@ // MESSAGE COMMAND_ACK PACKING #define MAVLINK_MSG_ID_COMMAND_ACK 76 +#define MAVLINK_MSG_ID_COMMAND_ACK_LEN 8 +#define MAVLINK_MSG_76_LEN 8 typedef struct __mavlink_command_ack_t { @@ -9,8 +11,6 @@ typedef struct __mavlink_command_ack_t } mavlink_command_ack_t; - - /** * @brief Pack a command_ack message * @param system_id ID of this system @@ -23,13 +23,13 @@ typedef struct __mavlink_command_ack_t */ static inline uint16_t mavlink_msg_command_ack_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, float command, float result) { - uint16_t i = 0; + mavlink_command_ack_t *p = (mavlink_command_ack_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_COMMAND_ACK; - i += put_float_by_index(command, i, msg->payload); // Current airspeed in m/s - i += put_float_by_index(result, i, msg->payload); // 1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION + p->command = command; // float:Current airspeed in m/s + p->result = result; // float:1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_COMMAND_ACK_LEN); } /** @@ -44,13 +44,13 @@ static inline uint16_t mavlink_msg_command_ack_pack(uint8_t system_id, uint8_t c */ static inline uint16_t mavlink_msg_command_ack_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, float command, float result) { - uint16_t i = 0; + mavlink_command_ack_t *p = (mavlink_command_ack_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_COMMAND_ACK; - i += put_float_by_index(command, i, msg->payload); // Current airspeed in m/s - i += put_float_by_index(result, i, msg->payload); // 1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION + p->command = command; // float:Current airspeed in m/s + p->result = result; // float:1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_COMMAND_ACK_LEN); } /** @@ -73,13 +73,36 @@ static inline uint16_t mavlink_msg_command_ack_encode(uint8_t system_id, uint8_t * @param command Current airspeed in m/s * @param result 1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_command_ack_send(mavlink_channel_t chan, float command, float result) { - mavlink_message_t msg; - mavlink_msg_command_ack_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, command, result); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_command_ack_t payload; + uint16_t checksum; + mavlink_command_ack_t *p = &payload; + + p->command = command; // float:Current airspeed in m/s + p->result = result; // float:1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_COMMAND_ACK_LEN; + hdr.msgid = MAVLINK_MSG_ID_COMMAND_ACK; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -92,12 +115,8 @@ static inline void mavlink_msg_command_ack_send(mavlink_channel_t chan, float co */ static inline float mavlink_msg_command_ack_get_command(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 (float)r.f; + mavlink_command_ack_t *p = (mavlink_command_ack_t *)&msg->payload[0]; + return (float)(p->command); } /** @@ -107,12 +126,8 @@ static inline float mavlink_msg_command_ack_get_command(const mavlink_message_t* */ static inline float mavlink_msg_command_ack_get_result(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float))[3]; - return (float)r.f; + mavlink_command_ack_t *p = (mavlink_command_ack_t *)&msg->payload[0]; + return (float)(p->result); } /** @@ -123,6 +138,5 @@ static inline float mavlink_msg_command_ack_get_result(const mavlink_message_t* */ static inline void mavlink_msg_command_ack_decode(const mavlink_message_t* msg, mavlink_command_ack_t* command_ack) { - command_ack->command = mavlink_msg_command_ack_get_command(msg); - command_ack->result = mavlink_msg_command_ack_get_result(msg); + memcpy( command_ack, msg->payload, sizeof(mavlink_command_ack_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_control_status.h b/thirdParty/mavlink/include/common/mavlink_msg_control_status.h index 746e889aafaf9d6b42e3bccccffbeb82a8d97f1d..8974f07d084f23b34563dfbc94db061271cc0233 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_control_status.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_control_status.h @@ -1,6 +1,8 @@ // MESSAGE CONTROL_STATUS PACKING #define MAVLINK_MSG_ID_CONTROL_STATUS 52 +#define MAVLINK_MSG_ID_CONTROL_STATUS_LEN 8 +#define MAVLINK_MSG_52_LEN 8 typedef struct __mavlink_control_status_t { @@ -15,8 +17,6 @@ typedef struct __mavlink_control_status_t } mavlink_control_status_t; - - /** * @brief Pack a control_status message * @param system_id ID of this system @@ -35,19 +35,19 @@ typedef struct __mavlink_control_status_t */ static inline uint16_t mavlink_msg_control_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t position_fix, uint8_t vision_fix, uint8_t gps_fix, uint8_t ahrs_health, uint8_t control_att, uint8_t control_pos_xy, uint8_t control_pos_z, uint8_t control_pos_yaw) { - uint16_t i = 0; + mavlink_control_status_t *p = (mavlink_control_status_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_CONTROL_STATUS; - i += put_uint8_t_by_index(position_fix, i, msg->payload); // Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix - i += put_uint8_t_by_index(vision_fix, i, msg->payload); // Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix - i += put_uint8_t_by_index(gps_fix, i, msg->payload); // GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix - i += put_uint8_t_by_index(ahrs_health, i, msg->payload); // Attitude estimation health: 0: poor, 255: excellent - i += put_uint8_t_by_index(control_att, i, msg->payload); // 0: Attitude control disabled, 1: enabled - i += put_uint8_t_by_index(control_pos_xy, i, msg->payload); // 0: X, Y position control disabled, 1: enabled - i += put_uint8_t_by_index(control_pos_z, i, msg->payload); // 0: Z position control disabled, 1: enabled - i += put_uint8_t_by_index(control_pos_yaw, i, msg->payload); // 0: Yaw angle control disabled, 1: enabled + p->position_fix = position_fix; // uint8_t:Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix + p->vision_fix = vision_fix; // uint8_t:Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix + p->gps_fix = gps_fix; // uint8_t:GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix + p->ahrs_health = ahrs_health; // uint8_t:Attitude estimation health: 0: poor, 255: excellent + p->control_att = control_att; // uint8_t:0: Attitude control disabled, 1: enabled + p->control_pos_xy = control_pos_xy; // uint8_t:0: X, Y position control disabled, 1: enabled + p->control_pos_z = control_pos_z; // uint8_t:0: Z position control disabled, 1: enabled + p->control_pos_yaw = control_pos_yaw; // uint8_t:0: Yaw angle control disabled, 1: enabled - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CONTROL_STATUS_LEN); } /** @@ -68,19 +68,19 @@ static inline uint16_t mavlink_msg_control_status_pack(uint8_t system_id, uint8_ */ static inline uint16_t mavlink_msg_control_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t position_fix, uint8_t vision_fix, uint8_t gps_fix, uint8_t ahrs_health, uint8_t control_att, uint8_t control_pos_xy, uint8_t control_pos_z, uint8_t control_pos_yaw) { - uint16_t i = 0; + mavlink_control_status_t *p = (mavlink_control_status_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_CONTROL_STATUS; - i += put_uint8_t_by_index(position_fix, i, msg->payload); // Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix - i += put_uint8_t_by_index(vision_fix, i, msg->payload); // Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix - i += put_uint8_t_by_index(gps_fix, i, msg->payload); // GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix - i += put_uint8_t_by_index(ahrs_health, i, msg->payload); // Attitude estimation health: 0: poor, 255: excellent - i += put_uint8_t_by_index(control_att, i, msg->payload); // 0: Attitude control disabled, 1: enabled - i += put_uint8_t_by_index(control_pos_xy, i, msg->payload); // 0: X, Y position control disabled, 1: enabled - i += put_uint8_t_by_index(control_pos_z, i, msg->payload); // 0: Z position control disabled, 1: enabled - i += put_uint8_t_by_index(control_pos_yaw, i, msg->payload); // 0: Yaw angle control disabled, 1: enabled + p->position_fix = position_fix; // uint8_t:Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix + p->vision_fix = vision_fix; // uint8_t:Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix + p->gps_fix = gps_fix; // uint8_t:GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix + p->ahrs_health = ahrs_health; // uint8_t:Attitude estimation health: 0: poor, 255: excellent + p->control_att = control_att; // uint8_t:0: Attitude control disabled, 1: enabled + p->control_pos_xy = control_pos_xy; // uint8_t:0: X, Y position control disabled, 1: enabled + p->control_pos_z = control_pos_z; // uint8_t:0: Z position control disabled, 1: enabled + p->control_pos_yaw = control_pos_yaw; // uint8_t:0: Yaw angle control disabled, 1: enabled - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CONTROL_STATUS_LEN); } /** @@ -109,13 +109,42 @@ static inline uint16_t mavlink_msg_control_status_encode(uint8_t system_id, uint * @param control_pos_z 0: Z position control disabled, 1: enabled * @param control_pos_yaw 0: Yaw angle control disabled, 1: enabled */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_control_status_send(mavlink_channel_t chan, uint8_t position_fix, uint8_t vision_fix, uint8_t gps_fix, uint8_t ahrs_health, uint8_t control_att, uint8_t control_pos_xy, uint8_t control_pos_z, uint8_t control_pos_yaw) { - mavlink_message_t msg; - mavlink_msg_control_status_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, position_fix, vision_fix, gps_fix, ahrs_health, control_att, control_pos_xy, control_pos_z, control_pos_yaw); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_control_status_t payload; + uint16_t checksum; + mavlink_control_status_t *p = &payload; + + p->position_fix = position_fix; // uint8_t:Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix + p->vision_fix = vision_fix; // uint8_t:Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix + p->gps_fix = gps_fix; // uint8_t:GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix + p->ahrs_health = ahrs_health; // uint8_t:Attitude estimation health: 0: poor, 255: excellent + p->control_att = control_att; // uint8_t:0: Attitude control disabled, 1: enabled + p->control_pos_xy = control_pos_xy; // uint8_t:0: X, Y position control disabled, 1: enabled + p->control_pos_z = control_pos_z; // uint8_t:0: Z position control disabled, 1: enabled + p->control_pos_yaw = control_pos_yaw; // uint8_t:0: Yaw angle control disabled, 1: enabled + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_CONTROL_STATUS_LEN; + hdr.msgid = MAVLINK_MSG_ID_CONTROL_STATUS; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -128,7 +157,8 @@ static inline void mavlink_msg_control_status_send(mavlink_channel_t chan, uint8 */ static inline uint8_t mavlink_msg_control_status_get_position_fix(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_control_status_t *p = (mavlink_control_status_t *)&msg->payload[0]; + return (uint8_t)(p->position_fix); } /** @@ -138,7 +168,8 @@ static inline uint8_t mavlink_msg_control_status_get_position_fix(const mavlink_ */ static inline uint8_t mavlink_msg_control_status_get_vision_fix(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_control_status_t *p = (mavlink_control_status_t *)&msg->payload[0]; + return (uint8_t)(p->vision_fix); } /** @@ -148,7 +179,8 @@ static inline uint8_t mavlink_msg_control_status_get_vision_fix(const mavlink_me */ static inline uint8_t mavlink_msg_control_status_get_gps_fix(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_control_status_t *p = (mavlink_control_status_t *)&msg->payload[0]; + return (uint8_t)(p->gps_fix); } /** @@ -158,7 +190,8 @@ static inline uint8_t mavlink_msg_control_status_get_gps_fix(const mavlink_messa */ static inline uint8_t mavlink_msg_control_status_get_ahrs_health(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_control_status_t *p = (mavlink_control_status_t *)&msg->payload[0]; + return (uint8_t)(p->ahrs_health); } /** @@ -168,7 +201,8 @@ static inline uint8_t mavlink_msg_control_status_get_ahrs_health(const mavlink_m */ static inline uint8_t mavlink_msg_control_status_get_control_att(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_control_status_t *p = (mavlink_control_status_t *)&msg->payload[0]; + return (uint8_t)(p->control_att); } /** @@ -178,7 +212,8 @@ static inline uint8_t mavlink_msg_control_status_get_control_att(const mavlink_m */ static inline uint8_t mavlink_msg_control_status_get_control_pos_xy(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_control_status_t *p = (mavlink_control_status_t *)&msg->payload[0]; + return (uint8_t)(p->control_pos_xy); } /** @@ -188,7 +223,8 @@ static inline uint8_t mavlink_msg_control_status_get_control_pos_xy(const mavlin */ static inline uint8_t mavlink_msg_control_status_get_control_pos_z(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_control_status_t *p = (mavlink_control_status_t *)&msg->payload[0]; + return (uint8_t)(p->control_pos_z); } /** @@ -198,7 +234,8 @@ static inline uint8_t mavlink_msg_control_status_get_control_pos_z(const mavlink */ static inline uint8_t mavlink_msg_control_status_get_control_pos_yaw(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_control_status_t *p = (mavlink_control_status_t *)&msg->payload[0]; + return (uint8_t)(p->control_pos_yaw); } /** @@ -209,12 +246,5 @@ static inline uint8_t mavlink_msg_control_status_get_control_pos_yaw(const mavli */ static inline void mavlink_msg_control_status_decode(const mavlink_message_t* msg, mavlink_control_status_t* control_status) { - control_status->position_fix = mavlink_msg_control_status_get_position_fix(msg); - control_status->vision_fix = mavlink_msg_control_status_get_vision_fix(msg); - control_status->gps_fix = mavlink_msg_control_status_get_gps_fix(msg); - control_status->ahrs_health = mavlink_msg_control_status_get_ahrs_health(msg); - control_status->control_att = mavlink_msg_control_status_get_control_att(msg); - control_status->control_pos_xy = mavlink_msg_control_status_get_control_pos_xy(msg); - control_status->control_pos_z = mavlink_msg_control_status_get_control_pos_z(msg); - control_status->control_pos_yaw = mavlink_msg_control_status_get_control_pos_yaw(msg); + memcpy( control_status, msg->payload, sizeof(mavlink_control_status_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_debug.h b/thirdParty/mavlink/include/common/mavlink_msg_debug.h index e98be847ada748a728ca21b1234bdcb7eb9d5f01..419ef33e84a55ecc228cc6e5e6b5dee322307a7c 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_debug.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_debug.h @@ -1,16 +1,16 @@ // MESSAGE DEBUG PACKING #define MAVLINK_MSG_ID_DEBUG 255 +#define MAVLINK_MSG_ID_DEBUG_LEN 5 +#define MAVLINK_MSG_255_LEN 5 typedef struct __mavlink_debug_t { - uint8_t ind; ///< index of debug variable float value; ///< DEBUG value + uint8_t ind; ///< index of debug variable } mavlink_debug_t; - - /** * @brief Pack a debug message * @param system_id ID of this system @@ -23,13 +23,13 @@ typedef struct __mavlink_debug_t */ static inline uint16_t mavlink_msg_debug_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t ind, float value) { - uint16_t i = 0; + mavlink_debug_t *p = (mavlink_debug_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_DEBUG; - i += put_uint8_t_by_index(ind, i, msg->payload); // index of debug variable - i += put_float_by_index(value, i, msg->payload); // DEBUG value + p->ind = ind; // uint8_t:index of debug variable + p->value = value; // float:DEBUG value - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DEBUG_LEN); } /** @@ -44,13 +44,13 @@ static inline uint16_t mavlink_msg_debug_pack(uint8_t system_id, uint8_t compone */ static inline uint16_t mavlink_msg_debug_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t ind, float value) { - uint16_t i = 0; + mavlink_debug_t *p = (mavlink_debug_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_DEBUG; - i += put_uint8_t_by_index(ind, i, msg->payload); // index of debug variable - i += put_float_by_index(value, i, msg->payload); // DEBUG value + p->ind = ind; // uint8_t:index of debug variable + p->value = value; // float:DEBUG value - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DEBUG_LEN); } /** @@ -73,13 +73,36 @@ static inline uint16_t mavlink_msg_debug_encode(uint8_t system_id, uint8_t compo * @param ind index of debug variable * @param value DEBUG value */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_debug_send(mavlink_channel_t chan, uint8_t ind, float value) { - mavlink_message_t msg; - mavlink_msg_debug_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, ind, value); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_debug_t payload; + uint16_t checksum; + mavlink_debug_t *p = &payload; + + p->ind = ind; // uint8_t:index of debug variable + p->value = value; // float:DEBUG value + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_DEBUG_LEN; + hdr.msgid = MAVLINK_MSG_ID_DEBUG; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -92,7 +115,8 @@ static inline void mavlink_msg_debug_send(mavlink_channel_t chan, uint8_t ind, f */ static inline uint8_t mavlink_msg_debug_get_ind(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_debug_t *p = (mavlink_debug_t *)&msg->payload[0]; + return (uint8_t)(p->ind); } /** @@ -102,12 +126,8 @@ static inline uint8_t mavlink_msg_debug_get_ind(const mavlink_message_t* msg) */ static inline float mavlink_msg_debug_get_value(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t))[3]; - return (float)r.f; + mavlink_debug_t *p = (mavlink_debug_t *)&msg->payload[0]; + return (float)(p->value); } /** @@ -118,6 +138,5 @@ static inline float mavlink_msg_debug_get_value(const mavlink_message_t* msg) */ static inline void mavlink_msg_debug_decode(const mavlink_message_t* msg, mavlink_debug_t* debug) { - debug->ind = mavlink_msg_debug_get_ind(msg); - debug->value = mavlink_msg_debug_get_value(msg); + memcpy( debug, msg->payload, sizeof(mavlink_debug_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_debug_vect.h b/thirdParty/mavlink/include/common/mavlink_msg_debug_vect.h index 6ea9e0558bf73d5956de6cc279a33f1d78bc0e28..53ef8bd11c0e017afd1afdf5c82b6ffee55892b8 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_debug_vect.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_debug_vect.h @@ -1,20 +1,20 @@ // MESSAGE DEBUG_VECT PACKING #define MAVLINK_MSG_ID_DEBUG_VECT 251 +#define MAVLINK_MSG_ID_DEBUG_VECT_LEN 30 +#define MAVLINK_MSG_251_LEN 30 typedef struct __mavlink_debug_vect_t { - char name[10]; ///< Name uint64_t usec; ///< Timestamp float x; ///< x float y; ///< y float z; ///< z + char name[10]; ///< Name } mavlink_debug_vect_t; - #define MAVLINK_MSG_DEBUG_VECT_FIELD_NAME_LEN 10 - /** * @brief Pack a debug_vect message * @param system_id ID of this system @@ -30,16 +30,16 @@ typedef struct __mavlink_debug_vect_t */ static inline uint16_t mavlink_msg_debug_vect_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const char* name, uint64_t usec, float x, float y, float z) { - uint16_t i = 0; + mavlink_debug_vect_t *p = (mavlink_debug_vect_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_DEBUG_VECT; - i += put_array_by_index((const int8_t*)name, sizeof(char)*10, i, msg->payload); // Name - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp - i += put_float_by_index(x, i, msg->payload); // x - i += put_float_by_index(y, i, msg->payload); // y - i += put_float_by_index(z, i, msg->payload); // z + memcpy(p->name, name, sizeof(p->name)); // char[10]:Name + p->usec = usec; // uint64_t:Timestamp + p->x = x; // float:x + p->y = y; // float:y + p->z = z; // float:z - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DEBUG_VECT_LEN); } /** @@ -57,16 +57,16 @@ static inline uint16_t mavlink_msg_debug_vect_pack(uint8_t system_id, uint8_t co */ static inline uint16_t mavlink_msg_debug_vect_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const char* name, uint64_t usec, float x, float y, float z) { - uint16_t i = 0; + mavlink_debug_vect_t *p = (mavlink_debug_vect_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_DEBUG_VECT; - i += put_array_by_index((const int8_t*)name, sizeof(char)*10, i, msg->payload); // Name - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp - i += put_float_by_index(x, i, msg->payload); // x - i += put_float_by_index(y, i, msg->payload); // y - i += put_float_by_index(z, i, msg->payload); // z + memcpy(p->name, name, sizeof(p->name)); // char[10]:Name + p->usec = usec; // uint64_t:Timestamp + p->x = x; // float:x + p->y = y; // float:y + p->z = z; // float:z - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DEBUG_VECT_LEN); } /** @@ -92,13 +92,39 @@ static inline uint16_t mavlink_msg_debug_vect_encode(uint8_t system_id, uint8_t * @param y y * @param z z */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_debug_vect_send(mavlink_channel_t chan, const char* name, uint64_t usec, float x, float y, float z) { - mavlink_message_t msg; - mavlink_msg_debug_vect_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, name, usec, x, y, z); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_debug_vect_t payload; + uint16_t checksum; + mavlink_debug_vect_t *p = &payload; + + memcpy(p->name, name, sizeof(p->name)); // char[10]:Name + p->usec = usec; // uint64_t:Timestamp + p->x = x; // float:x + p->y = y; // float:y + p->z = z; // float:z + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_DEBUG_VECT_LEN; + hdr.msgid = MAVLINK_MSG_ID_DEBUG_VECT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -109,11 +135,12 @@ static inline void mavlink_msg_debug_vect_send(mavlink_channel_t chan, const cha * * @return Name */ -static inline uint16_t mavlink_msg_debug_vect_get_name(const mavlink_message_t* msg, char* r_data) +static inline uint16_t mavlink_msg_debug_vect_get_name(const mavlink_message_t* msg, char* name) { + mavlink_debug_vect_t *p = (mavlink_debug_vect_t *)&msg->payload[0]; - memcpy(r_data, msg->payload, sizeof(char)*10); - return sizeof(char)*10; + memcpy(name, p->name, sizeof(p->name)); + return sizeof(p->name); } /** @@ -123,16 +150,8 @@ static inline uint16_t mavlink_msg_debug_vect_get_name(const mavlink_message_t* */ static inline uint64_t mavlink_msg_debug_vect_get_usec(const mavlink_message_t* msg) { - generic_64bit r; - r.b[7] = (msg->payload+sizeof(char)*10)[0]; - r.b[6] = (msg->payload+sizeof(char)*10)[1]; - r.b[5] = (msg->payload+sizeof(char)*10)[2]; - r.b[4] = (msg->payload+sizeof(char)*10)[3]; - r.b[3] = (msg->payload+sizeof(char)*10)[4]; - r.b[2] = (msg->payload+sizeof(char)*10)[5]; - r.b[1] = (msg->payload+sizeof(char)*10)[6]; - r.b[0] = (msg->payload+sizeof(char)*10)[7]; - return (uint64_t)r.ll; + mavlink_debug_vect_t *p = (mavlink_debug_vect_t *)&msg->payload[0]; + return (uint64_t)(p->usec); } /** @@ -142,12 +161,8 @@ static inline uint64_t mavlink_msg_debug_vect_get_usec(const mavlink_message_t* */ static inline float mavlink_msg_debug_vect_get_x(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(char)*10+sizeof(uint64_t))[0]; - r.b[2] = (msg->payload+sizeof(char)*10+sizeof(uint64_t))[1]; - r.b[1] = (msg->payload+sizeof(char)*10+sizeof(uint64_t))[2]; - r.b[0] = (msg->payload+sizeof(char)*10+sizeof(uint64_t))[3]; - return (float)r.f; + mavlink_debug_vect_t *p = (mavlink_debug_vect_t *)&msg->payload[0]; + return (float)(p->x); } /** @@ -157,12 +172,8 @@ static inline float mavlink_msg_debug_vect_get_x(const mavlink_message_t* msg) */ static inline float mavlink_msg_debug_vect_get_y(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(char)*10+sizeof(uint64_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(char)*10+sizeof(uint64_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(char)*10+sizeof(uint64_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(char)*10+sizeof(uint64_t)+sizeof(float))[3]; - return (float)r.f; + mavlink_debug_vect_t *p = (mavlink_debug_vect_t *)&msg->payload[0]; + return (float)(p->y); } /** @@ -172,12 +183,8 @@ static inline float mavlink_msg_debug_vect_get_y(const mavlink_message_t* msg) */ static inline float mavlink_msg_debug_vect_get_z(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(char)*10+sizeof(uint64_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(char)*10+sizeof(uint64_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(char)*10+sizeof(uint64_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(char)*10+sizeof(uint64_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_debug_vect_t *p = (mavlink_debug_vect_t *)&msg->payload[0]; + return (float)(p->z); } /** @@ -188,9 +195,5 @@ static inline float mavlink_msg_debug_vect_get_z(const mavlink_message_t* msg) */ static inline void mavlink_msg_debug_vect_decode(const mavlink_message_t* msg, mavlink_debug_vect_t* debug_vect) { - mavlink_msg_debug_vect_get_name(msg, debug_vect->name); - debug_vect->usec = mavlink_msg_debug_vect_get_usec(msg); - debug_vect->x = mavlink_msg_debug_vect_get_x(msg); - debug_vect->y = mavlink_msg_debug_vect_get_y(msg); - debug_vect->z = mavlink_msg_debug_vect_get_z(msg); + memcpy( debug_vect, msg->payload, sizeof(mavlink_debug_vect_t)); } 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..263d3e96784e4d0e967178a678814a29eeb73ecc --- /dev/null +++ b/thirdParty/mavlink/include/common/mavlink_msg_full_state.h @@ -0,0 +1,394 @@ +// MESSAGE FULL_STATE PACKING + +#define MAVLINK_MSG_ID_FULL_STATE 67 +#define MAVLINK_MSG_ID_FULL_STATE_LEN 56 +#define MAVLINK_MSG_67_LEN 56 + +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) +{ + mavlink_full_state_t *p = (mavlink_full_state_t *)&msg->payload[0]; + msg->msgid = MAVLINK_MSG_ID_FULL_STATE; + + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->roll = roll; // float:Roll angle (rad) + p->pitch = pitch; // float:Pitch angle (rad) + p->yaw = yaw; // float:Yaw angle (rad) + p->rollspeed = rollspeed; // float:Roll angular speed (rad/s) + p->pitchspeed = pitchspeed; // float:Pitch angular speed (rad/s) + p->yawspeed = yawspeed; // float:Yaw angular speed (rad/s) + p->lat = lat; // int32_t:Latitude, expressed as * 1E7 + p->lon = lon; // int32_t:Longitude, expressed as * 1E7 + p->alt = alt; // int32_t:Altitude in meters, expressed as * 1000 (millimeters) + p->vx = vx; // int16_t:Ground X Speed (Latitude), expressed as m/s * 100 + p->vy = vy; // int16_t:Ground Y Speed (Longitude), expressed as m/s * 100 + p->vz = vz; // int16_t:Ground Z Speed (Altitude), expressed as m/s * 100 + p->xacc = xacc; // int16_t:X acceleration (mg) + p->yacc = yacc; // int16_t:Y acceleration (mg) + p->zacc = zacc; // int16_t:Z acceleration (mg) + + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FULL_STATE_LEN); +} + +/** + * @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) +{ + mavlink_full_state_t *p = (mavlink_full_state_t *)&msg->payload[0]; + msg->msgid = MAVLINK_MSG_ID_FULL_STATE; + + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->roll = roll; // float:Roll angle (rad) + p->pitch = pitch; // float:Pitch angle (rad) + p->yaw = yaw; // float:Yaw angle (rad) + p->rollspeed = rollspeed; // float:Roll angular speed (rad/s) + p->pitchspeed = pitchspeed; // float:Pitch angular speed (rad/s) + p->yawspeed = yawspeed; // float:Yaw angular speed (rad/s) + p->lat = lat; // int32_t:Latitude, expressed as * 1E7 + p->lon = lon; // int32_t:Longitude, expressed as * 1E7 + p->alt = alt; // int32_t:Altitude in meters, expressed as * 1000 (millimeters) + p->vx = vx; // int16_t:Ground X Speed (Latitude), expressed as m/s * 100 + p->vy = vy; // int16_t:Ground Y Speed (Longitude), expressed as m/s * 100 + p->vz = vz; // int16_t:Ground Z Speed (Altitude), expressed as m/s * 100 + p->xacc = xacc; // int16_t:X acceleration (mg) + p->yacc = yacc; // int16_t:Y acceleration (mg) + p->zacc = zacc; // int16_t:Z acceleration (mg) + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FULL_STATE_LEN); +} + +/** + * @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_header_t hdr; + mavlink_full_state_t payload; + uint16_t checksum; + mavlink_full_state_t *p = &payload; + + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->roll = roll; // float:Roll angle (rad) + p->pitch = pitch; // float:Pitch angle (rad) + p->yaw = yaw; // float:Yaw angle (rad) + p->rollspeed = rollspeed; // float:Roll angular speed (rad/s) + p->pitchspeed = pitchspeed; // float:Pitch angular speed (rad/s) + p->yawspeed = yawspeed; // float:Yaw angular speed (rad/s) + p->lat = lat; // int32_t:Latitude, expressed as * 1E7 + p->lon = lon; // int32_t:Longitude, expressed as * 1E7 + p->alt = alt; // int32_t:Altitude in meters, expressed as * 1000 (millimeters) + p->vx = vx; // int16_t:Ground X Speed (Latitude), expressed as m/s * 100 + p->vy = vy; // int16_t:Ground Y Speed (Longitude), expressed as m/s * 100 + p->vz = vz; // int16_t:Ground Z Speed (Altitude), expressed as m/s * 100 + p->xacc = xacc; // int16_t:X acceleration (mg) + p->yacc = yacc; // int16_t:Y acceleration (mg) + p->zacc = zacc; // int16_t:Z acceleration (mg) + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_FULL_STATE_LEN; + hdr.msgid = MAVLINK_MSG_ID_FULL_STATE; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); +} + +#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) +{ + mavlink_full_state_t *p = (mavlink_full_state_t *)&msg->payload[0]; + return (uint64_t)(p->usec); +} + +/** + * @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) +{ + mavlink_full_state_t *p = (mavlink_full_state_t *)&msg->payload[0]; + return (float)(p->roll); +} + +/** + * @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) +{ + mavlink_full_state_t *p = (mavlink_full_state_t *)&msg->payload[0]; + return (float)(p->pitch); +} + +/** + * @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) +{ + mavlink_full_state_t *p = (mavlink_full_state_t *)&msg->payload[0]; + return (float)(p->yaw); +} + +/** + * @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) +{ + mavlink_full_state_t *p = (mavlink_full_state_t *)&msg->payload[0]; + return (float)(p->rollspeed); +} + +/** + * @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) +{ + mavlink_full_state_t *p = (mavlink_full_state_t *)&msg->payload[0]; + return (float)(p->pitchspeed); +} + +/** + * @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) +{ + mavlink_full_state_t *p = (mavlink_full_state_t *)&msg->payload[0]; + return (float)(p->yawspeed); +} + +/** + * @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) +{ + mavlink_full_state_t *p = (mavlink_full_state_t *)&msg->payload[0]; + return (int32_t)(p->lat); +} + +/** + * @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) +{ + mavlink_full_state_t *p = (mavlink_full_state_t *)&msg->payload[0]; + return (int32_t)(p->lon); +} + +/** + * @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) +{ + mavlink_full_state_t *p = (mavlink_full_state_t *)&msg->payload[0]; + return (int32_t)(p->alt); +} + +/** + * @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) +{ + mavlink_full_state_t *p = (mavlink_full_state_t *)&msg->payload[0]; + return (int16_t)(p->vx); +} + +/** + * @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) +{ + mavlink_full_state_t *p = (mavlink_full_state_t *)&msg->payload[0]; + return (int16_t)(p->vy); +} + +/** + * @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) +{ + mavlink_full_state_t *p = (mavlink_full_state_t *)&msg->payload[0]; + return (int16_t)(p->vz); +} + +/** + * @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) +{ + mavlink_full_state_t *p = (mavlink_full_state_t *)&msg->payload[0]; + return (int16_t)(p->xacc); +} + +/** + * @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) +{ + mavlink_full_state_t *p = (mavlink_full_state_t *)&msg->payload[0]; + return (int16_t)(p->yacc); +} + +/** + * @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) +{ + mavlink_full_state_t *p = (mavlink_full_state_t *)&msg->payload[0]; + return (int16_t)(p->zacc); +} + +/** + * @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) +{ + memcpy( full_state, msg->payload, sizeof(mavlink_full_state_t)); +} diff --git a/thirdParty/mavlink/include/common/mavlink_msg_global_position.h b/thirdParty/mavlink/include/common/mavlink_msg_global_position.h index 9d27f7714bcacb8c87d7706ee67a6cb2ecaea5bd..26e2d8a133d1a19498456f38a20484e062a18552 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_global_position.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_global_position.h @@ -1,6 +1,8 @@ // MESSAGE GLOBAL_POSITION PACKING #define MAVLINK_MSG_ID_GLOBAL_POSITION 33 +#define MAVLINK_MSG_ID_GLOBAL_POSITION_LEN 32 +#define MAVLINK_MSG_33_LEN 32 typedef struct __mavlink_global_position_t { @@ -14,8 +16,6 @@ typedef struct __mavlink_global_position_t } mavlink_global_position_t; - - /** * @brief Pack a global_position message * @param system_id ID of this system @@ -33,18 +33,18 @@ typedef struct __mavlink_global_position_t */ static inline uint16_t mavlink_msg_global_position_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t usec, float lat, float lon, float alt, float vx, float vy, float vz) { - uint16_t i = 0; + mavlink_global_position_t *p = (mavlink_global_position_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION; - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since unix epoch) - i += put_float_by_index(lat, i, msg->payload); // Latitude, in degrees - i += put_float_by_index(lon, i, msg->payload); // Longitude, in degrees - i += put_float_by_index(alt, i, msg->payload); // Absolute altitude, in meters - i += put_float_by_index(vx, i, msg->payload); // X Speed (in Latitude direction, positive: going north) - i += put_float_by_index(vy, i, msg->payload); // Y Speed (in Longitude direction, positive: going east) - i += put_float_by_index(vz, i, msg->payload); // Z Speed (in Altitude direction, positive: going up) + p->usec = usec; // uint64_t:Timestamp (microseconds since unix epoch) + p->lat = lat; // float:Latitude, in degrees + p->lon = lon; // float:Longitude, in degrees + p->alt = alt; // float:Absolute altitude, in meters + p->vx = vx; // float:X Speed (in Latitude direction, positive: going north) + p->vy = vy; // float:Y Speed (in Longitude direction, positive: going east) + p->vz = vz; // float:Z Speed (in Altitude direction, positive: going up) - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GLOBAL_POSITION_LEN); } /** @@ -64,18 +64,18 @@ static inline uint16_t mavlink_msg_global_position_pack(uint8_t system_id, uint8 */ static inline uint16_t mavlink_msg_global_position_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t usec, float lat, float lon, float alt, float vx, float vy, float vz) { - uint16_t i = 0; + mavlink_global_position_t *p = (mavlink_global_position_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION; - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since unix epoch) - i += put_float_by_index(lat, i, msg->payload); // Latitude, in degrees - i += put_float_by_index(lon, i, msg->payload); // Longitude, in degrees - i += put_float_by_index(alt, i, msg->payload); // Absolute altitude, in meters - i += put_float_by_index(vx, i, msg->payload); // X Speed (in Latitude direction, positive: going north) - i += put_float_by_index(vy, i, msg->payload); // Y Speed (in Longitude direction, positive: going east) - i += put_float_by_index(vz, i, msg->payload); // Z Speed (in Altitude direction, positive: going up) + p->usec = usec; // uint64_t:Timestamp (microseconds since unix epoch) + p->lat = lat; // float:Latitude, in degrees + p->lon = lon; // float:Longitude, in degrees + p->alt = alt; // float:Absolute altitude, in meters + p->vx = vx; // float:X Speed (in Latitude direction, positive: going north) + p->vy = vy; // float:Y Speed (in Longitude direction, positive: going east) + p->vz = vz; // float:Z Speed (in Altitude direction, positive: going up) - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GLOBAL_POSITION_LEN); } /** @@ -103,13 +103,41 @@ static inline uint16_t mavlink_msg_global_position_encode(uint8_t system_id, uin * @param vy Y Speed (in Longitude direction, positive: going east) * @param vz Z Speed (in Altitude direction, positive: going up) */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_global_position_send(mavlink_channel_t chan, uint64_t usec, float lat, float lon, float alt, float vx, float vy, float vz) { - mavlink_message_t msg; - mavlink_msg_global_position_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, usec, lat, lon, alt, vx, vy, vz); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_global_position_t payload; + uint16_t checksum; + mavlink_global_position_t *p = &payload; + + p->usec = usec; // uint64_t:Timestamp (microseconds since unix epoch) + p->lat = lat; // float:Latitude, in degrees + p->lon = lon; // float:Longitude, in degrees + p->alt = alt; // float:Absolute altitude, in meters + p->vx = vx; // float:X Speed (in Latitude direction, positive: going north) + p->vy = vy; // float:Y Speed (in Longitude direction, positive: going east) + p->vz = vz; // float:Z Speed (in Altitude direction, positive: going up) + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_GLOBAL_POSITION_LEN; + hdr.msgid = MAVLINK_MSG_ID_GLOBAL_POSITION; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -122,16 +150,8 @@ static inline void mavlink_msg_global_position_send(mavlink_channel_t chan, uint */ static inline uint64_t mavlink_msg_global_position_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; + mavlink_global_position_t *p = (mavlink_global_position_t *)&msg->payload[0]; + return (uint64_t)(p->usec); } /** @@ -141,12 +161,8 @@ static inline uint64_t mavlink_msg_global_position_get_usec(const mavlink_messag */ static inline float mavlink_msg_global_position_get_lat(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; + mavlink_global_position_t *p = (mavlink_global_position_t *)&msg->payload[0]; + return (float)(p->lat); } /** @@ -156,12 +172,8 @@ static inline float mavlink_msg_global_position_get_lat(const mavlink_message_t* */ static inline float mavlink_msg_global_position_get_lon(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; + mavlink_global_position_t *p = (mavlink_global_position_t *)&msg->payload[0]; + return (float)(p->lon); } /** @@ -171,12 +183,8 @@ static inline float mavlink_msg_global_position_get_lon(const mavlink_message_t* */ static inline float mavlink_msg_global_position_get_alt(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; + mavlink_global_position_t *p = (mavlink_global_position_t *)&msg->payload[0]; + return (float)(p->alt); } /** @@ -186,12 +194,8 @@ static inline float mavlink_msg_global_position_get_alt(const mavlink_message_t* */ static inline float mavlink_msg_global_position_get_vx(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; + mavlink_global_position_t *p = (mavlink_global_position_t *)&msg->payload[0]; + return (float)(p->vx); } /** @@ -201,12 +205,8 @@ static inline float mavlink_msg_global_position_get_vx(const mavlink_message_t* */ static inline float mavlink_msg_global_position_get_vy(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; + mavlink_global_position_t *p = (mavlink_global_position_t *)&msg->payload[0]; + return (float)(p->vy); } /** @@ -216,12 +216,8 @@ static inline float mavlink_msg_global_position_get_vy(const mavlink_message_t* */ static inline float mavlink_msg_global_position_get_vz(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; + mavlink_global_position_t *p = (mavlink_global_position_t *)&msg->payload[0]; + return (float)(p->vz); } /** @@ -232,11 +228,5 @@ static inline float mavlink_msg_global_position_get_vz(const mavlink_message_t* */ static inline void mavlink_msg_global_position_decode(const mavlink_message_t* msg, mavlink_global_position_t* global_position) { - global_position->usec = mavlink_msg_global_position_get_usec(msg); - global_position->lat = mavlink_msg_global_position_get_lat(msg); - global_position->lon = mavlink_msg_global_position_get_lon(msg); - global_position->alt = mavlink_msg_global_position_get_alt(msg); - global_position->vx = mavlink_msg_global_position_get_vx(msg); - global_position->vy = mavlink_msg_global_position_get_vy(msg); - global_position->vz = mavlink_msg_global_position_get_vz(msg); + memcpy( global_position, msg->payload, sizeof(mavlink_global_position_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_global_position_int.h b/thirdParty/mavlink/include/common/mavlink_msg_global_position_int.h index 1b3277a35798fe947b90ac468baa6fd827d27124..9dbc636e20c3e61fb0c1eef22c085096a0f27177 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_global_position_int.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_global_position_int.h @@ -1,20 +1,21 @@ // MESSAGE GLOBAL_POSITION_INT PACKING #define MAVLINK_MSG_ID_GLOBAL_POSITION_INT 73 +#define MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN 20 +#define MAVLINK_MSG_73_LEN 20 typedef struct __mavlink_global_position_int_t { int32_t lat; ///< Latitude, expressed as * 1E7 int32_t lon; ///< Longitude, expressed as * 1E7 - int32_t alt; ///< Altitude in meters, expressed as * 1000 (millimeters) + int32_t alt; ///< Altitude in meters, expressed as * 1000 (millimeters), above MSL 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 + uint16_t hdg; ///< Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 } mavlink_global_position_int_t; - - /** * @brief Pack a global_position_int message * @param system_id ID of this system @@ -23,25 +24,27 @@ typedef struct __mavlink_global_position_int_t * * @param lat Latitude, expressed as * 1E7 * @param lon Longitude, expressed as * 1E7 - * @param alt Altitude in meters, expressed as * 1000 (millimeters) + * @param alt Altitude in meters, expressed as * 1000 (millimeters), above MSL * @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 hdg Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_global_position_int_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz) +static inline uint16_t mavlink_msg_global_position_int_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, uint16_t hdg) { - uint16_t i = 0; + mavlink_global_position_int_t *p = (mavlink_global_position_int_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_INT; - 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 + p->lat = lat; // int32_t:Latitude, expressed as * 1E7 + p->lon = lon; // int32_t:Longitude, expressed as * 1E7 + p->alt = alt; // int32_t:Altitude in meters, expressed as * 1000 (millimeters), above MSL + p->vx = vx; // int16_t:Ground X Speed (Latitude), expressed as m/s * 100 + p->vy = vy; // int16_t:Ground Y Speed (Longitude), expressed as m/s * 100 + p->vz = vz; // int16_t:Ground Z Speed (Altitude), expressed as m/s * 100 + p->hdg = hdg; // uint16_t:Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN); } /** @@ -52,25 +55,27 @@ static inline uint16_t mavlink_msg_global_position_int_pack(uint8_t system_id, u * @param msg The MAVLink message to compress the data into * @param lat Latitude, expressed as * 1E7 * @param lon Longitude, expressed as * 1E7 - * @param alt Altitude in meters, expressed as * 1000 (millimeters) + * @param alt Altitude in meters, expressed as * 1000 (millimeters), above MSL * @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 hdg Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_global_position_int_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz) +static inline uint16_t mavlink_msg_global_position_int_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, uint16_t hdg) { - uint16_t i = 0; + mavlink_global_position_int_t *p = (mavlink_global_position_int_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_INT; - 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 + p->lat = lat; // int32_t:Latitude, expressed as * 1E7 + p->lon = lon; // int32_t:Longitude, expressed as * 1E7 + p->alt = alt; // int32_t:Altitude in meters, expressed as * 1000 (millimeters), above MSL + p->vx = vx; // int16_t:Ground X Speed (Latitude), expressed as m/s * 100 + p->vy = vy; // int16_t:Ground Y Speed (Longitude), expressed as m/s * 100 + p->vz = vz; // int16_t:Ground Z Speed (Altitude), expressed as m/s * 100 + p->hdg = hdg; // uint16_t:Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN); } /** @@ -83,7 +88,7 @@ static inline uint16_t mavlink_msg_global_position_int_pack_chan(uint8_t system_ */ static inline uint16_t mavlink_msg_global_position_int_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_global_position_int_t* global_position_int) { - return mavlink_msg_global_position_int_pack(system_id, component_id, msg, global_position_int->lat, global_position_int->lon, global_position_int->alt, global_position_int->vx, global_position_int->vy, global_position_int->vz); + return mavlink_msg_global_position_int_pack(system_id, component_id, msg, global_position_int->lat, global_position_int->lon, global_position_int->alt, global_position_int->vx, global_position_int->vy, global_position_int->vz, global_position_int->hdg); } /** @@ -92,18 +97,47 @@ static inline uint16_t mavlink_msg_global_position_int_encode(uint8_t system_id, * * @param lat Latitude, expressed as * 1E7 * @param lon Longitude, expressed as * 1E7 - * @param alt Altitude in meters, expressed as * 1000 (millimeters) + * @param alt Altitude in meters, expressed as * 1000 (millimeters), above MSL * @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 hdg Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_global_position_int_send(mavlink_channel_t chan, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz) + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS +static inline void mavlink_msg_global_position_int_send(mavlink_channel_t chan, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, uint16_t hdg) { - mavlink_message_t msg; - mavlink_msg_global_position_int_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, lat, lon, alt, vx, vy, vz); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_global_position_int_t payload; + uint16_t checksum; + mavlink_global_position_int_t *p = &payload; + + p->lat = lat; // int32_t:Latitude, expressed as * 1E7 + p->lon = lon; // int32_t:Longitude, expressed as * 1E7 + p->alt = alt; // int32_t:Altitude in meters, expressed as * 1000 (millimeters), above MSL + p->vx = vx; // int16_t:Ground X Speed (Latitude), expressed as m/s * 100 + p->vy = vy; // int16_t:Ground Y Speed (Longitude), expressed as m/s * 100 + p->vz = vz; // int16_t:Ground Z Speed (Altitude), expressed as m/s * 100 + p->hdg = hdg; // uint16_t:Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN; + hdr.msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_INT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -116,12 +150,8 @@ static inline void mavlink_msg_global_position_int_send(mavlink_channel_t chan, */ static inline int32_t mavlink_msg_global_position_int_get_lat(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 (int32_t)r.i; + mavlink_global_position_int_t *p = (mavlink_global_position_int_t *)&msg->payload[0]; + return (int32_t)(p->lat); } /** @@ -131,27 +161,19 @@ static inline int32_t mavlink_msg_global_position_int_get_lat(const mavlink_mess */ static inline int32_t mavlink_msg_global_position_int_get_lon(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(int32_t))[0]; - r.b[2] = (msg->payload+sizeof(int32_t))[1]; - r.b[1] = (msg->payload+sizeof(int32_t))[2]; - r.b[0] = (msg->payload+sizeof(int32_t))[3]; - return (int32_t)r.i; + mavlink_global_position_int_t *p = (mavlink_global_position_int_t *)&msg->payload[0]; + return (int32_t)(p->lon); } /** * @brief Get field alt from global_position_int message * - * @return Altitude in meters, expressed as * 1000 (millimeters) + * @return Altitude in meters, expressed as * 1000 (millimeters), above MSL */ static inline int32_t mavlink_msg_global_position_int_get_alt(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(int32_t)+sizeof(int32_t))[0]; - r.b[2] = (msg->payload+sizeof(int32_t)+sizeof(int32_t))[1]; - r.b[1] = (msg->payload+sizeof(int32_t)+sizeof(int32_t))[2]; - r.b[0] = (msg->payload+sizeof(int32_t)+sizeof(int32_t))[3]; - return (int32_t)r.i; + mavlink_global_position_int_t *p = (mavlink_global_position_int_t *)&msg->payload[0]; + return (int32_t)(p->alt); } /** @@ -161,10 +183,8 @@ static inline int32_t mavlink_msg_global_position_int_get_alt(const mavlink_mess */ static inline int16_t mavlink_msg_global_position_int_get_vx(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t))[0]; - r.b[0] = (msg->payload+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t))[1]; - return (int16_t)r.s; + mavlink_global_position_int_t *p = (mavlink_global_position_int_t *)&msg->payload[0]; + return (int16_t)(p->vx); } /** @@ -174,10 +194,8 @@ static inline int16_t mavlink_msg_global_position_int_get_vx(const mavlink_messa */ static inline int16_t mavlink_msg_global_position_int_get_vy(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_global_position_int_t *p = (mavlink_global_position_int_t *)&msg->payload[0]; + return (int16_t)(p->vy); } /** @@ -187,10 +205,19 @@ static inline int16_t mavlink_msg_global_position_int_get_vy(const mavlink_messa */ static inline int16_t mavlink_msg_global_position_int_get_vz(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_global_position_int_t *p = (mavlink_global_position_int_t *)&msg->payload[0]; + return (int16_t)(p->vz); +} + +/** + * @brief Get field hdg from global_position_int message + * + * @return Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + */ +static inline uint16_t mavlink_msg_global_position_int_get_hdg(const mavlink_message_t* msg) +{ + mavlink_global_position_int_t *p = (mavlink_global_position_int_t *)&msg->payload[0]; + return (uint16_t)(p->hdg); } /** @@ -201,10 +228,5 @@ static inline int16_t mavlink_msg_global_position_int_get_vz(const mavlink_messa */ static inline void mavlink_msg_global_position_int_decode(const mavlink_message_t* msg, mavlink_global_position_int_t* global_position_int) { - global_position_int->lat = mavlink_msg_global_position_int_get_lat(msg); - global_position_int->lon = mavlink_msg_global_position_int_get_lon(msg); - global_position_int->alt = mavlink_msg_global_position_int_get_alt(msg); - global_position_int->vx = mavlink_msg_global_position_int_get_vx(msg); - global_position_int->vy = mavlink_msg_global_position_int_get_vy(msg); - global_position_int->vz = mavlink_msg_global_position_int_get_vz(msg); + memcpy( global_position_int, msg->payload, sizeof(mavlink_global_position_int_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_gps_local_origin_set.h b/thirdParty/mavlink/include/common/mavlink_msg_gps_local_origin_set.h index 156252854157547196b2fd439aa83d0aa3e41026..831fc696895003866a515a308749baf8d55d0029 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_gps_local_origin_set.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_gps_local_origin_set.h @@ -1,6 +1,8 @@ // MESSAGE GPS_LOCAL_ORIGIN_SET PACKING #define MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET 49 +#define MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET_LEN 12 +#define MAVLINK_MSG_49_LEN 12 typedef struct __mavlink_gps_local_origin_set_t { @@ -10,8 +12,6 @@ typedef struct __mavlink_gps_local_origin_set_t } mavlink_gps_local_origin_set_t; - - /** * @brief Pack a gps_local_origin_set message * @param system_id ID of this system @@ -25,14 +25,14 @@ typedef struct __mavlink_gps_local_origin_set_t */ static inline uint16_t mavlink_msg_gps_local_origin_set_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, int32_t latitude, int32_t longitude, int32_t altitude) { - uint16_t i = 0; + mavlink_gps_local_origin_set_t *p = (mavlink_gps_local_origin_set_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET; - i += put_int32_t_by_index(latitude, i, msg->payload); // Latitude (WGS84), expressed as * 1E7 - i += put_int32_t_by_index(longitude, i, msg->payload); // Longitude (WGS84), expressed as * 1E7 - i += put_int32_t_by_index(altitude, i, msg->payload); // Altitude(WGS84), expressed as * 1000 + p->latitude = latitude; // int32_t:Latitude (WGS84), expressed as * 1E7 + p->longitude = longitude; // int32_t:Longitude (WGS84), expressed as * 1E7 + p->altitude = altitude; // int32_t:Altitude(WGS84), expressed as * 1000 - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET_LEN); } /** @@ -48,14 +48,14 @@ static inline uint16_t mavlink_msg_gps_local_origin_set_pack(uint8_t system_id, */ static inline uint16_t mavlink_msg_gps_local_origin_set_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, int32_t latitude, int32_t longitude, int32_t altitude) { - uint16_t i = 0; + mavlink_gps_local_origin_set_t *p = (mavlink_gps_local_origin_set_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET; - i += put_int32_t_by_index(latitude, i, msg->payload); // Latitude (WGS84), expressed as * 1E7 - i += put_int32_t_by_index(longitude, i, msg->payload); // Longitude (WGS84), expressed as * 1E7 - i += put_int32_t_by_index(altitude, i, msg->payload); // Altitude(WGS84), expressed as * 1000 + p->latitude = latitude; // int32_t:Latitude (WGS84), expressed as * 1E7 + p->longitude = longitude; // int32_t:Longitude (WGS84), expressed as * 1E7 + p->altitude = altitude; // int32_t:Altitude(WGS84), expressed as * 1000 - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET_LEN); } /** @@ -79,13 +79,37 @@ static inline uint16_t mavlink_msg_gps_local_origin_set_encode(uint8_t system_id * @param longitude Longitude (WGS84), expressed as * 1E7 * @param altitude Altitude(WGS84), expressed as * 1000 */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_gps_local_origin_set_send(mavlink_channel_t chan, int32_t latitude, int32_t longitude, int32_t altitude) { - mavlink_message_t msg; - mavlink_msg_gps_local_origin_set_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, latitude, longitude, altitude); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_gps_local_origin_set_t payload; + uint16_t checksum; + mavlink_gps_local_origin_set_t *p = &payload; + + p->latitude = latitude; // int32_t:Latitude (WGS84), expressed as * 1E7 + p->longitude = longitude; // int32_t:Longitude (WGS84), expressed as * 1E7 + p->altitude = altitude; // int32_t:Altitude(WGS84), expressed as * 1000 + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET_LEN; + hdr.msgid = MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -98,12 +122,8 @@ static inline void mavlink_msg_gps_local_origin_set_send(mavlink_channel_t chan, */ static inline int32_t mavlink_msg_gps_local_origin_set_get_latitude(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 (int32_t)r.i; + mavlink_gps_local_origin_set_t *p = (mavlink_gps_local_origin_set_t *)&msg->payload[0]; + return (int32_t)(p->latitude); } /** @@ -113,12 +133,8 @@ static inline int32_t mavlink_msg_gps_local_origin_set_get_latitude(const mavlin */ static inline int32_t mavlink_msg_gps_local_origin_set_get_longitude(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(int32_t))[0]; - r.b[2] = (msg->payload+sizeof(int32_t))[1]; - r.b[1] = (msg->payload+sizeof(int32_t))[2]; - r.b[0] = (msg->payload+sizeof(int32_t))[3]; - return (int32_t)r.i; + mavlink_gps_local_origin_set_t *p = (mavlink_gps_local_origin_set_t *)&msg->payload[0]; + return (int32_t)(p->longitude); } /** @@ -128,12 +144,8 @@ static inline int32_t mavlink_msg_gps_local_origin_set_get_longitude(const mavli */ static inline int32_t mavlink_msg_gps_local_origin_set_get_altitude(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(int32_t)+sizeof(int32_t))[0]; - r.b[2] = (msg->payload+sizeof(int32_t)+sizeof(int32_t))[1]; - r.b[1] = (msg->payload+sizeof(int32_t)+sizeof(int32_t))[2]; - r.b[0] = (msg->payload+sizeof(int32_t)+sizeof(int32_t))[3]; - return (int32_t)r.i; + mavlink_gps_local_origin_set_t *p = (mavlink_gps_local_origin_set_t *)&msg->payload[0]; + return (int32_t)(p->altitude); } /** @@ -144,7 +156,5 @@ static inline int32_t mavlink_msg_gps_local_origin_set_get_altitude(const mavlin */ static inline void mavlink_msg_gps_local_origin_set_decode(const mavlink_message_t* msg, mavlink_gps_local_origin_set_t* gps_local_origin_set) { - gps_local_origin_set->latitude = mavlink_msg_gps_local_origin_set_get_latitude(msg); - gps_local_origin_set->longitude = mavlink_msg_gps_local_origin_set_get_longitude(msg); - gps_local_origin_set->altitude = mavlink_msg_gps_local_origin_set_get_altitude(msg); + memcpy( gps_local_origin_set, msg->payload, sizeof(mavlink_gps_local_origin_set_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_gps_raw.h b/thirdParty/mavlink/include/common/mavlink_msg_gps_raw.h index 52d33a23b23a4aca8fab0ce8c9b1308dc3104e72..0ee5e0f3f7ea93742f32ce2655cc7dd90fd5675b 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_gps_raw.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_gps_raw.h @@ -1,11 +1,12 @@ // MESSAGE GPS_RAW PACKING #define MAVLINK_MSG_ID_GPS_RAW 32 +#define MAVLINK_MSG_ID_GPS_RAW_LEN 38 +#define MAVLINK_MSG_32_LEN 38 typedef struct __mavlink_gps_raw_t { uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) - uint8_t fix_type; ///< 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. float lat; ///< Latitude in degrees float lon; ///< Longitude in degrees float alt; ///< Altitude in meters @@ -13,11 +14,11 @@ typedef struct __mavlink_gps_raw_t float epv; ///< GPS VDOP float v; ///< GPS ground speed float hdg; ///< Compass heading in degrees, 0..360 degrees + uint8_t fix_type; ///< 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. + uint8_t satellites_visible; ///< Number of satellites visible } mavlink_gps_raw_t; - - /** * @brief Pack a gps_raw message * @param system_id ID of this system @@ -33,24 +34,26 @@ typedef struct __mavlink_gps_raw_t * @param epv GPS VDOP * @param v GPS ground speed * @param hdg Compass heading in degrees, 0..360 degrees + * @param satellites_visible Number of satellites visible * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_gps_raw_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t usec, uint8_t fix_type, float lat, float lon, float alt, float eph, float epv, float v, float hdg) +static inline uint16_t mavlink_msg_gps_raw_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t usec, uint8_t fix_type, float lat, float lon, float alt, float eph, float epv, float v, float hdg, uint8_t satellites_visible) { - uint16_t i = 0; + mavlink_gps_raw_t *p = (mavlink_gps_raw_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GPS_RAW; - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - i += put_uint8_t_by_index(fix_type, i, msg->payload); // 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. - i += put_float_by_index(lat, i, msg->payload); // Latitude in degrees - i += put_float_by_index(lon, i, msg->payload); // Longitude in degrees - i += put_float_by_index(alt, i, msg->payload); // Altitude in meters - i += put_float_by_index(eph, i, msg->payload); // GPS HDOP - i += put_float_by_index(epv, i, msg->payload); // GPS VDOP - i += put_float_by_index(v, i, msg->payload); // GPS ground speed - i += put_float_by_index(hdg, i, msg->payload); // Compass heading in degrees, 0..360 degrees + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->fix_type = fix_type; // uint8_t: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. + p->lat = lat; // float:Latitude in degrees + p->lon = lon; // float:Longitude in degrees + p->alt = alt; // float:Altitude in meters + p->eph = eph; // float:GPS HDOP + p->epv = epv; // float:GPS VDOP + p->v = v; // float:GPS ground speed + p->hdg = hdg; // float:Compass heading in degrees, 0..360 degrees + p->satellites_visible = satellites_visible; // uint8_t:Number of satellites visible - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_RAW_LEN); } /** @@ -68,24 +71,26 @@ static inline uint16_t mavlink_msg_gps_raw_pack(uint8_t system_id, uint8_t compo * @param epv GPS VDOP * @param v GPS ground speed * @param hdg Compass heading in degrees, 0..360 degrees + * @param satellites_visible Number of satellites visible * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_gps_raw_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t usec, uint8_t fix_type, float lat, float lon, float alt, float eph, float epv, float v, float hdg) +static inline uint16_t mavlink_msg_gps_raw_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t usec, uint8_t fix_type, float lat, float lon, float alt, float eph, float epv, float v, float hdg, uint8_t satellites_visible) { - uint16_t i = 0; + mavlink_gps_raw_t *p = (mavlink_gps_raw_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GPS_RAW; - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - i += put_uint8_t_by_index(fix_type, i, msg->payload); // 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. - i += put_float_by_index(lat, i, msg->payload); // Latitude in degrees - i += put_float_by_index(lon, i, msg->payload); // Longitude in degrees - i += put_float_by_index(alt, i, msg->payload); // Altitude in meters - i += put_float_by_index(eph, i, msg->payload); // GPS HDOP - i += put_float_by_index(epv, i, msg->payload); // GPS VDOP - i += put_float_by_index(v, i, msg->payload); // GPS ground speed - i += put_float_by_index(hdg, i, msg->payload); // Compass heading in degrees, 0..360 degrees + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->fix_type = fix_type; // uint8_t: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. + p->lat = lat; // float:Latitude in degrees + p->lon = lon; // float:Longitude in degrees + p->alt = alt; // float:Altitude in meters + p->eph = eph; // float:GPS HDOP + p->epv = epv; // float:GPS VDOP + p->v = v; // float:GPS ground speed + p->hdg = hdg; // float:Compass heading in degrees, 0..360 degrees + p->satellites_visible = satellites_visible; // uint8_t:Number of satellites visible - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_RAW_LEN); } /** @@ -98,7 +103,7 @@ static inline uint16_t mavlink_msg_gps_raw_pack_chan(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_gps_raw_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gps_raw_t* gps_raw) { - return mavlink_msg_gps_raw_pack(system_id, component_id, msg, gps_raw->usec, gps_raw->fix_type, gps_raw->lat, gps_raw->lon, gps_raw->alt, gps_raw->eph, gps_raw->epv, gps_raw->v, gps_raw->hdg); + return mavlink_msg_gps_raw_pack(system_id, component_id, msg, gps_raw->usec, gps_raw->fix_type, gps_raw->lat, gps_raw->lon, gps_raw->alt, gps_raw->eph, gps_raw->epv, gps_raw->v, gps_raw->hdg, gps_raw->satellites_visible); } /** @@ -114,14 +119,46 @@ static inline uint16_t mavlink_msg_gps_raw_encode(uint8_t system_id, uint8_t com * @param epv GPS VDOP * @param v GPS ground speed * @param hdg Compass heading in degrees, 0..360 degrees + * @param satellites_visible Number of satellites visible */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_gps_raw_send(mavlink_channel_t chan, uint64_t usec, uint8_t fix_type, float lat, float lon, float alt, float eph, float epv, float v, float hdg) + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS +static inline void mavlink_msg_gps_raw_send(mavlink_channel_t chan, uint64_t usec, uint8_t fix_type, float lat, float lon, float alt, float eph, float epv, float v, float hdg, uint8_t satellites_visible) { - mavlink_message_t msg; - mavlink_msg_gps_raw_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, usec, fix_type, lat, lon, alt, eph, epv, v, hdg); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_gps_raw_t payload; + uint16_t checksum; + mavlink_gps_raw_t *p = &payload; + + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->fix_type = fix_type; // uint8_t: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. + p->lat = lat; // float:Latitude in degrees + p->lon = lon; // float:Longitude in degrees + p->alt = alt; // float:Altitude in meters + p->eph = eph; // float:GPS HDOP + p->epv = epv; // float:GPS VDOP + p->v = v; // float:GPS ground speed + p->hdg = hdg; // float:Compass heading in degrees, 0..360 degrees + p->satellites_visible = satellites_visible; // uint8_t:Number of satellites visible + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_GPS_RAW_LEN; + hdr.msgid = MAVLINK_MSG_ID_GPS_RAW; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -134,16 +171,8 @@ static inline void mavlink_msg_gps_raw_send(mavlink_channel_t chan, uint64_t use */ static inline uint64_t mavlink_msg_gps_raw_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; + mavlink_gps_raw_t *p = (mavlink_gps_raw_t *)&msg->payload[0]; + return (uint64_t)(p->usec); } /** @@ -153,7 +182,8 @@ static inline uint64_t mavlink_msg_gps_raw_get_usec(const mavlink_message_t* msg */ static inline uint8_t mavlink_msg_gps_raw_get_fix_type(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint64_t))[0]; + mavlink_gps_raw_t *p = (mavlink_gps_raw_t *)&msg->payload[0]; + return (uint8_t)(p->fix_type); } /** @@ -163,12 +193,8 @@ static inline uint8_t mavlink_msg_gps_raw_get_fix_type(const mavlink_message_t* */ static inline float mavlink_msg_gps_raw_get_lat(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[3]; - return (float)r.f; + mavlink_gps_raw_t *p = (mavlink_gps_raw_t *)&msg->payload[0]; + return (float)(p->lat); } /** @@ -178,12 +204,8 @@ static inline float mavlink_msg_gps_raw_get_lat(const mavlink_message_t* msg) */ static inline float mavlink_msg_gps_raw_get_lon(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float))[3]; - return (float)r.f; + mavlink_gps_raw_t *p = (mavlink_gps_raw_t *)&msg->payload[0]; + return (float)(p->lon); } /** @@ -193,12 +215,8 @@ static inline float mavlink_msg_gps_raw_get_lon(const mavlink_message_t* msg) */ static inline float mavlink_msg_gps_raw_get_alt(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_gps_raw_t *p = (mavlink_gps_raw_t *)&msg->payload[0]; + return (float)(p->alt); } /** @@ -208,12 +226,8 @@ static inline float mavlink_msg_gps_raw_get_alt(const mavlink_message_t* msg) */ static inline float mavlink_msg_gps_raw_get_eph(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_gps_raw_t *p = (mavlink_gps_raw_t *)&msg->payload[0]; + return (float)(p->eph); } /** @@ -223,12 +237,8 @@ static inline float mavlink_msg_gps_raw_get_eph(const mavlink_message_t* msg) */ static inline float mavlink_msg_gps_raw_get_epv(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_gps_raw_t *p = (mavlink_gps_raw_t *)&msg->payload[0]; + return (float)(p->epv); } /** @@ -238,12 +248,8 @@ static inline float mavlink_msg_gps_raw_get_epv(const mavlink_message_t* msg) */ static inline float mavlink_msg_gps_raw_get_v(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_gps_raw_t *p = (mavlink_gps_raw_t *)&msg->payload[0]; + return (float)(p->v); } /** @@ -253,12 +259,19 @@ static inline float mavlink_msg_gps_raw_get_v(const mavlink_message_t* msg) */ static inline float mavlink_msg_gps_raw_get_hdg(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_gps_raw_t *p = (mavlink_gps_raw_t *)&msg->payload[0]; + return (float)(p->hdg); +} + +/** + * @brief Get field satellites_visible from gps_raw message + * + * @return Number of satellites visible + */ +static inline uint8_t mavlink_msg_gps_raw_get_satellites_visible(const mavlink_message_t* msg) +{ + mavlink_gps_raw_t *p = (mavlink_gps_raw_t *)&msg->payload[0]; + return (uint8_t)(p->satellites_visible); } /** @@ -269,13 +282,5 @@ static inline float mavlink_msg_gps_raw_get_hdg(const mavlink_message_t* msg) */ static inline void mavlink_msg_gps_raw_decode(const mavlink_message_t* msg, mavlink_gps_raw_t* gps_raw) { - gps_raw->usec = mavlink_msg_gps_raw_get_usec(msg); - gps_raw->fix_type = mavlink_msg_gps_raw_get_fix_type(msg); - gps_raw->lat = mavlink_msg_gps_raw_get_lat(msg); - gps_raw->lon = mavlink_msg_gps_raw_get_lon(msg); - gps_raw->alt = mavlink_msg_gps_raw_get_alt(msg); - gps_raw->eph = mavlink_msg_gps_raw_get_eph(msg); - gps_raw->epv = mavlink_msg_gps_raw_get_epv(msg); - gps_raw->v = mavlink_msg_gps_raw_get_v(msg); - gps_raw->hdg = mavlink_msg_gps_raw_get_hdg(msg); + memcpy( gps_raw, msg->payload, sizeof(mavlink_gps_raw_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_gps_raw_int.h b/thirdParty/mavlink/include/common/mavlink_msg_gps_raw_int.h index b49498febc6519f905ade7271b9c607910488511..c0cf3bba93dcddcc77e0d188317c17fa35e38da7 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_gps_raw_int.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_gps_raw_int.h @@ -1,23 +1,24 @@ // MESSAGE GPS_RAW_INT PACKING #define MAVLINK_MSG_ID_GPS_RAW_INT 25 +#define MAVLINK_MSG_ID_GPS_RAW_INT_LEN 30 +#define MAVLINK_MSG_25_LEN 30 typedef struct __mavlink_gps_raw_int_t { uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) - uint8_t fix_type; ///< 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. int32_t lat; ///< Latitude in 1E7 degrees int32_t lon; ///< Longitude in 1E7 degrees - int32_t alt; ///< Altitude in 1E3 meters (millimeters) - float eph; ///< GPS HDOP - float epv; ///< GPS VDOP - float v; ///< GPS ground speed (m/s) - float hdg; ///< Compass heading in degrees, 0..360 degrees + int32_t alt; ///< Altitude in 1E3 meters (millimeters) above MSL + uint16_t eph; ///< GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + uint16_t epv; ///< GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + uint16_t vel; ///< GPS ground speed (m/s * 100). If unknown, set to: 65535 + uint16_t hdg; ///< Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + uint8_t fix_type; ///< 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. + uint8_t satellites_visible; ///< Number of satellites visible. If unknown, set to 255 } mavlink_gps_raw_int_t; - - /** * @brief Pack a gps_raw_int message * @param system_id ID of this system @@ -28,29 +29,31 @@ typedef struct __mavlink_gps_raw_int_t * @param fix_type 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. * @param lat Latitude in 1E7 degrees * @param lon Longitude in 1E7 degrees - * @param alt Altitude in 1E3 meters (millimeters) - * @param eph GPS HDOP - * @param epv GPS VDOP - * @param v GPS ground speed (m/s) - * @param hdg Compass heading in degrees, 0..360 degrees + * @param alt Altitude in 1E3 meters (millimeters) above MSL + * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + * @param epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + * @param vel GPS ground speed (m/s * 100). If unknown, set to: 65535 + * @param hdg Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + * @param satellites_visible Number of satellites visible. If unknown, set to 255 * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_gps_raw_int_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, float eph, float epv, float v, float hdg) +static inline uint16_t mavlink_msg_gps_raw_int_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t hdg, uint8_t satellites_visible) { - uint16_t i = 0; + mavlink_gps_raw_int_t *p = (mavlink_gps_raw_int_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GPS_RAW_INT; - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - i += put_uint8_t_by_index(fix_type, i, msg->payload); // 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. - i += put_int32_t_by_index(lat, i, msg->payload); // Latitude in 1E7 degrees - i += put_int32_t_by_index(lon, i, msg->payload); // Longitude in 1E7 degrees - i += put_int32_t_by_index(alt, i, msg->payload); // Altitude in 1E3 meters (millimeters) - i += put_float_by_index(eph, i, msg->payload); // GPS HDOP - i += put_float_by_index(epv, i, msg->payload); // GPS VDOP - i += put_float_by_index(v, i, msg->payload); // GPS ground speed (m/s) - i += put_float_by_index(hdg, i, msg->payload); // Compass heading in degrees, 0..360 degrees + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->fix_type = fix_type; // uint8_t: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. + p->lat = lat; // int32_t:Latitude in 1E7 degrees + p->lon = lon; // int32_t:Longitude in 1E7 degrees + p->alt = alt; // int32_t:Altitude in 1E3 meters (millimeters) above MSL + p->eph = eph; // uint16_t:GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + p->epv = epv; // uint16_t:GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + p->vel = vel; // uint16_t:GPS ground speed (m/s * 100). If unknown, set to: 65535 + p->hdg = hdg; // uint16_t:Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + p->satellites_visible = satellites_visible; // uint8_t:Number of satellites visible. If unknown, set to 255 - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_RAW_INT_LEN); } /** @@ -63,29 +66,31 @@ static inline uint16_t mavlink_msg_gps_raw_int_pack(uint8_t system_id, uint8_t c * @param fix_type 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. * @param lat Latitude in 1E7 degrees * @param lon Longitude in 1E7 degrees - * @param alt Altitude in 1E3 meters (millimeters) - * @param eph GPS HDOP - * @param epv GPS VDOP - * @param v GPS ground speed (m/s) - * @param hdg Compass heading in degrees, 0..360 degrees + * @param alt Altitude in 1E3 meters (millimeters) above MSL + * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + * @param epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + * @param vel GPS ground speed (m/s * 100). If unknown, set to: 65535 + * @param hdg Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + * @param satellites_visible Number of satellites visible. If unknown, set to 255 * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_gps_raw_int_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, float eph, float epv, float v, float hdg) +static inline uint16_t mavlink_msg_gps_raw_int_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t hdg, uint8_t satellites_visible) { - uint16_t i = 0; + mavlink_gps_raw_int_t *p = (mavlink_gps_raw_int_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GPS_RAW_INT; - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - i += put_uint8_t_by_index(fix_type, i, msg->payload); // 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. - i += put_int32_t_by_index(lat, i, msg->payload); // Latitude in 1E7 degrees - i += put_int32_t_by_index(lon, i, msg->payload); // Longitude in 1E7 degrees - i += put_int32_t_by_index(alt, i, msg->payload); // Altitude in 1E3 meters (millimeters) - i += put_float_by_index(eph, i, msg->payload); // GPS HDOP - i += put_float_by_index(epv, i, msg->payload); // GPS VDOP - i += put_float_by_index(v, i, msg->payload); // GPS ground speed (m/s) - i += put_float_by_index(hdg, i, msg->payload); // Compass heading in degrees, 0..360 degrees + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->fix_type = fix_type; // uint8_t: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. + p->lat = lat; // int32_t:Latitude in 1E7 degrees + p->lon = lon; // int32_t:Longitude in 1E7 degrees + p->alt = alt; // int32_t:Altitude in 1E3 meters (millimeters) above MSL + p->eph = eph; // uint16_t:GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + p->epv = epv; // uint16_t:GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + p->vel = vel; // uint16_t:GPS ground speed (m/s * 100). If unknown, set to: 65535 + p->hdg = hdg; // uint16_t:Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + p->satellites_visible = satellites_visible; // uint8_t:Number of satellites visible. If unknown, set to 255 - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_RAW_INT_LEN); } /** @@ -98,7 +103,7 @@ static inline uint16_t mavlink_msg_gps_raw_int_pack_chan(uint8_t system_id, uint */ static inline uint16_t mavlink_msg_gps_raw_int_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gps_raw_int_t* gps_raw_int) { - return mavlink_msg_gps_raw_int_pack(system_id, component_id, msg, gps_raw_int->usec, gps_raw_int->fix_type, gps_raw_int->lat, gps_raw_int->lon, gps_raw_int->alt, gps_raw_int->eph, gps_raw_int->epv, gps_raw_int->v, gps_raw_int->hdg); + return mavlink_msg_gps_raw_int_pack(system_id, component_id, msg, gps_raw_int->usec, gps_raw_int->fix_type, gps_raw_int->lat, gps_raw_int->lon, gps_raw_int->alt, gps_raw_int->eph, gps_raw_int->epv, gps_raw_int->vel, gps_raw_int->hdg, gps_raw_int->satellites_visible); } /** @@ -109,19 +114,51 @@ static inline uint16_t mavlink_msg_gps_raw_int_encode(uint8_t system_id, uint8_t * @param fix_type 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. * @param lat Latitude in 1E7 degrees * @param lon Longitude in 1E7 degrees - * @param alt Altitude in 1E3 meters (millimeters) - * @param eph GPS HDOP - * @param epv GPS VDOP - * @param v GPS ground speed (m/s) - * @param hdg Compass heading in degrees, 0..360 degrees + * @param alt Altitude in 1E3 meters (millimeters) above MSL + * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + * @param epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + * @param vel GPS ground speed (m/s * 100). If unknown, set to: 65535 + * @param hdg Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + * @param satellites_visible Number of satellites visible. If unknown, set to 255 */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_gps_raw_int_send(mavlink_channel_t chan, uint64_t usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, float eph, float epv, float v, float hdg) + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS +static inline void mavlink_msg_gps_raw_int_send(mavlink_channel_t chan, uint64_t usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t hdg, uint8_t satellites_visible) { - mavlink_message_t msg; - mavlink_msg_gps_raw_int_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, usec, fix_type, lat, lon, alt, eph, epv, v, hdg); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_gps_raw_int_t payload; + uint16_t checksum; + mavlink_gps_raw_int_t *p = &payload; + + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->fix_type = fix_type; // uint8_t: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. + p->lat = lat; // int32_t:Latitude in 1E7 degrees + p->lon = lon; // int32_t:Longitude in 1E7 degrees + p->alt = alt; // int32_t:Altitude in 1E3 meters (millimeters) above MSL + p->eph = eph; // uint16_t:GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + p->epv = epv; // uint16_t:GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + p->vel = vel; // uint16_t:GPS ground speed (m/s * 100). If unknown, set to: 65535 + p->hdg = hdg; // uint16_t:Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + p->satellites_visible = satellites_visible; // uint8_t:Number of satellites visible. If unknown, set to 255 + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_GPS_RAW_INT_LEN; + hdr.msgid = MAVLINK_MSG_ID_GPS_RAW_INT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -134,16 +171,8 @@ static inline void mavlink_msg_gps_raw_int_send(mavlink_channel_t chan, uint64_t */ static inline uint64_t mavlink_msg_gps_raw_int_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; + mavlink_gps_raw_int_t *p = (mavlink_gps_raw_int_t *)&msg->payload[0]; + return (uint64_t)(p->usec); } /** @@ -153,7 +182,8 @@ static inline uint64_t mavlink_msg_gps_raw_int_get_usec(const mavlink_message_t* */ static inline uint8_t mavlink_msg_gps_raw_int_get_fix_type(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint64_t))[0]; + mavlink_gps_raw_int_t *p = (mavlink_gps_raw_int_t *)&msg->payload[0]; + return (uint8_t)(p->fix_type); } /** @@ -163,12 +193,8 @@ static inline uint8_t mavlink_msg_gps_raw_int_get_fix_type(const mavlink_message */ static inline int32_t mavlink_msg_gps_raw_int_get_lat(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[3]; - return (int32_t)r.i; + mavlink_gps_raw_int_t *p = (mavlink_gps_raw_int_t *)&msg->payload[0]; + return (int32_t)(p->lat); } /** @@ -178,87 +204,74 @@ static inline int32_t mavlink_msg_gps_raw_int_get_lat(const mavlink_message_t* m */ static inline int32_t mavlink_msg_gps_raw_int_get_lon(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t))[3]; - return (int32_t)r.i; + mavlink_gps_raw_int_t *p = (mavlink_gps_raw_int_t *)&msg->payload[0]; + return (int32_t)(p->lon); } /** * @brief Get field alt from gps_raw_int message * - * @return Altitude in 1E3 meters (millimeters) + * @return Altitude in 1E3 meters (millimeters) above MSL */ static inline int32_t mavlink_msg_gps_raw_int_get_alt(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t))[3]; - return (int32_t)r.i; + mavlink_gps_raw_int_t *p = (mavlink_gps_raw_int_t *)&msg->payload[0]; + return (int32_t)(p->alt); } /** * @brief Get field eph from gps_raw_int message * - * @return GPS HDOP + * @return GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 */ -static inline float mavlink_msg_gps_raw_int_get_eph(const mavlink_message_t* msg) +static inline uint16_t mavlink_msg_gps_raw_int_get_eph(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t))[3]; - return (float)r.f; + mavlink_gps_raw_int_t *p = (mavlink_gps_raw_int_t *)&msg->payload[0]; + return (uint16_t)(p->eph); } /** * @brief Get field epv from gps_raw_int message * - * @return GPS VDOP + * @return GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 */ -static inline float mavlink_msg_gps_raw_int_get_epv(const mavlink_message_t* msg) +static inline uint16_t mavlink_msg_gps_raw_int_get_epv(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float))[3]; - return (float)r.f; + mavlink_gps_raw_int_t *p = (mavlink_gps_raw_int_t *)&msg->payload[0]; + return (uint16_t)(p->epv); } /** - * @brief Get field v from gps_raw_int message + * @brief Get field vel from gps_raw_int message * - * @return GPS ground speed (m/s) + * @return GPS ground speed (m/s * 100). If unknown, set to: 65535 */ -static inline float mavlink_msg_gps_raw_int_get_v(const mavlink_message_t* msg) +static inline uint16_t mavlink_msg_gps_raw_int_get_vel(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_gps_raw_int_t *p = (mavlink_gps_raw_int_t *)&msg->payload[0]; + return (uint16_t)(p->vel); } /** * @brief Get field hdg from gps_raw_int message * - * @return Compass heading in degrees, 0..360 degrees + * @return Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + */ +static inline uint16_t mavlink_msg_gps_raw_int_get_hdg(const mavlink_message_t* msg) +{ + mavlink_gps_raw_int_t *p = (mavlink_gps_raw_int_t *)&msg->payload[0]; + return (uint16_t)(p->hdg); +} + +/** + * @brief Get field satellites_visible from gps_raw_int message + * + * @return Number of satellites visible. If unknown, set to 255 */ -static inline float mavlink_msg_gps_raw_int_get_hdg(const mavlink_message_t* msg) +static inline uint8_t mavlink_msg_gps_raw_int_get_satellites_visible(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_gps_raw_int_t *p = (mavlink_gps_raw_int_t *)&msg->payload[0]; + return (uint8_t)(p->satellites_visible); } /** @@ -269,13 +282,5 @@ static inline float mavlink_msg_gps_raw_int_get_hdg(const mavlink_message_t* msg */ static inline void mavlink_msg_gps_raw_int_decode(const mavlink_message_t* msg, mavlink_gps_raw_int_t* gps_raw_int) { - gps_raw_int->usec = mavlink_msg_gps_raw_int_get_usec(msg); - gps_raw_int->fix_type = mavlink_msg_gps_raw_int_get_fix_type(msg); - gps_raw_int->lat = mavlink_msg_gps_raw_int_get_lat(msg); - gps_raw_int->lon = mavlink_msg_gps_raw_int_get_lon(msg); - gps_raw_int->alt = mavlink_msg_gps_raw_int_get_alt(msg); - gps_raw_int->eph = mavlink_msg_gps_raw_int_get_eph(msg); - gps_raw_int->epv = mavlink_msg_gps_raw_int_get_epv(msg); - gps_raw_int->v = mavlink_msg_gps_raw_int_get_v(msg); - gps_raw_int->hdg = mavlink_msg_gps_raw_int_get_hdg(msg); + memcpy( gps_raw_int, msg->payload, sizeof(mavlink_gps_raw_int_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_gps_set_global_origin.h b/thirdParty/mavlink/include/common/mavlink_msg_gps_set_global_origin.h index 47ddc2e23b90a31fe6f6c3efa43a4e68735c0f4b..bccfdba02c9b6b66ba22022d5c12b6c77f7c1519 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_gps_set_global_origin.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_gps_set_global_origin.h @@ -1,19 +1,19 @@ // MESSAGE GPS_SET_GLOBAL_ORIGIN PACKING #define MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN 48 +#define MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN_LEN 14 +#define MAVLINK_MSG_48_LEN 14 typedef struct __mavlink_gps_set_global_origin_t { - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID int32_t latitude; ///< global position * 1E7 int32_t longitude; ///< global position * 1E7 int32_t altitude; ///< global position * 1000 + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID } mavlink_gps_set_global_origin_t; - - /** * @brief Pack a gps_set_global_origin message * @param system_id ID of this system @@ -29,16 +29,16 @@ typedef struct __mavlink_gps_set_global_origin_t */ static inline uint16_t mavlink_msg_gps_set_global_origin_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, int32_t latitude, int32_t longitude, int32_t altitude) { - uint16_t i = 0; + mavlink_gps_set_global_origin_t *p = (mavlink_gps_set_global_origin_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN; - 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_int32_t_by_index(latitude, i, msg->payload); // global position * 1E7 - i += put_int32_t_by_index(longitude, i, msg->payload); // global position * 1E7 - i += put_int32_t_by_index(altitude, i, msg->payload); // global position * 1000 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->latitude = latitude; // int32_t:global position * 1E7 + p->longitude = longitude; // int32_t:global position * 1E7 + p->altitude = altitude; // int32_t:global position * 1000 - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN_LEN); } /** @@ -56,16 +56,16 @@ static inline uint16_t mavlink_msg_gps_set_global_origin_pack(uint8_t system_id, */ static inline uint16_t mavlink_msg_gps_set_global_origin_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, int32_t latitude, int32_t longitude, int32_t altitude) { - uint16_t i = 0; + mavlink_gps_set_global_origin_t *p = (mavlink_gps_set_global_origin_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN; - 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_int32_t_by_index(latitude, i, msg->payload); // global position * 1E7 - i += put_int32_t_by_index(longitude, i, msg->payload); // global position * 1E7 - i += put_int32_t_by_index(altitude, i, msg->payload); // global position * 1000 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->latitude = latitude; // int32_t:global position * 1E7 + p->longitude = longitude; // int32_t:global position * 1E7 + p->altitude = altitude; // int32_t:global position * 1000 - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN_LEN); } /** @@ -91,13 +91,39 @@ static inline uint16_t mavlink_msg_gps_set_global_origin_encode(uint8_t system_i * @param longitude global position * 1E7 * @param altitude global position * 1000 */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_gps_set_global_origin_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int32_t latitude, int32_t longitude, int32_t altitude) { - mavlink_message_t msg; - mavlink_msg_gps_set_global_origin_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, latitude, longitude, altitude); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_gps_set_global_origin_t payload; + uint16_t checksum; + mavlink_gps_set_global_origin_t *p = &payload; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->latitude = latitude; // int32_t:global position * 1E7 + p->longitude = longitude; // int32_t:global position * 1E7 + p->altitude = altitude; // int32_t:global position * 1000 + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN_LEN; + hdr.msgid = MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -110,7 +136,8 @@ static inline void mavlink_msg_gps_set_global_origin_send(mavlink_channel_t chan */ static inline uint8_t mavlink_msg_gps_set_global_origin_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_gps_set_global_origin_t *p = (mavlink_gps_set_global_origin_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); } /** @@ -120,7 +147,8 @@ static inline uint8_t mavlink_msg_gps_set_global_origin_get_target_system(const */ static inline uint8_t mavlink_msg_gps_set_global_origin_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_gps_set_global_origin_t *p = (mavlink_gps_set_global_origin_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); } /** @@ -130,12 +158,8 @@ static inline uint8_t mavlink_msg_gps_set_global_origin_get_target_component(con */ static inline int32_t mavlink_msg_gps_set_global_origin_get_latitude(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 (int32_t)r.i; + mavlink_gps_set_global_origin_t *p = (mavlink_gps_set_global_origin_t *)&msg->payload[0]; + return (int32_t)(p->latitude); } /** @@ -145,12 +169,8 @@ static inline int32_t mavlink_msg_gps_set_global_origin_get_latitude(const mavli */ static inline int32_t mavlink_msg_gps_set_global_origin_get_longitude(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(int32_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(int32_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(int32_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(int32_t))[3]; - return (int32_t)r.i; + mavlink_gps_set_global_origin_t *p = (mavlink_gps_set_global_origin_t *)&msg->payload[0]; + return (int32_t)(p->longitude); } /** @@ -160,12 +180,8 @@ static inline int32_t mavlink_msg_gps_set_global_origin_get_longitude(const mavl */ static inline int32_t mavlink_msg_gps_set_global_origin_get_altitude(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t))[3]; - return (int32_t)r.i; + mavlink_gps_set_global_origin_t *p = (mavlink_gps_set_global_origin_t *)&msg->payload[0]; + return (int32_t)(p->altitude); } /** @@ -176,9 +192,5 @@ static inline int32_t mavlink_msg_gps_set_global_origin_get_altitude(const mavli */ static inline void mavlink_msg_gps_set_global_origin_decode(const mavlink_message_t* msg, mavlink_gps_set_global_origin_t* gps_set_global_origin) { - gps_set_global_origin->target_system = mavlink_msg_gps_set_global_origin_get_target_system(msg); - gps_set_global_origin->target_component = mavlink_msg_gps_set_global_origin_get_target_component(msg); - gps_set_global_origin->latitude = mavlink_msg_gps_set_global_origin_get_latitude(msg); - gps_set_global_origin->longitude = mavlink_msg_gps_set_global_origin_get_longitude(msg); - gps_set_global_origin->altitude = mavlink_msg_gps_set_global_origin_get_altitude(msg); + memcpy( gps_set_global_origin, msg->payload, sizeof(mavlink_gps_set_global_origin_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_gps_status.h b/thirdParty/mavlink/include/common/mavlink_msg_gps_status.h index fd04a9a262f39d53f5a566d977a6112dbe9d6add..a974432d6d66d0877c791a78b91ed56f462e2156 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_gps_status.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_gps_status.h @@ -1,25 +1,25 @@ // MESSAGE GPS_STATUS PACKING #define MAVLINK_MSG_ID_GPS_STATUS 27 +#define MAVLINK_MSG_ID_GPS_STATUS_LEN 101 +#define MAVLINK_MSG_27_LEN 101 typedef struct __mavlink_gps_status_t { uint8_t satellites_visible; ///< Number of satellites visible - int8_t satellite_prn[20]; ///< Global satellite ID - int8_t satellite_used[20]; ///< 0: Satellite not used, 1: used for localization - int8_t satellite_elevation[20]; ///< Elevation (0: right on top of receiver, 90: on the horizon) of satellite - int8_t satellite_azimuth[20]; ///< Direction of satellite, 0: 0 deg, 255: 360 deg. - int8_t satellite_snr[20]; ///< Signal to noise ratio of satellite + uint8_t satellite_prn[20]; ///< Global satellite ID + uint8_t satellite_used[20]; ///< 0: Satellite not used, 1: used for localization + uint8_t satellite_elevation[20]; ///< Elevation (0: right on top of receiver, 90: on the horizon) of satellite + uint8_t satellite_azimuth[20]; ///< Direction of satellite, 0: 0 deg, 255: 360 deg. + uint8_t satellite_snr[20]; ///< Signal to noise ratio of satellite } mavlink_gps_status_t; - #define MAVLINK_MSG_GPS_STATUS_FIELD_SATELLITE_PRN_LEN 20 #define MAVLINK_MSG_GPS_STATUS_FIELD_SATELLITE_USED_LEN 20 #define MAVLINK_MSG_GPS_STATUS_FIELD_SATELLITE_ELEVATION_LEN 20 #define MAVLINK_MSG_GPS_STATUS_FIELD_SATELLITE_AZIMUTH_LEN 20 #define MAVLINK_MSG_GPS_STATUS_FIELD_SATELLITE_SNR_LEN 20 - /** * @brief Pack a gps_status message * @param system_id ID of this system @@ -34,19 +34,19 @@ typedef struct __mavlink_gps_status_t * @param satellite_snr Signal to noise ratio of satellite * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_gps_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t satellites_visible, const int8_t* satellite_prn, const int8_t* satellite_used, const int8_t* satellite_elevation, const int8_t* satellite_azimuth, const int8_t* satellite_snr) +static inline uint16_t mavlink_msg_gps_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t satellites_visible, const uint8_t* satellite_prn, const uint8_t* satellite_used, const uint8_t* satellite_elevation, const uint8_t* satellite_azimuth, const uint8_t* satellite_snr) { - uint16_t i = 0; + mavlink_gps_status_t *p = (mavlink_gps_status_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GPS_STATUS; - i += put_uint8_t_by_index(satellites_visible, i, msg->payload); // Number of satellites visible - i += put_array_by_index(satellite_prn, 20, i, msg->payload); // Global satellite ID - i += put_array_by_index(satellite_used, 20, i, msg->payload); // 0: Satellite not used, 1: used for localization - i += put_array_by_index(satellite_elevation, 20, i, msg->payload); // Elevation (0: right on top of receiver, 90: on the horizon) of satellite - i += put_array_by_index(satellite_azimuth, 20, i, msg->payload); // Direction of satellite, 0: 0 deg, 255: 360 deg. - i += put_array_by_index(satellite_snr, 20, i, msg->payload); // Signal to noise ratio of satellite + p->satellites_visible = satellites_visible; // uint8_t:Number of satellites visible + memcpy(p->satellite_prn, satellite_prn, sizeof(p->satellite_prn)); // uint8_t[20]:Global satellite ID + memcpy(p->satellite_used, satellite_used, sizeof(p->satellite_used)); // uint8_t[20]:0: Satellite not used, 1: used for localization + memcpy(p->satellite_elevation, satellite_elevation, sizeof(p->satellite_elevation)); // uint8_t[20]:Elevation (0: right on top of receiver, 90: on the horizon) of satellite + memcpy(p->satellite_azimuth, satellite_azimuth, sizeof(p->satellite_azimuth)); // uint8_t[20]:Direction of satellite, 0: 0 deg, 255: 360 deg. + memcpy(p->satellite_snr, satellite_snr, sizeof(p->satellite_snr)); // uint8_t[20]:Signal to noise ratio of satellite - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_STATUS_LEN); } /** @@ -63,19 +63,19 @@ static inline uint16_t mavlink_msg_gps_status_pack(uint8_t system_id, uint8_t co * @param satellite_snr Signal to noise ratio of satellite * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_gps_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t satellites_visible, const int8_t* satellite_prn, const int8_t* satellite_used, const int8_t* satellite_elevation, const int8_t* satellite_azimuth, const int8_t* satellite_snr) +static inline uint16_t mavlink_msg_gps_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t satellites_visible, const uint8_t* satellite_prn, const uint8_t* satellite_used, const uint8_t* satellite_elevation, const uint8_t* satellite_azimuth, const uint8_t* satellite_snr) { - uint16_t i = 0; + mavlink_gps_status_t *p = (mavlink_gps_status_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GPS_STATUS; - i += put_uint8_t_by_index(satellites_visible, i, msg->payload); // Number of satellites visible - i += put_array_by_index(satellite_prn, 20, i, msg->payload); // Global satellite ID - i += put_array_by_index(satellite_used, 20, i, msg->payload); // 0: Satellite not used, 1: used for localization - i += put_array_by_index(satellite_elevation, 20, i, msg->payload); // Elevation (0: right on top of receiver, 90: on the horizon) of satellite - i += put_array_by_index(satellite_azimuth, 20, i, msg->payload); // Direction of satellite, 0: 0 deg, 255: 360 deg. - i += put_array_by_index(satellite_snr, 20, i, msg->payload); // Signal to noise ratio of satellite + p->satellites_visible = satellites_visible; // uint8_t:Number of satellites visible + memcpy(p->satellite_prn, satellite_prn, sizeof(p->satellite_prn)); // uint8_t[20]:Global satellite ID + memcpy(p->satellite_used, satellite_used, sizeof(p->satellite_used)); // uint8_t[20]:0: Satellite not used, 1: used for localization + memcpy(p->satellite_elevation, satellite_elevation, sizeof(p->satellite_elevation)); // uint8_t[20]:Elevation (0: right on top of receiver, 90: on the horizon) of satellite + memcpy(p->satellite_azimuth, satellite_azimuth, sizeof(p->satellite_azimuth)); // uint8_t[20]:Direction of satellite, 0: 0 deg, 255: 360 deg. + memcpy(p->satellite_snr, satellite_snr, sizeof(p->satellite_snr)); // uint8_t[20]:Signal to noise ratio of satellite - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_STATUS_LEN); } /** @@ -102,13 +102,40 @@ static inline uint16_t mavlink_msg_gps_status_encode(uint8_t system_id, uint8_t * @param satellite_azimuth Direction of satellite, 0: 0 deg, 255: 360 deg. * @param satellite_snr Signal to noise ratio of satellite */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_gps_status_send(mavlink_channel_t chan, uint8_t satellites_visible, const int8_t* satellite_prn, const int8_t* satellite_used, const int8_t* satellite_elevation, const int8_t* satellite_azimuth, const int8_t* satellite_snr) + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS +static inline void mavlink_msg_gps_status_send(mavlink_channel_t chan, uint8_t satellites_visible, const uint8_t* satellite_prn, const uint8_t* satellite_used, const uint8_t* satellite_elevation, const uint8_t* satellite_azimuth, const uint8_t* satellite_snr) { - mavlink_message_t msg; - mavlink_msg_gps_status_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, satellites_visible, satellite_prn, satellite_used, satellite_elevation, satellite_azimuth, satellite_snr); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_gps_status_t payload; + uint16_t checksum; + mavlink_gps_status_t *p = &payload; + + p->satellites_visible = satellites_visible; // uint8_t:Number of satellites visible + memcpy(p->satellite_prn, satellite_prn, sizeof(p->satellite_prn)); // uint8_t[20]:Global satellite ID + memcpy(p->satellite_used, satellite_used, sizeof(p->satellite_used)); // uint8_t[20]:0: Satellite not used, 1: used for localization + memcpy(p->satellite_elevation, satellite_elevation, sizeof(p->satellite_elevation)); // uint8_t[20]:Elevation (0: right on top of receiver, 90: on the horizon) of satellite + memcpy(p->satellite_azimuth, satellite_azimuth, sizeof(p->satellite_azimuth)); // uint8_t[20]:Direction of satellite, 0: 0 deg, 255: 360 deg. + memcpy(p->satellite_snr, satellite_snr, sizeof(p->satellite_snr)); // uint8_t[20]:Signal to noise ratio of satellite + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_GPS_STATUS_LEN; + hdr.msgid = MAVLINK_MSG_ID_GPS_STATUS; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -121,7 +148,8 @@ static inline void mavlink_msg_gps_status_send(mavlink_channel_t chan, uint8_t s */ static inline uint8_t mavlink_msg_gps_status_get_satellites_visible(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_gps_status_t *p = (mavlink_gps_status_t *)&msg->payload[0]; + return (uint8_t)(p->satellites_visible); } /** @@ -129,11 +157,12 @@ static inline uint8_t mavlink_msg_gps_status_get_satellites_visible(const mavlin * * @return Global satellite ID */ -static inline uint16_t mavlink_msg_gps_status_get_satellite_prn(const mavlink_message_t* msg, int8_t* r_data) +static inline uint16_t mavlink_msg_gps_status_get_satellite_prn(const mavlink_message_t* msg, uint8_t* satellite_prn) { + mavlink_gps_status_t *p = (mavlink_gps_status_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint8_t), 20); - return 20; + memcpy(satellite_prn, p->satellite_prn, sizeof(p->satellite_prn)); + return sizeof(p->satellite_prn); } /** @@ -141,11 +170,12 @@ static inline uint16_t mavlink_msg_gps_status_get_satellite_prn(const mavlink_me * * @return 0: Satellite not used, 1: used for localization */ -static inline uint16_t mavlink_msg_gps_status_get_satellite_used(const mavlink_message_t* msg, int8_t* r_data) +static inline uint16_t mavlink_msg_gps_status_get_satellite_used(const mavlink_message_t* msg, uint8_t* satellite_used) { + mavlink_gps_status_t *p = (mavlink_gps_status_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint8_t)+20, 20); - return 20; + memcpy(satellite_used, p->satellite_used, sizeof(p->satellite_used)); + return sizeof(p->satellite_used); } /** @@ -153,11 +183,12 @@ static inline uint16_t mavlink_msg_gps_status_get_satellite_used(const mavlink_m * * @return Elevation (0: right on top of receiver, 90: on the horizon) of satellite */ -static inline uint16_t mavlink_msg_gps_status_get_satellite_elevation(const mavlink_message_t* msg, int8_t* r_data) +static inline uint16_t mavlink_msg_gps_status_get_satellite_elevation(const mavlink_message_t* msg, uint8_t* satellite_elevation) { + mavlink_gps_status_t *p = (mavlink_gps_status_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint8_t)+20+20, 20); - return 20; + memcpy(satellite_elevation, p->satellite_elevation, sizeof(p->satellite_elevation)); + return sizeof(p->satellite_elevation); } /** @@ -165,11 +196,12 @@ static inline uint16_t mavlink_msg_gps_status_get_satellite_elevation(const mavl * * @return Direction of satellite, 0: 0 deg, 255: 360 deg. */ -static inline uint16_t mavlink_msg_gps_status_get_satellite_azimuth(const mavlink_message_t* msg, int8_t* r_data) +static inline uint16_t mavlink_msg_gps_status_get_satellite_azimuth(const mavlink_message_t* msg, uint8_t* satellite_azimuth) { + mavlink_gps_status_t *p = (mavlink_gps_status_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint8_t)+20+20+20, 20); - return 20; + memcpy(satellite_azimuth, p->satellite_azimuth, sizeof(p->satellite_azimuth)); + return sizeof(p->satellite_azimuth); } /** @@ -177,11 +209,12 @@ static inline uint16_t mavlink_msg_gps_status_get_satellite_azimuth(const mavlin * * @return Signal to noise ratio of satellite */ -static inline uint16_t mavlink_msg_gps_status_get_satellite_snr(const mavlink_message_t* msg, int8_t* r_data) +static inline uint16_t mavlink_msg_gps_status_get_satellite_snr(const mavlink_message_t* msg, uint8_t* satellite_snr) { + mavlink_gps_status_t *p = (mavlink_gps_status_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint8_t)+20+20+20+20, 20); - return 20; + memcpy(satellite_snr, p->satellite_snr, sizeof(p->satellite_snr)); + return sizeof(p->satellite_snr); } /** @@ -192,10 +225,5 @@ static inline uint16_t mavlink_msg_gps_status_get_satellite_snr(const mavlink_me */ static inline void mavlink_msg_gps_status_decode(const mavlink_message_t* msg, mavlink_gps_status_t* gps_status) { - gps_status->satellites_visible = mavlink_msg_gps_status_get_satellites_visible(msg); - mavlink_msg_gps_status_get_satellite_prn(msg, gps_status->satellite_prn); - mavlink_msg_gps_status_get_satellite_used(msg, gps_status->satellite_used); - mavlink_msg_gps_status_get_satellite_elevation(msg, gps_status->satellite_elevation); - mavlink_msg_gps_status_get_satellite_azimuth(msg, gps_status->satellite_azimuth); - mavlink_msg_gps_status_get_satellite_snr(msg, gps_status->satellite_snr); + memcpy( gps_status, msg->payload, sizeof(mavlink_gps_status_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_heartbeat.h b/thirdParty/mavlink/include/common/mavlink_msg_heartbeat.h index 0e5c4db5ca00fb627f2c5072cd8b2d1d7cbd02ad..7c686831eee88b7e0cb4e92e8ffd8f771c7a51ae 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_heartbeat.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_heartbeat.h @@ -1,6 +1,8 @@ // MESSAGE HEARTBEAT PACKING #define MAVLINK_MSG_ID_HEARTBEAT 0 +#define MAVLINK_MSG_ID_HEARTBEAT_LEN 3 +#define MAVLINK_MSG_0_LEN 3 typedef struct __mavlink_heartbeat_t { @@ -10,8 +12,6 @@ typedef struct __mavlink_heartbeat_t } mavlink_heartbeat_t; - - /** * @brief Pack a heartbeat message * @param system_id ID of this system @@ -24,14 +24,14 @@ typedef struct __mavlink_heartbeat_t */ static inline uint16_t mavlink_msg_heartbeat_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t type, uint8_t autopilot) { - uint16_t i = 0; + mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_HEARTBEAT; - i += put_uint8_t_by_index(type, i, msg->payload); // Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) - i += put_uint8_t_by_index(autopilot, i, msg->payload); // Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM - i += put_uint8_t_by_index(2, i, msg->payload); // MAVLink version + p->type = type; // uint8_t:Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + p->autopilot = autopilot; // uint8_t:Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM - return mavlink_finalize_message(msg, system_id, component_id, i); + p->mavlink_version = MAVLINK_VERSION; // uint8_t_mavlink_version:MAVLink version + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HEARTBEAT_LEN); } /** @@ -46,14 +46,14 @@ static inline uint16_t mavlink_msg_heartbeat_pack(uint8_t system_id, uint8_t com */ static inline uint16_t mavlink_msg_heartbeat_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t type, uint8_t autopilot) { - uint16_t i = 0; + mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_HEARTBEAT; - i += put_uint8_t_by_index(type, i, msg->payload); // Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) - i += put_uint8_t_by_index(autopilot, i, msg->payload); // Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM - i += put_uint8_t_by_index(2, i, msg->payload); // MAVLink version + p->type = type; // uint8_t:Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + p->autopilot = autopilot; // uint8_t:Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + p->mavlink_version = MAVLINK_VERSION; // uint8_t_mavlink_version:MAVLink version + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HEARTBEAT_LEN); } /** @@ -76,13 +76,37 @@ static inline uint16_t mavlink_msg_heartbeat_encode(uint8_t system_id, uint8_t c * @param type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) * @param autopilot Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t type, uint8_t autopilot) { - mavlink_message_t msg; - mavlink_msg_heartbeat_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, type, autopilot); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_heartbeat_t payload; + uint16_t checksum; + mavlink_heartbeat_t *p = &payload; + + p->type = type; // uint8_t:Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + p->autopilot = autopilot; // uint8_t:Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM + + p->mavlink_version = MAVLINK_VERSION; // uint8_t_mavlink_version:MAVLink version + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_HEARTBEAT_LEN; + hdr.msgid = MAVLINK_MSG_ID_HEARTBEAT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -95,7 +119,8 @@ static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t ty */ static inline uint8_t mavlink_msg_heartbeat_get_type(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg->payload[0]; + return (uint8_t)(p->type); } /** @@ -105,7 +130,8 @@ static inline uint8_t mavlink_msg_heartbeat_get_type(const mavlink_message_t* ms */ static inline uint8_t mavlink_msg_heartbeat_get_autopilot(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg->payload[0]; + return (uint8_t)(p->autopilot); } /** @@ -115,7 +141,8 @@ static inline uint8_t mavlink_msg_heartbeat_get_autopilot(const mavlink_message_ */ static inline uint8_t mavlink_msg_heartbeat_get_mavlink_version(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg->payload[0]; + return (uint8_t)(p->mavlink_version); } /** @@ -126,7 +153,5 @@ static inline uint8_t mavlink_msg_heartbeat_get_mavlink_version(const mavlink_me */ static inline void mavlink_msg_heartbeat_decode(const mavlink_message_t* msg, mavlink_heartbeat_t* heartbeat) { - heartbeat->type = mavlink_msg_heartbeat_get_type(msg); - heartbeat->autopilot = mavlink_msg_heartbeat_get_autopilot(msg); - heartbeat->mavlink_version = mavlink_msg_heartbeat_get_mavlink_version(msg); + memcpy( heartbeat, msg->payload, sizeof(mavlink_heartbeat_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_local_position.h b/thirdParty/mavlink/include/common/mavlink_msg_local_position.h index c9cb453c0e0dd4e5ef4aa771b7612a5aa30ca6db..fb6648714a61221f93d9feae50461dd8756466cd 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_local_position.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_local_position.h @@ -1,6 +1,8 @@ // MESSAGE LOCAL_POSITION PACKING #define MAVLINK_MSG_ID_LOCAL_POSITION 31 +#define MAVLINK_MSG_ID_LOCAL_POSITION_LEN 32 +#define MAVLINK_MSG_31_LEN 32 typedef struct __mavlink_local_position_t { @@ -14,8 +16,6 @@ typedef struct __mavlink_local_position_t } mavlink_local_position_t; - - /** * @brief Pack a local_position message * @param system_id ID of this system @@ -33,18 +33,18 @@ typedef struct __mavlink_local_position_t */ static inline uint16_t mavlink_msg_local_position_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t usec, float x, float y, float z, float vx, float vy, float vz) { - uint16_t i = 0; + mavlink_local_position_t *p = (mavlink_local_position_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION; - 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(x, i, msg->payload); // X Position - i += put_float_by_index(y, i, msg->payload); // Y Position - i += put_float_by_index(z, i, msg->payload); // Z Position - i += put_float_by_index(vx, i, msg->payload); // X Speed - i += put_float_by_index(vy, i, msg->payload); // Y Speed - i += put_float_by_index(vz, i, msg->payload); // Z Speed + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->x = x; // float:X Position + p->y = y; // float:Y Position + p->z = z; // float:Z Position + p->vx = vx; // float:X Speed + p->vy = vy; // float:Y Speed + p->vz = vz; // float:Z Speed - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOCAL_POSITION_LEN); } /** @@ -64,18 +64,18 @@ static inline uint16_t mavlink_msg_local_position_pack(uint8_t system_id, uint8_ */ static inline uint16_t mavlink_msg_local_position_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t usec, float x, float y, float z, float vx, float vy, float vz) { - uint16_t i = 0; + mavlink_local_position_t *p = (mavlink_local_position_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION; - 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(x, i, msg->payload); // X Position - i += put_float_by_index(y, i, msg->payload); // Y Position - i += put_float_by_index(z, i, msg->payload); // Z Position - i += put_float_by_index(vx, i, msg->payload); // X Speed - i += put_float_by_index(vy, i, msg->payload); // Y Speed - i += put_float_by_index(vz, i, msg->payload); // Z Speed + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->x = x; // float:X Position + p->y = y; // float:Y Position + p->z = z; // float:Z Position + p->vx = vx; // float:X Speed + p->vy = vy; // float:Y Speed + p->vz = vz; // float:Z Speed - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOCAL_POSITION_LEN); } /** @@ -103,13 +103,41 @@ static inline uint16_t mavlink_msg_local_position_encode(uint8_t system_id, uint * @param vy Y Speed * @param vz Z Speed */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_local_position_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float vx, float vy, float vz) { - mavlink_message_t msg; - mavlink_msg_local_position_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, usec, x, y, z, vx, vy, vz); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_local_position_t payload; + uint16_t checksum; + mavlink_local_position_t *p = &payload; + + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->x = x; // float:X Position + p->y = y; // float:Y Position + p->z = z; // float:Z Position + p->vx = vx; // float:X Speed + p->vy = vy; // float:Y Speed + p->vz = vz; // float:Z Speed + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_LOCAL_POSITION_LEN; + hdr.msgid = MAVLINK_MSG_ID_LOCAL_POSITION; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -122,16 +150,8 @@ static inline void mavlink_msg_local_position_send(mavlink_channel_t chan, uint6 */ static inline uint64_t mavlink_msg_local_position_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; + mavlink_local_position_t *p = (mavlink_local_position_t *)&msg->payload[0]; + return (uint64_t)(p->usec); } /** @@ -141,12 +161,8 @@ static inline uint64_t mavlink_msg_local_position_get_usec(const mavlink_message */ static inline float mavlink_msg_local_position_get_x(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; + mavlink_local_position_t *p = (mavlink_local_position_t *)&msg->payload[0]; + return (float)(p->x); } /** @@ -156,12 +172,8 @@ static inline float mavlink_msg_local_position_get_x(const mavlink_message_t* ms */ static inline float mavlink_msg_local_position_get_y(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; + mavlink_local_position_t *p = (mavlink_local_position_t *)&msg->payload[0]; + return (float)(p->y); } /** @@ -171,12 +183,8 @@ static inline float mavlink_msg_local_position_get_y(const mavlink_message_t* ms */ static inline float mavlink_msg_local_position_get_z(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; + mavlink_local_position_t *p = (mavlink_local_position_t *)&msg->payload[0]; + return (float)(p->z); } /** @@ -186,12 +194,8 @@ static inline float mavlink_msg_local_position_get_z(const mavlink_message_t* ms */ static inline float mavlink_msg_local_position_get_vx(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; + mavlink_local_position_t *p = (mavlink_local_position_t *)&msg->payload[0]; + return (float)(p->vx); } /** @@ -201,12 +205,8 @@ static inline float mavlink_msg_local_position_get_vx(const mavlink_message_t* m */ static inline float mavlink_msg_local_position_get_vy(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; + mavlink_local_position_t *p = (mavlink_local_position_t *)&msg->payload[0]; + return (float)(p->vy); } /** @@ -216,12 +216,8 @@ static inline float mavlink_msg_local_position_get_vy(const mavlink_message_t* m */ static inline float mavlink_msg_local_position_get_vz(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; + mavlink_local_position_t *p = (mavlink_local_position_t *)&msg->payload[0]; + return (float)(p->vz); } /** @@ -232,11 +228,5 @@ static inline float mavlink_msg_local_position_get_vz(const mavlink_message_t* m */ static inline void mavlink_msg_local_position_decode(const mavlink_message_t* msg, mavlink_local_position_t* local_position) { - local_position->usec = mavlink_msg_local_position_get_usec(msg); - local_position->x = mavlink_msg_local_position_get_x(msg); - local_position->y = mavlink_msg_local_position_get_y(msg); - local_position->z = mavlink_msg_local_position_get_z(msg); - local_position->vx = mavlink_msg_local_position_get_vx(msg); - local_position->vy = mavlink_msg_local_position_get_vy(msg); - local_position->vz = mavlink_msg_local_position_get_vz(msg); + memcpy( local_position, msg->payload, sizeof(mavlink_local_position_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_local_position_setpoint.h b/thirdParty/mavlink/include/common/mavlink_msg_local_position_setpoint.h index fcf4733476ec7c3312819ebac11a91a0d10ba1c5..8ea4ca458c1bef4ac423c22eb2fc47aeb293f85e 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_local_position_setpoint.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_local_position_setpoint.h @@ -1,6 +1,8 @@ // MESSAGE LOCAL_POSITION_SETPOINT PACKING #define MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT 51 +#define MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN 16 +#define MAVLINK_MSG_51_LEN 16 typedef struct __mavlink_local_position_setpoint_t { @@ -11,8 +13,6 @@ typedef struct __mavlink_local_position_setpoint_t } mavlink_local_position_setpoint_t; - - /** * @brief Pack a local_position_setpoint message * @param system_id ID of this system @@ -27,15 +27,15 @@ typedef struct __mavlink_local_position_setpoint_t */ static inline uint16_t mavlink_msg_local_position_setpoint_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, float x, float y, float z, float yaw) { - uint16_t i = 0; + mavlink_local_position_setpoint_t *p = (mavlink_local_position_setpoint_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT; - i += put_float_by_index(x, i, msg->payload); // x position - i += put_float_by_index(y, i, msg->payload); // y position - i += put_float_by_index(z, i, msg->payload); // z position - i += put_float_by_index(yaw, i, msg->payload); // Desired yaw angle + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float:Desired yaw angle - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN); } /** @@ -52,15 +52,15 @@ static inline uint16_t mavlink_msg_local_position_setpoint_pack(uint8_t system_i */ static inline uint16_t mavlink_msg_local_position_setpoint_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, float x, float y, float z, float yaw) { - uint16_t i = 0; + mavlink_local_position_setpoint_t *p = (mavlink_local_position_setpoint_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT; - i += put_float_by_index(x, i, msg->payload); // x position - i += put_float_by_index(y, i, msg->payload); // y position - i += put_float_by_index(z, i, msg->payload); // z position - i += put_float_by_index(yaw, i, msg->payload); // Desired yaw angle + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float:Desired yaw angle - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN); } /** @@ -85,13 +85,38 @@ static inline uint16_t mavlink_msg_local_position_setpoint_encode(uint8_t system * @param z z position * @param yaw Desired yaw angle */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_local_position_setpoint_send(mavlink_channel_t chan, float x, float y, float z, float yaw) { - mavlink_message_t msg; - mavlink_msg_local_position_setpoint_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, x, y, z, yaw); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_local_position_setpoint_t payload; + uint16_t checksum; + mavlink_local_position_setpoint_t *p = &payload; + + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float:Desired yaw angle + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN; + hdr.msgid = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -104,12 +129,8 @@ static inline void mavlink_msg_local_position_setpoint_send(mavlink_channel_t ch */ static inline float mavlink_msg_local_position_setpoint_get_x(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 (float)r.f; + mavlink_local_position_setpoint_t *p = (mavlink_local_position_setpoint_t *)&msg->payload[0]; + return (float)(p->x); } /** @@ -119,12 +140,8 @@ static inline float mavlink_msg_local_position_setpoint_get_x(const mavlink_mess */ static inline float mavlink_msg_local_position_setpoint_get_y(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float))[3]; - return (float)r.f; + mavlink_local_position_setpoint_t *p = (mavlink_local_position_setpoint_t *)&msg->payload[0]; + return (float)(p->y); } /** @@ -134,12 +151,8 @@ static inline float mavlink_msg_local_position_setpoint_get_y(const mavlink_mess */ static inline float mavlink_msg_local_position_setpoint_get_z(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_local_position_setpoint_t *p = (mavlink_local_position_setpoint_t *)&msg->payload[0]; + return (float)(p->z); } /** @@ -149,12 +162,8 @@ static inline float mavlink_msg_local_position_setpoint_get_z(const mavlink_mess */ static inline float mavlink_msg_local_position_setpoint_get_yaw(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_local_position_setpoint_t *p = (mavlink_local_position_setpoint_t *)&msg->payload[0]; + return (float)(p->yaw); } /** @@ -165,8 +174,5 @@ static inline float mavlink_msg_local_position_setpoint_get_yaw(const mavlink_me */ static inline void mavlink_msg_local_position_setpoint_decode(const mavlink_message_t* msg, mavlink_local_position_setpoint_t* local_position_setpoint) { - local_position_setpoint->x = mavlink_msg_local_position_setpoint_get_x(msg); - local_position_setpoint->y = mavlink_msg_local_position_setpoint_get_y(msg); - local_position_setpoint->z = mavlink_msg_local_position_setpoint_get_z(msg); - local_position_setpoint->yaw = mavlink_msg_local_position_setpoint_get_yaw(msg); + memcpy( local_position_setpoint, msg->payload, sizeof(mavlink_local_position_setpoint_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_local_position_setpoint_set.h b/thirdParty/mavlink/include/common/mavlink_msg_local_position_setpoint_set.h index f3383287eb40a3154f961b5698f0208d1b13f1c9..d7aa0d5bc04bc2fd20faf0709647d2b518b4a77f 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_local_position_setpoint_set.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_local_position_setpoint_set.h @@ -1,20 +1,20 @@ // MESSAGE LOCAL_POSITION_SETPOINT_SET PACKING #define MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET 50 +#define MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET_LEN 18 +#define MAVLINK_MSG_50_LEN 18 typedef struct __mavlink_local_position_setpoint_set_t { - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID float x; ///< x position float y; ///< y position float z; ///< z position float yaw; ///< Desired yaw angle + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID } mavlink_local_position_setpoint_set_t; - - /** * @brief Pack a local_position_setpoint_set message * @param system_id ID of this system @@ -31,17 +31,17 @@ typedef struct __mavlink_local_position_setpoint_set_t */ static inline uint16_t mavlink_msg_local_position_setpoint_set_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, float x, float y, float z, float yaw) { - uint16_t i = 0; + mavlink_local_position_setpoint_set_t *p = (mavlink_local_position_setpoint_set_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET; - 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(x, i, msg->payload); // x position - i += put_float_by_index(y, i, msg->payload); // y position - i += put_float_by_index(z, i, msg->payload); // z position - i += put_float_by_index(yaw, i, msg->payload); // Desired yaw angle + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float:Desired yaw angle - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET_LEN); } /** @@ -60,17 +60,17 @@ static inline uint16_t mavlink_msg_local_position_setpoint_set_pack(uint8_t syst */ static inline uint16_t mavlink_msg_local_position_setpoint_set_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 x, float y, float z, float yaw) { - uint16_t i = 0; + mavlink_local_position_setpoint_set_t *p = (mavlink_local_position_setpoint_set_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET; - 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(x, i, msg->payload); // x position - i += put_float_by_index(y, i, msg->payload); // y position - i += put_float_by_index(z, i, msg->payload); // z position - i += put_float_by_index(yaw, i, msg->payload); // Desired yaw angle + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float:Desired yaw angle - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET_LEN); } /** @@ -97,13 +97,40 @@ static inline uint16_t mavlink_msg_local_position_setpoint_set_encode(uint8_t sy * @param z z position * @param yaw Desired yaw angle */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_local_position_setpoint_set_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float x, float y, float z, float yaw) { - mavlink_message_t msg; - mavlink_msg_local_position_setpoint_set_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, x, y, z, yaw); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_local_position_setpoint_set_t payload; + uint16_t checksum; + mavlink_local_position_setpoint_set_t *p = &payload; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float:Desired yaw angle + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET_LEN; + hdr.msgid = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -116,7 +143,8 @@ static inline void mavlink_msg_local_position_setpoint_set_send(mavlink_channel_ */ static inline uint8_t mavlink_msg_local_position_setpoint_set_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_local_position_setpoint_set_t *p = (mavlink_local_position_setpoint_set_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); } /** @@ -126,7 +154,8 @@ static inline uint8_t mavlink_msg_local_position_setpoint_set_get_target_system( */ static inline uint8_t mavlink_msg_local_position_setpoint_set_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_local_position_setpoint_set_t *p = (mavlink_local_position_setpoint_set_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); } /** @@ -136,12 +165,8 @@ static inline uint8_t mavlink_msg_local_position_setpoint_set_get_target_compone */ static inline float mavlink_msg_local_position_setpoint_set_get_x(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; + mavlink_local_position_setpoint_set_t *p = (mavlink_local_position_setpoint_set_t *)&msg->payload[0]; + return (float)(p->x); } /** @@ -151,12 +176,8 @@ static inline float mavlink_msg_local_position_setpoint_set_get_x(const mavlink_ */ static inline float mavlink_msg_local_position_setpoint_set_get_y(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; + mavlink_local_position_setpoint_set_t *p = (mavlink_local_position_setpoint_set_t *)&msg->payload[0]; + return (float)(p->y); } /** @@ -166,12 +187,8 @@ static inline float mavlink_msg_local_position_setpoint_set_get_y(const mavlink_ */ static inline float mavlink_msg_local_position_setpoint_set_get_z(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; + mavlink_local_position_setpoint_set_t *p = (mavlink_local_position_setpoint_set_t *)&msg->payload[0]; + return (float)(p->z); } /** @@ -181,12 +198,8 @@ static inline float mavlink_msg_local_position_setpoint_set_get_z(const mavlink_ */ static inline float mavlink_msg_local_position_setpoint_set_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)+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; + mavlink_local_position_setpoint_set_t *p = (mavlink_local_position_setpoint_set_t *)&msg->payload[0]; + return (float)(p->yaw); } /** @@ -197,10 +210,5 @@ static inline float mavlink_msg_local_position_setpoint_set_get_yaw(const mavlin */ static inline void mavlink_msg_local_position_setpoint_set_decode(const mavlink_message_t* msg, mavlink_local_position_setpoint_set_t* local_position_setpoint_set) { - local_position_setpoint_set->target_system = mavlink_msg_local_position_setpoint_set_get_target_system(msg); - local_position_setpoint_set->target_component = mavlink_msg_local_position_setpoint_set_get_target_component(msg); - local_position_setpoint_set->x = mavlink_msg_local_position_setpoint_set_get_x(msg); - local_position_setpoint_set->y = mavlink_msg_local_position_setpoint_set_get_y(msg); - local_position_setpoint_set->z = mavlink_msg_local_position_setpoint_set_get_z(msg); - local_position_setpoint_set->yaw = mavlink_msg_local_position_setpoint_set_get_yaw(msg); + memcpy( local_position_setpoint_set, msg->payload, sizeof(mavlink_local_position_setpoint_set_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_manual_control.h b/thirdParty/mavlink/include/common/mavlink_msg_manual_control.h index deec098f63c8cbd6779a077c96c5bf806e7bd308..eeb05487af7ebfb08b8269d8b376bd2c2dac9270 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_manual_control.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_manual_control.h @@ -1,14 +1,16 @@ // MESSAGE MANUAL_CONTROL PACKING #define MAVLINK_MSG_ID_MANUAL_CONTROL 69 +#define MAVLINK_MSG_ID_MANUAL_CONTROL_LEN 21 +#define MAVLINK_MSG_69_LEN 21 typedef struct __mavlink_manual_control_t { - uint8_t target; ///< The system to be controlled float roll; ///< roll float pitch; ///< pitch float yaw; ///< yaw float thrust; ///< thrust + uint8_t target; ///< The system to be controlled uint8_t roll_manual; ///< roll control enabled auto:0, manual:1 uint8_t pitch_manual; ///< pitch auto:0, manual:1 uint8_t yaw_manual; ///< yaw auto:0, manual:1 @@ -16,8 +18,6 @@ typedef struct __mavlink_manual_control_t } mavlink_manual_control_t; - - /** * @brief Pack a manual_control message * @param system_id ID of this system @@ -37,20 +37,20 @@ typedef struct __mavlink_manual_control_t */ static inline uint16_t mavlink_msg_manual_control_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target, float roll, float pitch, float yaw, float thrust, uint8_t roll_manual, uint8_t pitch_manual, uint8_t yaw_manual, uint8_t thrust_manual) { - uint16_t i = 0; + mavlink_manual_control_t *p = (mavlink_manual_control_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_MANUAL_CONTROL; - i += put_uint8_t_by_index(target, i, msg->payload); // The system to be controlled - i += put_float_by_index(roll, i, msg->payload); // roll - i += put_float_by_index(pitch, i, msg->payload); // pitch - i += put_float_by_index(yaw, i, msg->payload); // yaw - i += put_float_by_index(thrust, i, msg->payload); // thrust - i += put_uint8_t_by_index(roll_manual, i, msg->payload); // roll control enabled auto:0, manual:1 - i += put_uint8_t_by_index(pitch_manual, i, msg->payload); // pitch auto:0, manual:1 - i += put_uint8_t_by_index(yaw_manual, i, msg->payload); // yaw auto:0, manual:1 - i += put_uint8_t_by_index(thrust_manual, i, msg->payload); // thrust auto:0, manual:1 + p->target = target; // uint8_t:The system to be controlled + p->roll = roll; // float:roll + p->pitch = pitch; // float:pitch + p->yaw = yaw; // float:yaw + p->thrust = thrust; // float:thrust + p->roll_manual = roll_manual; // uint8_t:roll control enabled auto:0, manual:1 + p->pitch_manual = pitch_manual; // uint8_t:pitch auto:0, manual:1 + p->yaw_manual = yaw_manual; // uint8_t:yaw auto:0, manual:1 + p->thrust_manual = thrust_manual; // uint8_t:thrust auto:0, manual:1 - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN); } /** @@ -72,20 +72,20 @@ static inline uint16_t mavlink_msg_manual_control_pack(uint8_t system_id, uint8_ */ static inline uint16_t mavlink_msg_manual_control_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target, float roll, float pitch, float yaw, float thrust, uint8_t roll_manual, uint8_t pitch_manual, uint8_t yaw_manual, uint8_t thrust_manual) { - uint16_t i = 0; + mavlink_manual_control_t *p = (mavlink_manual_control_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_MANUAL_CONTROL; - i += put_uint8_t_by_index(target, i, msg->payload); // The system to be controlled - i += put_float_by_index(roll, i, msg->payload); // roll - i += put_float_by_index(pitch, i, msg->payload); // pitch - i += put_float_by_index(yaw, i, msg->payload); // yaw - i += put_float_by_index(thrust, i, msg->payload); // thrust - i += put_uint8_t_by_index(roll_manual, i, msg->payload); // roll control enabled auto:0, manual:1 - i += put_uint8_t_by_index(pitch_manual, i, msg->payload); // pitch auto:0, manual:1 - i += put_uint8_t_by_index(yaw_manual, i, msg->payload); // yaw auto:0, manual:1 - i += put_uint8_t_by_index(thrust_manual, i, msg->payload); // thrust auto:0, manual:1 + p->target = target; // uint8_t:The system to be controlled + p->roll = roll; // float:roll + p->pitch = pitch; // float:pitch + p->yaw = yaw; // float:yaw + p->thrust = thrust; // float:thrust + p->roll_manual = roll_manual; // uint8_t:roll control enabled auto:0, manual:1 + p->pitch_manual = pitch_manual; // uint8_t:pitch auto:0, manual:1 + p->yaw_manual = yaw_manual; // uint8_t:yaw auto:0, manual:1 + p->thrust_manual = thrust_manual; // uint8_t:thrust auto:0, manual:1 - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN); } /** @@ -115,13 +115,43 @@ static inline uint16_t mavlink_msg_manual_control_encode(uint8_t system_id, uint * @param yaw_manual yaw auto:0, manual:1 * @param thrust_manual thrust auto:0, manual:1 */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_manual_control_send(mavlink_channel_t chan, uint8_t target, float roll, float pitch, float yaw, float thrust, uint8_t roll_manual, uint8_t pitch_manual, uint8_t yaw_manual, uint8_t thrust_manual) { - mavlink_message_t msg; - mavlink_msg_manual_control_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target, roll, pitch, yaw, thrust, roll_manual, pitch_manual, yaw_manual, thrust_manual); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_manual_control_t payload; + uint16_t checksum; + mavlink_manual_control_t *p = &payload; + + p->target = target; // uint8_t:The system to be controlled + p->roll = roll; // float:roll + p->pitch = pitch; // float:pitch + p->yaw = yaw; // float:yaw + p->thrust = thrust; // float:thrust + p->roll_manual = roll_manual; // uint8_t:roll control enabled auto:0, manual:1 + p->pitch_manual = pitch_manual; // uint8_t:pitch auto:0, manual:1 + p->yaw_manual = yaw_manual; // uint8_t:yaw auto:0, manual:1 + p->thrust_manual = thrust_manual; // uint8_t:thrust auto:0, manual:1 + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_MANUAL_CONTROL_LEN; + hdr.msgid = MAVLINK_MSG_ID_MANUAL_CONTROL; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -134,7 +164,8 @@ static inline void mavlink_msg_manual_control_send(mavlink_channel_t chan, uint8 */ static inline uint8_t mavlink_msg_manual_control_get_target(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_manual_control_t *p = (mavlink_manual_control_t *)&msg->payload[0]; + return (uint8_t)(p->target); } /** @@ -144,12 +175,8 @@ static inline uint8_t mavlink_msg_manual_control_get_target(const mavlink_messag */ static inline float mavlink_msg_manual_control_get_roll(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t))[3]; - return (float)r.f; + mavlink_manual_control_t *p = (mavlink_manual_control_t *)&msg->payload[0]; + return (float)(p->roll); } /** @@ -159,12 +186,8 @@ static inline float mavlink_msg_manual_control_get_roll(const mavlink_message_t* */ static inline float mavlink_msg_manual_control_get_pitch(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(float))[3]; - return (float)r.f; + mavlink_manual_control_t *p = (mavlink_manual_control_t *)&msg->payload[0]; + return (float)(p->pitch); } /** @@ -174,12 +197,8 @@ static inline float mavlink_msg_manual_control_get_pitch(const mavlink_message_t */ static inline float mavlink_msg_manual_control_get_yaw(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_manual_control_t *p = (mavlink_manual_control_t *)&msg->payload[0]; + return (float)(p->yaw); } /** @@ -189,12 +208,8 @@ static inline float mavlink_msg_manual_control_get_yaw(const mavlink_message_t* */ static inline float mavlink_msg_manual_control_get_thrust(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_manual_control_t *p = (mavlink_manual_control_t *)&msg->payload[0]; + return (float)(p->thrust); } /** @@ -204,7 +219,8 @@ static inline float mavlink_msg_manual_control_get_thrust(const mavlink_message_ */ static inline uint8_t mavlink_msg_manual_control_get_roll_manual(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; + mavlink_manual_control_t *p = (mavlink_manual_control_t *)&msg->payload[0]; + return (uint8_t)(p->roll_manual); } /** @@ -214,7 +230,8 @@ static inline uint8_t mavlink_msg_manual_control_get_roll_manual(const mavlink_m */ static inline uint8_t mavlink_msg_manual_control_get_pitch_manual(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t))[0]; + mavlink_manual_control_t *p = (mavlink_manual_control_t *)&msg->payload[0]; + return (uint8_t)(p->pitch_manual); } /** @@ -224,7 +241,8 @@ static inline uint8_t mavlink_msg_manual_control_get_pitch_manual(const mavlink_ */ static inline uint8_t mavlink_msg_manual_control_get_yaw_manual(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_manual_control_t *p = (mavlink_manual_control_t *)&msg->payload[0]; + return (uint8_t)(p->yaw_manual); } /** @@ -234,7 +252,8 @@ static inline uint8_t mavlink_msg_manual_control_get_yaw_manual(const mavlink_me */ static inline uint8_t mavlink_msg_manual_control_get_thrust_manual(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_manual_control_t *p = (mavlink_manual_control_t *)&msg->payload[0]; + return (uint8_t)(p->thrust_manual); } /** @@ -245,13 +264,5 @@ static inline uint8_t mavlink_msg_manual_control_get_thrust_manual(const mavlink */ static inline void mavlink_msg_manual_control_decode(const mavlink_message_t* msg, mavlink_manual_control_t* manual_control) { - manual_control->target = mavlink_msg_manual_control_get_target(msg); - manual_control->roll = mavlink_msg_manual_control_get_roll(msg); - manual_control->pitch = mavlink_msg_manual_control_get_pitch(msg); - manual_control->yaw = mavlink_msg_manual_control_get_yaw(msg); - manual_control->thrust = mavlink_msg_manual_control_get_thrust(msg); - manual_control->roll_manual = mavlink_msg_manual_control_get_roll_manual(msg); - manual_control->pitch_manual = mavlink_msg_manual_control_get_pitch_manual(msg); - manual_control->yaw_manual = mavlink_msg_manual_control_get_yaw_manual(msg); - manual_control->thrust_manual = mavlink_msg_manual_control_get_thrust_manual(msg); + memcpy( manual_control, msg->payload, sizeof(mavlink_manual_control_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_memory_vect.h b/thirdParty/mavlink/include/common/mavlink_msg_memory_vect.h new file mode 100644 index 0000000000000000000000000000000000000000..ce90911d16cf70390764733c51da6c4bddf87214 --- /dev/null +++ b/thirdParty/mavlink/include/common/mavlink_msg_memory_vect.h @@ -0,0 +1,181 @@ +// MESSAGE MEMORY_VECT PACKING + +#define MAVLINK_MSG_ID_MEMORY_VECT 250 +#define MAVLINK_MSG_ID_MEMORY_VECT_LEN 36 +#define MAVLINK_MSG_250_LEN 36 + +typedef struct __mavlink_memory_vect_t +{ + uint16_t address; ///< Starting address of the debug variables + uint8_t ver; ///< Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below + uint8_t type; ///< Type code of the memory variables. for ver = 1: 0=16 x int16_t, 1=16 x uint16_t, 2=16 x Q15, 3=16 x 1Q14 + int8_t value[32]; ///< Memory contents at specified address + +} mavlink_memory_vect_t; +#define MAVLINK_MSG_MEMORY_VECT_FIELD_VALUE_LEN 32 + +/** + * @brief Pack a memory_vect 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 address Starting address of the debug variables + * @param ver Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below + * @param type Type code of the memory variables. for ver = 1: 0=16 x int16_t, 1=16 x uint16_t, 2=16 x Q15, 3=16 x 1Q14 + * @param value Memory contents at specified address + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_memory_vect_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint16_t address, uint8_t ver, uint8_t type, const int8_t* value) +{ + mavlink_memory_vect_t *p = (mavlink_memory_vect_t *)&msg->payload[0]; + msg->msgid = MAVLINK_MSG_ID_MEMORY_VECT; + + p->address = address; // uint16_t:Starting address of the debug variables + p->ver = ver; // uint8_t:Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below + p->type = type; // uint8_t:Type code of the memory variables. for ver = 1: 0=16 x int16_t, 1=16 x uint16_t, 2=16 x Q15, 3=16 x 1Q14 + memcpy(p->value, value, sizeof(p->value)); // int8_t[32]:Memory contents at specified address + + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MEMORY_VECT_LEN); +} + +/** + * @brief Pack a memory_vect 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 address Starting address of the debug variables + * @param ver Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below + * @param type Type code of the memory variables. for ver = 1: 0=16 x int16_t, 1=16 x uint16_t, 2=16 x Q15, 3=16 x 1Q14 + * @param value Memory contents at specified address + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_memory_vect_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint16_t address, uint8_t ver, uint8_t type, const int8_t* value) +{ + mavlink_memory_vect_t *p = (mavlink_memory_vect_t *)&msg->payload[0]; + msg->msgid = MAVLINK_MSG_ID_MEMORY_VECT; + + p->address = address; // uint16_t:Starting address of the debug variables + p->ver = ver; // uint8_t:Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below + p->type = type; // uint8_t:Type code of the memory variables. for ver = 1: 0=16 x int16_t, 1=16 x uint16_t, 2=16 x Q15, 3=16 x 1Q14 + memcpy(p->value, value, sizeof(p->value)); // int8_t[32]:Memory contents at specified address + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MEMORY_VECT_LEN); +} + +/** + * @brief Encode a memory_vect 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 memory_vect C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_memory_vect_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_memory_vect_t* memory_vect) +{ + return mavlink_msg_memory_vect_pack(system_id, component_id, msg, memory_vect->address, memory_vect->ver, memory_vect->type, memory_vect->value); +} + +/** + * @brief Send a memory_vect message + * @param chan MAVLink channel to send the message + * + * @param address Starting address of the debug variables + * @param ver Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below + * @param type Type code of the memory variables. for ver = 1: 0=16 x int16_t, 1=16 x uint16_t, 2=16 x Q15, 3=16 x 1Q14 + * @param value Memory contents at specified address + */ + + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS +static inline void mavlink_msg_memory_vect_send(mavlink_channel_t chan, uint16_t address, uint8_t ver, uint8_t type, const int8_t* value) +{ + mavlink_header_t hdr; + mavlink_memory_vect_t payload; + uint16_t checksum; + mavlink_memory_vect_t *p = &payload; + + p->address = address; // uint16_t:Starting address of the debug variables + p->ver = ver; // uint8_t:Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below + p->type = type; // uint8_t:Type code of the memory variables. for ver = 1: 0=16 x int16_t, 1=16 x uint16_t, 2=16 x Q15, 3=16 x 1Q14 + memcpy(p->value, value, sizeof(p->value)); // int8_t[32]:Memory contents at specified address + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_MEMORY_VECT_LEN; + hdr.msgid = MAVLINK_MSG_ID_MEMORY_VECT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); +} + +#endif +// MESSAGE MEMORY_VECT UNPACKING + +/** + * @brief Get field address from memory_vect message + * + * @return Starting address of the debug variables + */ +static inline uint16_t mavlink_msg_memory_vect_get_address(const mavlink_message_t* msg) +{ + mavlink_memory_vect_t *p = (mavlink_memory_vect_t *)&msg->payload[0]; + return (uint16_t)(p->address); +} + +/** + * @brief Get field ver from memory_vect message + * + * @return Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below + */ +static inline uint8_t mavlink_msg_memory_vect_get_ver(const mavlink_message_t* msg) +{ + mavlink_memory_vect_t *p = (mavlink_memory_vect_t *)&msg->payload[0]; + return (uint8_t)(p->ver); +} + +/** + * @brief Get field type from memory_vect message + * + * @return Type code of the memory variables. for ver = 1: 0=16 x int16_t, 1=16 x uint16_t, 2=16 x Q15, 3=16 x 1Q14 + */ +static inline uint8_t mavlink_msg_memory_vect_get_type(const mavlink_message_t* msg) +{ + mavlink_memory_vect_t *p = (mavlink_memory_vect_t *)&msg->payload[0]; + return (uint8_t)(p->type); +} + +/** + * @brief Get field value from memory_vect message + * + * @return Memory contents at specified address + */ +static inline uint16_t mavlink_msg_memory_vect_get_value(const mavlink_message_t* msg, int8_t* value) +{ + mavlink_memory_vect_t *p = (mavlink_memory_vect_t *)&msg->payload[0]; + + memcpy(value, p->value, sizeof(p->value)); + return sizeof(p->value); +} + +/** + * @brief Decode a memory_vect message into a struct + * + * @param msg The message to decode + * @param memory_vect C-struct to decode the message contents into + */ +static inline void mavlink_msg_memory_vect_decode(const mavlink_message_t* msg, mavlink_memory_vect_t* memory_vect) +{ + memcpy( memory_vect, msg->payload, sizeof(mavlink_memory_vect_t)); +} diff --git a/thirdParty/mavlink/include/common/mavlink_msg_named_value_float.h b/thirdParty/mavlink/include/common/mavlink_msg_named_value_float.h index aaff215ce8cac16790d964a392624222902252c3..ce5c680ad29985cbf971f0c62f70a91babb949e3 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_named_value_float.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_named_value_float.h @@ -1,17 +1,17 @@ // MESSAGE NAMED_VALUE_FLOAT PACKING #define MAVLINK_MSG_ID_NAMED_VALUE_FLOAT 252 +#define MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN 14 +#define MAVLINK_MSG_252_LEN 14 typedef struct __mavlink_named_value_float_t { - char name[10]; ///< Name of the debug variable float value; ///< Floating point value + char name[10]; ///< Name of the debug variable } mavlink_named_value_float_t; - #define MAVLINK_MSG_NAMED_VALUE_FLOAT_FIELD_NAME_LEN 10 - /** * @brief Pack a named_value_float message * @param system_id ID of this system @@ -24,13 +24,13 @@ typedef struct __mavlink_named_value_float_t */ static inline uint16_t mavlink_msg_named_value_float_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const char* name, float value) { - uint16_t i = 0; + mavlink_named_value_float_t *p = (mavlink_named_value_float_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; - i += put_array_by_index((const int8_t*)name, sizeof(char)*10, i, msg->payload); // Name of the debug variable - i += put_float_by_index(value, i, msg->payload); // Floating point value + memcpy(p->name, name, sizeof(p->name)); // char[10]:Name of the debug variable + p->value = value; // float:Floating point value - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN); } /** @@ -45,13 +45,13 @@ static inline uint16_t mavlink_msg_named_value_float_pack(uint8_t system_id, uin */ static inline uint16_t mavlink_msg_named_value_float_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const char* name, float value) { - uint16_t i = 0; + mavlink_named_value_float_t *p = (mavlink_named_value_float_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; - i += put_array_by_index((const int8_t*)name, sizeof(char)*10, i, msg->payload); // Name of the debug variable - i += put_float_by_index(value, i, msg->payload); // Floating point value + memcpy(p->name, name, sizeof(p->name)); // char[10]:Name of the debug variable + p->value = value; // float:Floating point value - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN); } /** @@ -74,13 +74,36 @@ static inline uint16_t mavlink_msg_named_value_float_encode(uint8_t system_id, u * @param name Name of the debug variable * @param value Floating point value */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_named_value_float_send(mavlink_channel_t chan, const char* name, float value) { - mavlink_message_t msg; - mavlink_msg_named_value_float_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, name, value); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_named_value_float_t payload; + uint16_t checksum; + mavlink_named_value_float_t *p = &payload; + + memcpy(p->name, name, sizeof(p->name)); // char[10]:Name of the debug variable + p->value = value; // float:Floating point value + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN; + hdr.msgid = MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -91,11 +114,12 @@ static inline void mavlink_msg_named_value_float_send(mavlink_channel_t chan, co * * @return Name of the debug variable */ -static inline uint16_t mavlink_msg_named_value_float_get_name(const mavlink_message_t* msg, char* r_data) +static inline uint16_t mavlink_msg_named_value_float_get_name(const mavlink_message_t* msg, char* name) { + mavlink_named_value_float_t *p = (mavlink_named_value_float_t *)&msg->payload[0]; - memcpy(r_data, msg->payload, sizeof(char)*10); - return sizeof(char)*10; + memcpy(name, p->name, sizeof(p->name)); + return sizeof(p->name); } /** @@ -105,12 +129,8 @@ static inline uint16_t mavlink_msg_named_value_float_get_name(const mavlink_mess */ static inline float mavlink_msg_named_value_float_get_value(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(char)*10)[0]; - r.b[2] = (msg->payload+sizeof(char)*10)[1]; - r.b[1] = (msg->payload+sizeof(char)*10)[2]; - r.b[0] = (msg->payload+sizeof(char)*10)[3]; - return (float)r.f; + mavlink_named_value_float_t *p = (mavlink_named_value_float_t *)&msg->payload[0]; + return (float)(p->value); } /** @@ -121,6 +141,5 @@ static inline float mavlink_msg_named_value_float_get_value(const mavlink_messag */ static inline void mavlink_msg_named_value_float_decode(const mavlink_message_t* msg, mavlink_named_value_float_t* named_value_float) { - mavlink_msg_named_value_float_get_name(msg, named_value_float->name); - named_value_float->value = mavlink_msg_named_value_float_get_value(msg); + memcpy( named_value_float, msg->payload, sizeof(mavlink_named_value_float_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_named_value_int.h b/thirdParty/mavlink/include/common/mavlink_msg_named_value_int.h index 92bf5fcb2a00dc154013dc64efe8f6dfc97a4ce4..1c75a5be169bfa871a79e3dea843819a98770c65 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_named_value_int.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_named_value_int.h @@ -1,17 +1,17 @@ // MESSAGE NAMED_VALUE_INT PACKING #define MAVLINK_MSG_ID_NAMED_VALUE_INT 253 +#define MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN 14 +#define MAVLINK_MSG_253_LEN 14 typedef struct __mavlink_named_value_int_t { - char name[10]; ///< Name of the debug variable int32_t value; ///< Signed integer value + char name[10]; ///< Name of the debug variable } mavlink_named_value_int_t; - #define MAVLINK_MSG_NAMED_VALUE_INT_FIELD_NAME_LEN 10 - /** * @brief Pack a named_value_int message * @param system_id ID of this system @@ -24,13 +24,13 @@ typedef struct __mavlink_named_value_int_t */ static inline uint16_t mavlink_msg_named_value_int_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const char* name, int32_t value) { - uint16_t i = 0; + mavlink_named_value_int_t *p = (mavlink_named_value_int_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_NAMED_VALUE_INT; - i += put_array_by_index((const int8_t*)name, sizeof(char)*10, i, msg->payload); // Name of the debug variable - i += put_int32_t_by_index(value, i, msg->payload); // Signed integer value + memcpy(p->name, name, sizeof(p->name)); // char[10]:Name of the debug variable + p->value = value; // int32_t:Signed integer value - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN); } /** @@ -45,13 +45,13 @@ static inline uint16_t mavlink_msg_named_value_int_pack(uint8_t system_id, uint8 */ static inline uint16_t mavlink_msg_named_value_int_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const char* name, int32_t value) { - uint16_t i = 0; + mavlink_named_value_int_t *p = (mavlink_named_value_int_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_NAMED_VALUE_INT; - i += put_array_by_index((const int8_t*)name, sizeof(char)*10, i, msg->payload); // Name of the debug variable - i += put_int32_t_by_index(value, i, msg->payload); // Signed integer value + memcpy(p->name, name, sizeof(p->name)); // char[10]:Name of the debug variable + p->value = value; // int32_t:Signed integer value - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN); } /** @@ -74,13 +74,36 @@ static inline uint16_t mavlink_msg_named_value_int_encode(uint8_t system_id, uin * @param name Name of the debug variable * @param value Signed integer value */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_named_value_int_send(mavlink_channel_t chan, const char* name, int32_t value) { - mavlink_message_t msg; - mavlink_msg_named_value_int_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, name, value); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_named_value_int_t payload; + uint16_t checksum; + mavlink_named_value_int_t *p = &payload; + + memcpy(p->name, name, sizeof(p->name)); // char[10]:Name of the debug variable + p->value = value; // int32_t:Signed integer value + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN; + hdr.msgid = MAVLINK_MSG_ID_NAMED_VALUE_INT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -91,11 +114,12 @@ static inline void mavlink_msg_named_value_int_send(mavlink_channel_t chan, cons * * @return Name of the debug variable */ -static inline uint16_t mavlink_msg_named_value_int_get_name(const mavlink_message_t* msg, char* r_data) +static inline uint16_t mavlink_msg_named_value_int_get_name(const mavlink_message_t* msg, char* name) { + mavlink_named_value_int_t *p = (mavlink_named_value_int_t *)&msg->payload[0]; - memcpy(r_data, msg->payload, sizeof(char)*10); - return sizeof(char)*10; + memcpy(name, p->name, sizeof(p->name)); + return sizeof(p->name); } /** @@ -105,12 +129,8 @@ static inline uint16_t mavlink_msg_named_value_int_get_name(const mavlink_messag */ static inline int32_t mavlink_msg_named_value_int_get_value(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(char)*10)[0]; - r.b[2] = (msg->payload+sizeof(char)*10)[1]; - r.b[1] = (msg->payload+sizeof(char)*10)[2]; - r.b[0] = (msg->payload+sizeof(char)*10)[3]; - return (int32_t)r.i; + mavlink_named_value_int_t *p = (mavlink_named_value_int_t *)&msg->payload[0]; + return (int32_t)(p->value); } /** @@ -121,6 +141,5 @@ static inline int32_t mavlink_msg_named_value_int_get_value(const mavlink_messag */ static inline void mavlink_msg_named_value_int_decode(const mavlink_message_t* msg, mavlink_named_value_int_t* named_value_int) { - mavlink_msg_named_value_int_get_name(msg, named_value_int->name); - named_value_int->value = mavlink_msg_named_value_int_get_value(msg); + memcpy( named_value_int, msg->payload, sizeof(mavlink_named_value_int_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_nav_controller_output.h b/thirdParty/mavlink/include/common/mavlink_msg_nav_controller_output.h index 8d0b81eb877a57469875d785f85215e4e941f071..ef878a3f7c4a09794cde2f22ce2078add6cde4f1 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_nav_controller_output.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_nav_controller_output.h @@ -1,22 +1,22 @@ // MESSAGE NAV_CONTROLLER_OUTPUT PACKING #define MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT 62 +#define MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN 26 +#define MAVLINK_MSG_62_LEN 26 typedef struct __mavlink_nav_controller_output_t { float nav_roll; ///< Current desired roll in degrees float nav_pitch; ///< Current desired pitch in degrees - int16_t nav_bearing; ///< Current desired heading in degrees - int16_t target_bearing; ///< Bearing to current waypoint/target in degrees - uint16_t wp_dist; ///< Distance to active waypoint in meters float alt_error; ///< Current altitude error in meters float aspd_error; ///< Current airspeed error in meters/second float xtrack_error; ///< Current crosstrack error on x-y plane in meters + int16_t nav_bearing; ///< Current desired heading in degrees + int16_t target_bearing; ///< Bearing to current waypoint/target in degrees + uint16_t wp_dist; ///< Distance to active waypoint in meters } mavlink_nav_controller_output_t; - - /** * @brief Pack a nav_controller_output message * @param system_id ID of this system @@ -35,19 +35,19 @@ typedef struct __mavlink_nav_controller_output_t */ static inline uint16_t mavlink_msg_nav_controller_output_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, float nav_roll, float nav_pitch, int16_t nav_bearing, int16_t target_bearing, uint16_t wp_dist, float alt_error, float aspd_error, float xtrack_error) { - uint16_t i = 0; + mavlink_nav_controller_output_t *p = (mavlink_nav_controller_output_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT; - i += put_float_by_index(nav_roll, i, msg->payload); // Current desired roll in degrees - i += put_float_by_index(nav_pitch, i, msg->payload); // Current desired pitch in degrees - i += put_int16_t_by_index(nav_bearing, i, msg->payload); // Current desired heading in degrees - i += put_int16_t_by_index(target_bearing, i, msg->payload); // Bearing to current waypoint/target in degrees - i += put_uint16_t_by_index(wp_dist, i, msg->payload); // Distance to active waypoint in meters - i += put_float_by_index(alt_error, i, msg->payload); // Current altitude error in meters - i += put_float_by_index(aspd_error, i, msg->payload); // Current airspeed error in meters/second - i += put_float_by_index(xtrack_error, i, msg->payload); // Current crosstrack error on x-y plane in meters + p->nav_roll = nav_roll; // float:Current desired roll in degrees + p->nav_pitch = nav_pitch; // float:Current desired pitch in degrees + p->nav_bearing = nav_bearing; // int16_t:Current desired heading in degrees + p->target_bearing = target_bearing; // int16_t:Bearing to current waypoint/target in degrees + p->wp_dist = wp_dist; // uint16_t:Distance to active waypoint in meters + p->alt_error = alt_error; // float:Current altitude error in meters + p->aspd_error = aspd_error; // float:Current airspeed error in meters/second + p->xtrack_error = xtrack_error; // float:Current crosstrack error on x-y plane in meters - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN); } /** @@ -68,19 +68,19 @@ static inline uint16_t mavlink_msg_nav_controller_output_pack(uint8_t system_id, */ static inline uint16_t mavlink_msg_nav_controller_output_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, float nav_roll, float nav_pitch, int16_t nav_bearing, int16_t target_bearing, uint16_t wp_dist, float alt_error, float aspd_error, float xtrack_error) { - uint16_t i = 0; + mavlink_nav_controller_output_t *p = (mavlink_nav_controller_output_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT; - i += put_float_by_index(nav_roll, i, msg->payload); // Current desired roll in degrees - i += put_float_by_index(nav_pitch, i, msg->payload); // Current desired pitch in degrees - i += put_int16_t_by_index(nav_bearing, i, msg->payload); // Current desired heading in degrees - i += put_int16_t_by_index(target_bearing, i, msg->payload); // Bearing to current waypoint/target in degrees - i += put_uint16_t_by_index(wp_dist, i, msg->payload); // Distance to active waypoint in meters - i += put_float_by_index(alt_error, i, msg->payload); // Current altitude error in meters - i += put_float_by_index(aspd_error, i, msg->payload); // Current airspeed error in meters/second - i += put_float_by_index(xtrack_error, i, msg->payload); // Current crosstrack error on x-y plane in meters + p->nav_roll = nav_roll; // float:Current desired roll in degrees + p->nav_pitch = nav_pitch; // float:Current desired pitch in degrees + p->nav_bearing = nav_bearing; // int16_t:Current desired heading in degrees + p->target_bearing = target_bearing; // int16_t:Bearing to current waypoint/target in degrees + p->wp_dist = wp_dist; // uint16_t:Distance to active waypoint in meters + p->alt_error = alt_error; // float:Current altitude error in meters + p->aspd_error = aspd_error; // float:Current airspeed error in meters/second + p->xtrack_error = xtrack_error; // float:Current crosstrack error on x-y plane in meters - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN); } /** @@ -109,13 +109,42 @@ static inline uint16_t mavlink_msg_nav_controller_output_encode(uint8_t system_i * @param aspd_error Current airspeed error in meters/second * @param xtrack_error Current crosstrack error on x-y plane in meters */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_nav_controller_output_send(mavlink_channel_t chan, float nav_roll, float nav_pitch, int16_t nav_bearing, int16_t target_bearing, uint16_t wp_dist, float alt_error, float aspd_error, float xtrack_error) { - mavlink_message_t msg; - mavlink_msg_nav_controller_output_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, nav_roll, nav_pitch, nav_bearing, target_bearing, wp_dist, alt_error, aspd_error, xtrack_error); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_nav_controller_output_t payload; + uint16_t checksum; + mavlink_nav_controller_output_t *p = &payload; + + p->nav_roll = nav_roll; // float:Current desired roll in degrees + p->nav_pitch = nav_pitch; // float:Current desired pitch in degrees + p->nav_bearing = nav_bearing; // int16_t:Current desired heading in degrees + p->target_bearing = target_bearing; // int16_t:Bearing to current waypoint/target in degrees + p->wp_dist = wp_dist; // uint16_t:Distance to active waypoint in meters + p->alt_error = alt_error; // float:Current altitude error in meters + p->aspd_error = aspd_error; // float:Current airspeed error in meters/second + p->xtrack_error = xtrack_error; // float:Current crosstrack error on x-y plane in meters + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN; + hdr.msgid = MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -128,12 +157,8 @@ static inline void mavlink_msg_nav_controller_output_send(mavlink_channel_t chan */ static inline float mavlink_msg_nav_controller_output_get_nav_roll(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 (float)r.f; + mavlink_nav_controller_output_t *p = (mavlink_nav_controller_output_t *)&msg->payload[0]; + return (float)(p->nav_roll); } /** @@ -143,12 +168,8 @@ static inline float mavlink_msg_nav_controller_output_get_nav_roll(const mavlink */ static inline float mavlink_msg_nav_controller_output_get_nav_pitch(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float))[3]; - return (float)r.f; + mavlink_nav_controller_output_t *p = (mavlink_nav_controller_output_t *)&msg->payload[0]; + return (float)(p->nav_pitch); } /** @@ -158,10 +179,8 @@ static inline float mavlink_msg_nav_controller_output_get_nav_pitch(const mavlin */ static inline int16_t mavlink_msg_nav_controller_output_get_nav_bearing(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float))[0]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float))[1]; - return (int16_t)r.s; + mavlink_nav_controller_output_t *p = (mavlink_nav_controller_output_t *)&msg->payload[0]; + return (int16_t)(p->nav_bearing); } /** @@ -171,10 +190,8 @@ static inline int16_t mavlink_msg_nav_controller_output_get_nav_bearing(const ma */ static inline int16_t mavlink_msg_nav_controller_output_get_target_bearing(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_nav_controller_output_t *p = (mavlink_nav_controller_output_t *)&msg->payload[0]; + return (int16_t)(p->target_bearing); } /** @@ -184,10 +201,8 @@ static inline int16_t mavlink_msg_nav_controller_output_get_target_bearing(const */ static inline uint16_t mavlink_msg_nav_controller_output_get_wp_dist(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (uint16_t)r.s; + mavlink_nav_controller_output_t *p = (mavlink_nav_controller_output_t *)&msg->payload[0]; + return (uint16_t)(p->wp_dist); } /** @@ -197,12 +212,8 @@ static inline uint16_t mavlink_msg_nav_controller_output_get_wp_dist(const mavli */ static inline float mavlink_msg_nav_controller_output_get_alt_error(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t)+sizeof(uint16_t))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t)+sizeof(uint16_t))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t)+sizeof(uint16_t))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t)+sizeof(uint16_t))[3]; - return (float)r.f; + mavlink_nav_controller_output_t *p = (mavlink_nav_controller_output_t *)&msg->payload[0]; + return (float)(p->alt_error); } /** @@ -212,12 +223,8 @@ static inline float mavlink_msg_nav_controller_output_get_alt_error(const mavlin */ static inline float mavlink_msg_nav_controller_output_get_aspd_error(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t)+sizeof(uint16_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t)+sizeof(uint16_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t)+sizeof(uint16_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t)+sizeof(uint16_t)+sizeof(float))[3]; - return (float)r.f; + mavlink_nav_controller_output_t *p = (mavlink_nav_controller_output_t *)&msg->payload[0]; + return (float)(p->aspd_error); } /** @@ -227,12 +234,8 @@ static inline float mavlink_msg_nav_controller_output_get_aspd_error(const mavli */ static inline float mavlink_msg_nav_controller_output_get_xtrack_error(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_nav_controller_output_t *p = (mavlink_nav_controller_output_t *)&msg->payload[0]; + return (float)(p->xtrack_error); } /** @@ -243,12 +246,5 @@ static inline float mavlink_msg_nav_controller_output_get_xtrack_error(const mav */ static inline void mavlink_msg_nav_controller_output_decode(const mavlink_message_t* msg, mavlink_nav_controller_output_t* nav_controller_output) { - nav_controller_output->nav_roll = mavlink_msg_nav_controller_output_get_nav_roll(msg); - nav_controller_output->nav_pitch = mavlink_msg_nav_controller_output_get_nav_pitch(msg); - nav_controller_output->nav_bearing = mavlink_msg_nav_controller_output_get_nav_bearing(msg); - nav_controller_output->target_bearing = mavlink_msg_nav_controller_output_get_target_bearing(msg); - nav_controller_output->wp_dist = mavlink_msg_nav_controller_output_get_wp_dist(msg); - nav_controller_output->alt_error = mavlink_msg_nav_controller_output_get_alt_error(msg); - nav_controller_output->aspd_error = mavlink_msg_nav_controller_output_get_aspd_error(msg); - nav_controller_output->xtrack_error = mavlink_msg_nav_controller_output_get_xtrack_error(msg); + memcpy( nav_controller_output, msg->payload, sizeof(mavlink_nav_controller_output_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_param_request_list.h b/thirdParty/mavlink/include/common/mavlink_msg_param_request_list.h index e4912fb591a68b8bd59e7c9c7c0915a5fbc56c20..74ed51442ea5f7430252143c31c96f01772fd804 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_param_request_list.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_param_request_list.h @@ -1,6 +1,8 @@ // MESSAGE PARAM_REQUEST_LIST PACKING #define MAVLINK_MSG_ID_PARAM_REQUEST_LIST 21 +#define MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN 2 +#define MAVLINK_MSG_21_LEN 2 typedef struct __mavlink_param_request_list_t { @@ -9,8 +11,6 @@ typedef struct __mavlink_param_request_list_t } mavlink_param_request_list_t; - - /** * @brief Pack a param_request_list message * @param system_id ID of this system @@ -23,13 +23,13 @@ typedef struct __mavlink_param_request_list_t */ static inline uint16_t mavlink_msg_param_request_list_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component) { - uint16_t i = 0; + mavlink_param_request_list_t *p = (mavlink_param_request_list_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_LIST; - 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 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN); } /** @@ -44,13 +44,13 @@ static inline uint16_t mavlink_msg_param_request_list_pack(uint8_t system_id, ui */ static inline uint16_t mavlink_msg_param_request_list_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 i = 0; + mavlink_param_request_list_t *p = (mavlink_param_request_list_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_LIST; - 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 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN); } /** @@ -73,13 +73,36 @@ static inline uint16_t mavlink_msg_param_request_list_encode(uint8_t system_id, * @param target_system System ID * @param target_component Component ID */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_param_request_list_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) { - mavlink_message_t msg; - mavlink_msg_param_request_list_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_param_request_list_t payload; + uint16_t checksum; + mavlink_param_request_list_t *p = &payload; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN; + hdr.msgid = MAVLINK_MSG_ID_PARAM_REQUEST_LIST; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -92,7 +115,8 @@ static inline void mavlink_msg_param_request_list_send(mavlink_channel_t chan, u */ static inline uint8_t mavlink_msg_param_request_list_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_param_request_list_t *p = (mavlink_param_request_list_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); } /** @@ -102,7 +126,8 @@ static inline uint8_t mavlink_msg_param_request_list_get_target_system(const mav */ static inline uint8_t mavlink_msg_param_request_list_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_param_request_list_t *p = (mavlink_param_request_list_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); } /** @@ -113,6 +138,5 @@ static inline uint8_t mavlink_msg_param_request_list_get_target_component(const */ static inline void mavlink_msg_param_request_list_decode(const mavlink_message_t* msg, mavlink_param_request_list_t* param_request_list) { - param_request_list->target_system = mavlink_msg_param_request_list_get_target_system(msg); - param_request_list->target_component = mavlink_msg_param_request_list_get_target_component(msg); + memcpy( param_request_list, msg->payload, sizeof(mavlink_param_request_list_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_param_request_read.h b/thirdParty/mavlink/include/common/mavlink_msg_param_request_read.h index 2cd9d2932ee35c6f2f48f72bcf1140d6cef1541f..86c0a4b1d355a4b77eec0aecbe3f6df7e44e7e9e 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_param_request_read.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_param_request_read.h @@ -1,18 +1,18 @@ // MESSAGE PARAM_REQUEST_READ PACKING #define MAVLINK_MSG_ID_PARAM_REQUEST_READ 20 +#define MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN 20 +#define MAVLINK_MSG_20_LEN 20 typedef struct __mavlink_param_request_read_t { + int16_t param_index; ///< Parameter index. Send -1 to use the param ID field as identifier uint8_t target_system; ///< System ID uint8_t target_component; ///< Component ID - int8_t param_id[15]; ///< Onboard parameter id - int16_t param_index; ///< Parameter index. Send -1 to use the param ID field as identifier + char param_id[16]; ///< Onboard parameter id } mavlink_param_request_read_t; - -#define MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN 15 - +#define MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN 16 /** * @brief Pack a param_request_read message @@ -26,17 +26,17 @@ typedef struct __mavlink_param_request_read_t * @param param_index Parameter index. Send -1 to use the param ID field as identifier * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_param_request_read_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, const int8_t* param_id, int16_t param_index) +static inline uint16_t mavlink_msg_param_request_read_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, const char* param_id, int16_t param_index) { - uint16_t i = 0; + mavlink_param_request_read_t *p = (mavlink_param_request_read_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_READ; - 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_array_by_index(param_id, 15, i, msg->payload); // Onboard parameter id - i += put_int16_t_by_index(param_index, i, msg->payload); // Parameter index. Send -1 to use the param ID field as identifier + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[16]:Onboard parameter id + p->param_index = param_index; // int16_t:Parameter index. Send -1 to use the param ID field as identifier - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN); } /** @@ -51,17 +51,17 @@ static inline uint16_t mavlink_msg_param_request_read_pack(uint8_t system_id, ui * @param param_index Parameter index. Send -1 to use the param ID field as identifier * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_param_request_read_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, const int8_t* param_id, int16_t param_index) +static inline uint16_t mavlink_msg_param_request_read_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, const char* param_id, int16_t param_index) { - uint16_t i = 0; + mavlink_param_request_read_t *p = (mavlink_param_request_read_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_READ; - 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_array_by_index(param_id, 15, i, msg->payload); // Onboard parameter id - i += put_int16_t_by_index(param_index, i, msg->payload); // Parameter index. Send -1 to use the param ID field as identifier + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[16]:Onboard parameter id + p->param_index = param_index; // int16_t:Parameter index. Send -1 to use the param ID field as identifier - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN); } /** @@ -86,13 +86,38 @@ static inline uint16_t mavlink_msg_param_request_read_encode(uint8_t system_id, * @param param_id Onboard parameter id * @param param_index Parameter index. Send -1 to use the param ID field as identifier */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_param_request_read_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const int8_t* param_id, int16_t param_index) + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS +static inline void mavlink_msg_param_request_read_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const char* param_id, int16_t param_index) { - mavlink_message_t msg; - mavlink_msg_param_request_read_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, param_id, param_index); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_param_request_read_t payload; + uint16_t checksum; + mavlink_param_request_read_t *p = &payload; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[16]:Onboard parameter id + p->param_index = param_index; // int16_t:Parameter index. Send -1 to use the param ID field as identifier + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN; + hdr.msgid = MAVLINK_MSG_ID_PARAM_REQUEST_READ; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -105,7 +130,8 @@ static inline void mavlink_msg_param_request_read_send(mavlink_channel_t chan, u */ static inline uint8_t mavlink_msg_param_request_read_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_param_request_read_t *p = (mavlink_param_request_read_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); } /** @@ -115,7 +141,8 @@ static inline uint8_t mavlink_msg_param_request_read_get_target_system(const mav */ static inline uint8_t mavlink_msg_param_request_read_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_param_request_read_t *p = (mavlink_param_request_read_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); } /** @@ -123,11 +150,12 @@ static inline uint8_t mavlink_msg_param_request_read_get_target_component(const * * @return Onboard parameter id */ -static inline uint16_t mavlink_msg_param_request_read_get_param_id(const mavlink_message_t* msg, int8_t* r_data) +static inline uint16_t mavlink_msg_param_request_read_get_param_id(const mavlink_message_t* msg, char* param_id) { + mavlink_param_request_read_t *p = (mavlink_param_request_read_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint8_t)+sizeof(uint8_t), 15); - return 15; + memcpy(param_id, p->param_id, sizeof(p->param_id)); + return sizeof(p->param_id); } /** @@ -137,10 +165,8 @@ static inline uint16_t mavlink_msg_param_request_read_get_param_id(const mavlink */ static inline int16_t mavlink_msg_param_request_read_get_param_index(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+15)[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+15)[1]; - return (int16_t)r.s; + mavlink_param_request_read_t *p = (mavlink_param_request_read_t *)&msg->payload[0]; + return (int16_t)(p->param_index); } /** @@ -151,8 +177,5 @@ static inline int16_t mavlink_msg_param_request_read_get_param_index(const mavli */ static inline void mavlink_msg_param_request_read_decode(const mavlink_message_t* msg, mavlink_param_request_read_t* param_request_read) { - param_request_read->target_system = mavlink_msg_param_request_read_get_target_system(msg); - param_request_read->target_component = mavlink_msg_param_request_read_get_target_component(msg); - mavlink_msg_param_request_read_get_param_id(msg, param_request_read->param_id); - param_request_read->param_index = mavlink_msg_param_request_read_get_param_index(msg); + memcpy( param_request_read, msg->payload, sizeof(mavlink_param_request_read_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_param_set.h b/thirdParty/mavlink/include/common/mavlink_msg_param_set.h index de1c3140221526b23f87edd935f306dd4cdcf9f6..7dfb5b6dccd7391f6731f21a6b0ad8d54dbea1cb 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_param_set.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_param_set.h @@ -1,18 +1,18 @@ // MESSAGE PARAM_SET PACKING #define MAVLINK_MSG_ID_PARAM_SET 23 +#define MAVLINK_MSG_ID_PARAM_SET_LEN 22 +#define MAVLINK_MSG_23_LEN 22 typedef struct __mavlink_param_set_t { + float param_value; ///< Onboard parameter value uint8_t target_system; ///< System ID uint8_t target_component; ///< Component ID - int8_t param_id[15]; ///< Onboard parameter id - float param_value; ///< Onboard parameter value + char param_id[16]; ///< Onboard parameter id } mavlink_param_set_t; - -#define MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN 15 - +#define MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN 16 /** * @brief Pack a param_set message @@ -26,17 +26,17 @@ typedef struct __mavlink_param_set_t * @param param_value Onboard parameter value * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_param_set_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, const int8_t* param_id, float param_value) +static inline uint16_t mavlink_msg_param_set_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, const char* param_id, float param_value) { - uint16_t i = 0; + mavlink_param_set_t *p = (mavlink_param_set_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_PARAM_SET; - 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_array_by_index(param_id, 15, i, msg->payload); // Onboard parameter id - i += put_float_by_index(param_value, i, msg->payload); // Onboard parameter value + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[16]:Onboard parameter id + p->param_value = param_value; // float:Onboard parameter value - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_SET_LEN); } /** @@ -51,17 +51,17 @@ static inline uint16_t mavlink_msg_param_set_pack(uint8_t system_id, uint8_t com * @param param_value Onboard parameter value * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_param_set_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, const int8_t* param_id, float param_value) +static inline uint16_t mavlink_msg_param_set_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, const char* param_id, float param_value) { - uint16_t i = 0; + mavlink_param_set_t *p = (mavlink_param_set_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_PARAM_SET; - 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_array_by_index(param_id, 15, i, msg->payload); // Onboard parameter id - i += put_float_by_index(param_value, i, msg->payload); // Onboard parameter value + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[16]:Onboard parameter id + p->param_value = param_value; // float:Onboard parameter value - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_SET_LEN); } /** @@ -86,13 +86,38 @@ static inline uint16_t mavlink_msg_param_set_encode(uint8_t system_id, uint8_t c * @param param_id Onboard parameter id * @param param_value Onboard parameter value */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_param_set_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const int8_t* param_id, float param_value) + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS +static inline void mavlink_msg_param_set_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const char* param_id, float param_value) { - mavlink_message_t msg; - mavlink_msg_param_set_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, param_id, param_value); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_param_set_t payload; + uint16_t checksum; + mavlink_param_set_t *p = &payload; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[16]:Onboard parameter id + p->param_value = param_value; // float:Onboard parameter value + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_PARAM_SET_LEN; + hdr.msgid = MAVLINK_MSG_ID_PARAM_SET; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -105,7 +130,8 @@ static inline void mavlink_msg_param_set_send(mavlink_channel_t chan, uint8_t ta */ static inline uint8_t mavlink_msg_param_set_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_param_set_t *p = (mavlink_param_set_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); } /** @@ -115,7 +141,8 @@ static inline uint8_t mavlink_msg_param_set_get_target_system(const mavlink_mess */ static inline uint8_t mavlink_msg_param_set_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_param_set_t *p = (mavlink_param_set_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); } /** @@ -123,11 +150,12 @@ static inline uint8_t mavlink_msg_param_set_get_target_component(const mavlink_m * * @return Onboard parameter id */ -static inline uint16_t mavlink_msg_param_set_get_param_id(const mavlink_message_t* msg, int8_t* r_data) +static inline uint16_t mavlink_msg_param_set_get_param_id(const mavlink_message_t* msg, char* param_id) { + mavlink_param_set_t *p = (mavlink_param_set_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint8_t)+sizeof(uint8_t), 15); - return 15; + memcpy(param_id, p->param_id, sizeof(p->param_id)); + return sizeof(p->param_id); } /** @@ -137,12 +165,8 @@ static inline uint16_t mavlink_msg_param_set_get_param_id(const mavlink_message_ */ static inline float mavlink_msg_param_set_get_param_value(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+15)[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+15)[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+15)[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+15)[3]; - return (float)r.f; + mavlink_param_set_t *p = (mavlink_param_set_t *)&msg->payload[0]; + return (float)(p->param_value); } /** @@ -153,8 +177,5 @@ static inline float mavlink_msg_param_set_get_param_value(const mavlink_message_ */ static inline void mavlink_msg_param_set_decode(const mavlink_message_t* msg, mavlink_param_set_t* param_set) { - param_set->target_system = mavlink_msg_param_set_get_target_system(msg); - param_set->target_component = mavlink_msg_param_set_get_target_component(msg); - mavlink_msg_param_set_get_param_id(msg, param_set->param_id); - param_set->param_value = mavlink_msg_param_set_get_param_value(msg); + memcpy( param_set, msg->payload, sizeof(mavlink_param_set_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_param_value.h b/thirdParty/mavlink/include/common/mavlink_msg_param_value.h index 239c96c42f6f4303d16496f74ddf181e22cf4ee2..19f618ab4404abb4b55d25bf96d884564c2d9865 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_param_value.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_param_value.h @@ -1,18 +1,18 @@ // MESSAGE PARAM_VALUE PACKING #define MAVLINK_MSG_ID_PARAM_VALUE 22 +#define MAVLINK_MSG_ID_PARAM_VALUE_LEN 24 +#define MAVLINK_MSG_22_LEN 24 typedef struct __mavlink_param_value_t { - int8_t param_id[15]; ///< Onboard parameter id float param_value; ///< Onboard parameter value uint16_t param_count; ///< Total number of onboard parameters uint16_t param_index; ///< Index of this onboard parameter + char param_id[16]; ///< Onboard parameter id } mavlink_param_value_t; - -#define MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN 15 - +#define MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN 16 /** * @brief Pack a param_value message @@ -26,17 +26,17 @@ typedef struct __mavlink_param_value_t * @param param_index Index of this onboard parameter * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_param_value_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const int8_t* param_id, float param_value, uint16_t param_count, uint16_t param_index) +static inline uint16_t mavlink_msg_param_value_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const char* param_id, float param_value, uint16_t param_count, uint16_t param_index) { - uint16_t i = 0; + mavlink_param_value_t *p = (mavlink_param_value_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_PARAM_VALUE; - i += put_array_by_index(param_id, 15, i, msg->payload); // Onboard parameter id - i += put_float_by_index(param_value, i, msg->payload); // Onboard parameter value - i += put_uint16_t_by_index(param_count, i, msg->payload); // Total number of onboard parameters - i += put_uint16_t_by_index(param_index, i, msg->payload); // Index of this onboard parameter + memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[16]:Onboard parameter id + p->param_value = param_value; // float:Onboard parameter value + p->param_count = param_count; // uint16_t:Total number of onboard parameters + p->param_index = param_index; // uint16_t:Index of this onboard parameter - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_VALUE_LEN); } /** @@ -51,17 +51,17 @@ static inline uint16_t mavlink_msg_param_value_pack(uint8_t system_id, uint8_t c * @param param_index Index of this onboard parameter * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_param_value_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const int8_t* param_id, float param_value, uint16_t param_count, uint16_t param_index) +static inline uint16_t mavlink_msg_param_value_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const char* param_id, float param_value, uint16_t param_count, uint16_t param_index) { - uint16_t i = 0; + mavlink_param_value_t *p = (mavlink_param_value_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_PARAM_VALUE; - i += put_array_by_index(param_id, 15, i, msg->payload); // Onboard parameter id - i += put_float_by_index(param_value, i, msg->payload); // Onboard parameter value - i += put_uint16_t_by_index(param_count, i, msg->payload); // Total number of onboard parameters - i += put_uint16_t_by_index(param_index, i, msg->payload); // Index of this onboard parameter + memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[16]:Onboard parameter id + p->param_value = param_value; // float:Onboard parameter value + p->param_count = param_count; // uint16_t:Total number of onboard parameters + p->param_index = param_index; // uint16_t:Index of this onboard parameter - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_VALUE_LEN); } /** @@ -86,13 +86,38 @@ static inline uint16_t mavlink_msg_param_value_encode(uint8_t system_id, uint8_t * @param param_count Total number of onboard parameters * @param param_index Index of this onboard parameter */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_param_value_send(mavlink_channel_t chan, const int8_t* param_id, float param_value, uint16_t param_count, uint16_t param_index) + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS +static inline void mavlink_msg_param_value_send(mavlink_channel_t chan, const char* param_id, float param_value, uint16_t param_count, uint16_t param_index) { - mavlink_message_t msg; - mavlink_msg_param_value_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, param_id, param_value, param_count, param_index); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_param_value_t payload; + uint16_t checksum; + mavlink_param_value_t *p = &payload; + + memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[16]:Onboard parameter id + p->param_value = param_value; // float:Onboard parameter value + p->param_count = param_count; // uint16_t:Total number of onboard parameters + p->param_index = param_index; // uint16_t:Index of this onboard parameter + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_PARAM_VALUE_LEN; + hdr.msgid = MAVLINK_MSG_ID_PARAM_VALUE; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -103,11 +128,12 @@ static inline void mavlink_msg_param_value_send(mavlink_channel_t chan, const in * * @return Onboard parameter id */ -static inline uint16_t mavlink_msg_param_value_get_param_id(const mavlink_message_t* msg, int8_t* r_data) +static inline uint16_t mavlink_msg_param_value_get_param_id(const mavlink_message_t* msg, char* param_id) { + mavlink_param_value_t *p = (mavlink_param_value_t *)&msg->payload[0]; - memcpy(r_data, msg->payload, 15); - return 15; + memcpy(param_id, p->param_id, sizeof(p->param_id)); + return sizeof(p->param_id); } /** @@ -117,12 +143,8 @@ static inline uint16_t mavlink_msg_param_value_get_param_id(const mavlink_messag */ static inline float mavlink_msg_param_value_get_param_value(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+15)[0]; - r.b[2] = (msg->payload+15)[1]; - r.b[1] = (msg->payload+15)[2]; - r.b[0] = (msg->payload+15)[3]; - return (float)r.f; + mavlink_param_value_t *p = (mavlink_param_value_t *)&msg->payload[0]; + return (float)(p->param_value); } /** @@ -132,10 +154,8 @@ static inline float mavlink_msg_param_value_get_param_value(const mavlink_messag */ static inline uint16_t mavlink_msg_param_value_get_param_count(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+15+sizeof(float))[0]; - r.b[0] = (msg->payload+15+sizeof(float))[1]; - return (uint16_t)r.s; + mavlink_param_value_t *p = (mavlink_param_value_t *)&msg->payload[0]; + return (uint16_t)(p->param_count); } /** @@ -145,10 +165,8 @@ static inline uint16_t mavlink_msg_param_value_get_param_count(const mavlink_mes */ static inline uint16_t mavlink_msg_param_value_get_param_index(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+15+sizeof(float)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+15+sizeof(float)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_param_value_t *p = (mavlink_param_value_t *)&msg->payload[0]; + return (uint16_t)(p->param_index); } /** @@ -159,8 +177,5 @@ static inline uint16_t mavlink_msg_param_value_get_param_index(const mavlink_mes */ static inline void mavlink_msg_param_value_decode(const mavlink_message_t* msg, mavlink_param_value_t* param_value) { - mavlink_msg_param_value_get_param_id(msg, param_value->param_id); - param_value->param_value = mavlink_msg_param_value_get_param_value(msg); - param_value->param_count = mavlink_msg_param_value_get_param_count(msg); - param_value->param_index = mavlink_msg_param_value_get_param_index(msg); + memcpy( param_value, msg->payload, sizeof(mavlink_param_value_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_ping.h b/thirdParty/mavlink/include/common/mavlink_msg_ping.h index 2a27b1e5cb4fbb6be7ab00ae89d9f310dd248346..f22a195c79680f912bca5ed20499cfa20685e585 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_ping.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_ping.h @@ -1,18 +1,18 @@ // MESSAGE PING PACKING #define MAVLINK_MSG_ID_PING 3 +#define MAVLINK_MSG_ID_PING_LEN 14 +#define MAVLINK_MSG_3_LEN 14 typedef struct __mavlink_ping_t { + uint64_t time; ///< Unix timestamp in microseconds uint32_t seq; ///< PING sequence uint8_t target_system; ///< 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 uint8_t target_component; ///< 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 - uint64_t time; ///< Unix timestamp in microseconds } mavlink_ping_t; - - /** * @brief Pack a ping message * @param system_id ID of this system @@ -27,15 +27,15 @@ typedef struct __mavlink_ping_t */ static inline uint16_t mavlink_msg_ping_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint32_t seq, uint8_t target_system, uint8_t target_component, uint64_t time) { - uint16_t i = 0; + mavlink_ping_t *p = (mavlink_ping_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_PING; - i += put_uint32_t_by_index(seq, i, msg->payload); // PING sequence - i += put_uint8_t_by_index(target_system, i, msg->payload); // 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 - i += put_uint8_t_by_index(target_component, i, msg->payload); // 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 - i += put_uint64_t_by_index(time, i, msg->payload); // Unix timestamp in microseconds + p->seq = seq; // uint32_t:PING sequence + p->target_system = target_system; // uint8_t: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 + p->target_component = target_component; // uint8_t: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 + p->time = time; // uint64_t:Unix timestamp in microseconds - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PING_LEN); } /** @@ -52,15 +52,15 @@ static inline uint16_t mavlink_msg_ping_pack(uint8_t system_id, uint8_t componen */ static inline uint16_t mavlink_msg_ping_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint32_t seq, uint8_t target_system, uint8_t target_component, uint64_t time) { - uint16_t i = 0; + mavlink_ping_t *p = (mavlink_ping_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_PING; - i += put_uint32_t_by_index(seq, i, msg->payload); // PING sequence - i += put_uint8_t_by_index(target_system, i, msg->payload); // 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 - i += put_uint8_t_by_index(target_component, i, msg->payload); // 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 - i += put_uint64_t_by_index(time, i, msg->payload); // Unix timestamp in microseconds + p->seq = seq; // uint32_t:PING sequence + p->target_system = target_system; // uint8_t: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 + p->target_component = target_component; // uint8_t: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 + p->time = time; // uint64_t:Unix timestamp in microseconds - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PING_LEN); } /** @@ -85,13 +85,38 @@ static inline uint16_t mavlink_msg_ping_encode(uint8_t system_id, uint8_t compon * @param target_component 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 * @param time Unix timestamp in microseconds */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_ping_send(mavlink_channel_t chan, uint32_t seq, uint8_t target_system, uint8_t target_component, uint64_t time) { - mavlink_message_t msg; - mavlink_msg_ping_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, seq, target_system, target_component, time); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_ping_t payload; + uint16_t checksum; + mavlink_ping_t *p = &payload; + + p->seq = seq; // uint32_t:PING sequence + p->target_system = target_system; // uint8_t: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 + p->target_component = target_component; // uint8_t: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 + p->time = time; // uint64_t:Unix timestamp in microseconds + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_PING_LEN; + hdr.msgid = MAVLINK_MSG_ID_PING; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -104,12 +129,8 @@ static inline void mavlink_msg_ping_send(mavlink_channel_t chan, uint32_t seq, u */ static inline uint32_t mavlink_msg_ping_get_seq(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; + mavlink_ping_t *p = (mavlink_ping_t *)&msg->payload[0]; + return (uint32_t)(p->seq); } /** @@ -119,7 +140,8 @@ static inline uint32_t mavlink_msg_ping_get_seq(const mavlink_message_t* msg) */ static inline uint8_t mavlink_msg_ping_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint32_t))[0]; + mavlink_ping_t *p = (mavlink_ping_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); } /** @@ -129,7 +151,8 @@ static inline uint8_t mavlink_msg_ping_get_target_system(const mavlink_message_t */ static inline uint8_t mavlink_msg_ping_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint32_t)+sizeof(uint8_t))[0]; + mavlink_ping_t *p = (mavlink_ping_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); } /** @@ -139,16 +162,8 @@ static inline uint8_t mavlink_msg_ping_get_target_component(const mavlink_messag */ static inline uint64_t mavlink_msg_ping_get_time(const mavlink_message_t* msg) { - generic_64bit r; - r.b[7] = (msg->payload+sizeof(uint32_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[6] = (msg->payload+sizeof(uint32_t)+sizeof(uint8_t)+sizeof(uint8_t))[1]; - r.b[5] = (msg->payload+sizeof(uint32_t)+sizeof(uint8_t)+sizeof(uint8_t))[2]; - r.b[4] = (msg->payload+sizeof(uint32_t)+sizeof(uint8_t)+sizeof(uint8_t))[3]; - r.b[3] = (msg->payload+sizeof(uint32_t)+sizeof(uint8_t)+sizeof(uint8_t))[4]; - r.b[2] = (msg->payload+sizeof(uint32_t)+sizeof(uint8_t)+sizeof(uint8_t))[5]; - r.b[1] = (msg->payload+sizeof(uint32_t)+sizeof(uint8_t)+sizeof(uint8_t))[6]; - r.b[0] = (msg->payload+sizeof(uint32_t)+sizeof(uint8_t)+sizeof(uint8_t))[7]; - return (uint64_t)r.ll; + mavlink_ping_t *p = (mavlink_ping_t *)&msg->payload[0]; + return (uint64_t)(p->time); } /** @@ -159,8 +174,5 @@ static inline uint64_t mavlink_msg_ping_get_time(const mavlink_message_t* msg) */ static inline void mavlink_msg_ping_decode(const mavlink_message_t* msg, mavlink_ping_t* ping) { - ping->seq = mavlink_msg_ping_get_seq(msg); - ping->target_system = mavlink_msg_ping_get_target_system(msg); - ping->target_component = mavlink_msg_ping_get_target_component(msg); - ping->time = mavlink_msg_ping_get_time(msg); + memcpy( ping, msg->payload, sizeof(mavlink_ping_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_position_controller_output.h b/thirdParty/mavlink/include/common/mavlink_msg_position_controller_output.h index 389d9a2387a8b1b34fc02b5620811d42513af08a..951c293232b3bb9a5a24ef49725132e870e5a1b6 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_position_controller_output.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_position_controller_output.h @@ -1,6 +1,8 @@ // MESSAGE POSITION_CONTROLLER_OUTPUT PACKING #define MAVLINK_MSG_ID_POSITION_CONTROLLER_OUTPUT 61 +#define MAVLINK_MSG_ID_POSITION_CONTROLLER_OUTPUT_LEN 5 +#define MAVLINK_MSG_61_LEN 5 typedef struct __mavlink_position_controller_output_t { @@ -12,8 +14,6 @@ typedef struct __mavlink_position_controller_output_t } mavlink_position_controller_output_t; - - /** * @brief Pack a position_controller_output message * @param system_id ID of this system @@ -29,16 +29,16 @@ typedef struct __mavlink_position_controller_output_t */ 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; + mavlink_position_controller_output_t *p = (mavlink_position_controller_output_t *)&msg->payload[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% + p->enabled = enabled; // uint8_t:1: enabled, 0: disabled + p->x = x; // int8_t:Position x: -128: -100%, 127: +100% + p->y = y; // int8_t:Position y: -128: -100%, 127: +100% + p->z = z; // int8_t:Position z: -128: -100%, 127: +100% + p->yaw = yaw; // int8_t:Position yaw: -128: -100%, 127: +100% - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POSITION_CONTROLLER_OUTPUT_LEN); } /** @@ -56,16 +56,16 @@ static inline uint16_t mavlink_msg_position_controller_output_pack(uint8_t syste */ 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; + mavlink_position_controller_output_t *p = (mavlink_position_controller_output_t *)&msg->payload[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% + p->enabled = enabled; // uint8_t:1: enabled, 0: disabled + p->x = x; // int8_t:Position x: -128: -100%, 127: +100% + p->y = y; // int8_t:Position y: -128: -100%, 127: +100% + p->z = z; // int8_t:Position z: -128: -100%, 127: +100% + p->yaw = yaw; // int8_t:Position yaw: -128: -100%, 127: +100% - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POSITION_CONTROLLER_OUTPUT_LEN); } /** @@ -91,13 +91,39 @@ static inline uint16_t mavlink_msg_position_controller_output_encode(uint8_t sys * @param z Position z: -128: -100%, 127: +100% * @param yaw Position yaw: -128: -100%, 127: +100% */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#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); + mavlink_header_t hdr; + mavlink_position_controller_output_t payload; + uint16_t checksum; + mavlink_position_controller_output_t *p = &payload; + + p->enabled = enabled; // uint8_t:1: enabled, 0: disabled + p->x = x; // int8_t:Position x: -128: -100%, 127: +100% + p->y = y; // int8_t:Position y: -128: -100%, 127: +100% + p->z = z; // int8_t:Position z: -128: -100%, 127: +100% + p->yaw = yaw; // int8_t:Position yaw: -128: -100%, 127: +100% + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_POSITION_CONTROLLER_OUTPUT_LEN; + hdr.msgid = MAVLINK_MSG_ID_POSITION_CONTROLLER_OUTPUT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -110,7 +136,8 @@ static inline void mavlink_msg_position_controller_output_send(mavlink_channel_t */ static inline uint8_t mavlink_msg_position_controller_output_get_enabled(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_position_controller_output_t *p = (mavlink_position_controller_output_t *)&msg->payload[0]; + return (uint8_t)(p->enabled); } /** @@ -120,7 +147,8 @@ static inline uint8_t mavlink_msg_position_controller_output_get_enabled(const m */ 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]; + mavlink_position_controller_output_t *p = (mavlink_position_controller_output_t *)&msg->payload[0]; + return (int8_t)(p->x); } /** @@ -130,7 +158,8 @@ static inline int8_t mavlink_msg_position_controller_output_get_x(const mavlink_ */ 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]; + mavlink_position_controller_output_t *p = (mavlink_position_controller_output_t *)&msg->payload[0]; + return (int8_t)(p->y); } /** @@ -140,7 +169,8 @@ static inline int8_t mavlink_msg_position_controller_output_get_y(const mavlink_ */ 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]; + mavlink_position_controller_output_t *p = (mavlink_position_controller_output_t *)&msg->payload[0]; + return (int8_t)(p->z); } /** @@ -150,7 +180,8 @@ static inline int8_t mavlink_msg_position_controller_output_get_z(const mavlink_ */ 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]; + mavlink_position_controller_output_t *p = (mavlink_position_controller_output_t *)&msg->payload[0]; + return (int8_t)(p->yaw); } /** @@ -161,9 +192,5 @@ static inline int8_t mavlink_msg_position_controller_output_get_yaw(const mavlin */ 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); + memcpy( position_controller_output, msg->payload, sizeof(mavlink_position_controller_output_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_position_target.h b/thirdParty/mavlink/include/common/mavlink_msg_position_target.h index d6e4da3dd4e2db91448c4d1d88ffe7a021ff39d4..1a65ed0a405289f7dd32b608854b54361be5890b 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_position_target.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_position_target.h @@ -1,6 +1,8 @@ // MESSAGE POSITION_TARGET PACKING #define MAVLINK_MSG_ID_POSITION_TARGET 63 +#define MAVLINK_MSG_ID_POSITION_TARGET_LEN 16 +#define MAVLINK_MSG_63_LEN 16 typedef struct __mavlink_position_target_t { @@ -11,8 +13,6 @@ typedef struct __mavlink_position_target_t } mavlink_position_target_t; - - /** * @brief Pack a position_target message * @param system_id ID of this system @@ -27,15 +27,15 @@ typedef struct __mavlink_position_target_t */ static inline uint16_t mavlink_msg_position_target_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, float x, float y, float z, float yaw) { - uint16_t i = 0; + mavlink_position_target_t *p = (mavlink_position_target_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POSITION_TARGET; - i += put_float_by_index(x, i, msg->payload); // x position - i += put_float_by_index(y, i, msg->payload); // y position - i += put_float_by_index(z, i, msg->payload); // z position - i += put_float_by_index(yaw, i, msg->payload); // yaw orientation in radians, 0 = NORTH + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float:yaw orientation in radians, 0 = NORTH - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POSITION_TARGET_LEN); } /** @@ -52,15 +52,15 @@ static inline uint16_t mavlink_msg_position_target_pack(uint8_t system_id, uint8 */ static inline uint16_t mavlink_msg_position_target_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, float x, float y, float z, float yaw) { - uint16_t i = 0; + mavlink_position_target_t *p = (mavlink_position_target_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POSITION_TARGET; - i += put_float_by_index(x, i, msg->payload); // x position - i += put_float_by_index(y, i, msg->payload); // y position - i += put_float_by_index(z, i, msg->payload); // z position - i += put_float_by_index(yaw, i, msg->payload); // yaw orientation in radians, 0 = NORTH + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float:yaw orientation in radians, 0 = NORTH - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POSITION_TARGET_LEN); } /** @@ -85,13 +85,38 @@ static inline uint16_t mavlink_msg_position_target_encode(uint8_t system_id, uin * @param z z position * @param yaw yaw orientation in radians, 0 = NORTH */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_position_target_send(mavlink_channel_t chan, float x, float y, float z, float yaw) { - mavlink_message_t msg; - mavlink_msg_position_target_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, x, y, z, yaw); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_position_target_t payload; + uint16_t checksum; + mavlink_position_target_t *p = &payload; + + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float:yaw orientation in radians, 0 = NORTH + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_POSITION_TARGET_LEN; + hdr.msgid = MAVLINK_MSG_ID_POSITION_TARGET; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -104,12 +129,8 @@ static inline void mavlink_msg_position_target_send(mavlink_channel_t chan, floa */ static inline float mavlink_msg_position_target_get_x(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 (float)r.f; + mavlink_position_target_t *p = (mavlink_position_target_t *)&msg->payload[0]; + return (float)(p->x); } /** @@ -119,12 +140,8 @@ static inline float mavlink_msg_position_target_get_x(const mavlink_message_t* m */ static inline float mavlink_msg_position_target_get_y(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float))[3]; - return (float)r.f; + mavlink_position_target_t *p = (mavlink_position_target_t *)&msg->payload[0]; + return (float)(p->y); } /** @@ -134,12 +151,8 @@ static inline float mavlink_msg_position_target_get_y(const mavlink_message_t* m */ static inline float mavlink_msg_position_target_get_z(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_position_target_t *p = (mavlink_position_target_t *)&msg->payload[0]; + return (float)(p->z); } /** @@ -149,12 +162,8 @@ static inline float mavlink_msg_position_target_get_z(const mavlink_message_t* m */ static inline float mavlink_msg_position_target_get_yaw(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_position_target_t *p = (mavlink_position_target_t *)&msg->payload[0]; + return (float)(p->yaw); } /** @@ -165,8 +174,5 @@ static inline float mavlink_msg_position_target_get_yaw(const mavlink_message_t* */ static inline void mavlink_msg_position_target_decode(const mavlink_message_t* msg, mavlink_position_target_t* position_target) { - position_target->x = mavlink_msg_position_target_get_x(msg); - position_target->y = mavlink_msg_position_target_get_y(msg); - position_target->z = mavlink_msg_position_target_get_z(msg); - position_target->yaw = mavlink_msg_position_target_get_yaw(msg); + memcpy( position_target, msg->payload, sizeof(mavlink_position_target_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_raw_imu.h b/thirdParty/mavlink/include/common/mavlink_msg_raw_imu.h index 4ded0b71198d53b814d3fa6fa9fbdd106a2e94fd..aa0f306727d79d17928c03468aaca3ea2c4780a2 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_raw_imu.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_raw_imu.h @@ -1,6 +1,8 @@ // MESSAGE RAW_IMU PACKING #define MAVLINK_MSG_ID_RAW_IMU 28 +#define MAVLINK_MSG_ID_RAW_IMU_LEN 26 +#define MAVLINK_MSG_28_LEN 26 typedef struct __mavlink_raw_imu_t { @@ -17,8 +19,6 @@ typedef struct __mavlink_raw_imu_t } mavlink_raw_imu_t; - - /** * @brief Pack a raw_imu message * @param system_id ID of this system @@ -39,21 +39,21 @@ typedef struct __mavlink_raw_imu_t */ static inline uint16_t mavlink_msg_raw_imu_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t usec, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) { - uint16_t i = 0; + mavlink_raw_imu_t *p = (mavlink_raw_imu_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_RAW_IMU; - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - i += put_int16_t_by_index(xacc, i, msg->payload); // X acceleration (raw) - i += put_int16_t_by_index(yacc, i, msg->payload); // Y acceleration (raw) - i += put_int16_t_by_index(zacc, i, msg->payload); // Z acceleration (raw) - i += put_int16_t_by_index(xgyro, i, msg->payload); // Angular speed around X axis (raw) - i += put_int16_t_by_index(ygyro, i, msg->payload); // Angular speed around Y axis (raw) - i += put_int16_t_by_index(zgyro, i, msg->payload); // Angular speed around Z axis (raw) - i += put_int16_t_by_index(xmag, i, msg->payload); // X Magnetic field (raw) - i += put_int16_t_by_index(ymag, i, msg->payload); // Y Magnetic field (raw) - i += put_int16_t_by_index(zmag, i, msg->payload); // Z Magnetic field (raw) + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->xacc = xacc; // int16_t:X acceleration (raw) + p->yacc = yacc; // int16_t:Y acceleration (raw) + p->zacc = zacc; // int16_t:Z acceleration (raw) + p->xgyro = xgyro; // int16_t:Angular speed around X axis (raw) + p->ygyro = ygyro; // int16_t:Angular speed around Y axis (raw) + p->zgyro = zgyro; // int16_t:Angular speed around Z axis (raw) + p->xmag = xmag; // int16_t:X Magnetic field (raw) + p->ymag = ymag; // int16_t:Y Magnetic field (raw) + p->zmag = zmag; // int16_t:Z Magnetic field (raw) - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RAW_IMU_LEN); } /** @@ -76,21 +76,21 @@ static inline uint16_t mavlink_msg_raw_imu_pack(uint8_t system_id, uint8_t compo */ static inline uint16_t mavlink_msg_raw_imu_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t usec, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) { - uint16_t i = 0; + mavlink_raw_imu_t *p = (mavlink_raw_imu_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_RAW_IMU; - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - i += put_int16_t_by_index(xacc, i, msg->payload); // X acceleration (raw) - i += put_int16_t_by_index(yacc, i, msg->payload); // Y acceleration (raw) - i += put_int16_t_by_index(zacc, i, msg->payload); // Z acceleration (raw) - i += put_int16_t_by_index(xgyro, i, msg->payload); // Angular speed around X axis (raw) - i += put_int16_t_by_index(ygyro, i, msg->payload); // Angular speed around Y axis (raw) - i += put_int16_t_by_index(zgyro, i, msg->payload); // Angular speed around Z axis (raw) - i += put_int16_t_by_index(xmag, i, msg->payload); // X Magnetic field (raw) - i += put_int16_t_by_index(ymag, i, msg->payload); // Y Magnetic field (raw) - i += put_int16_t_by_index(zmag, i, msg->payload); // Z Magnetic field (raw) + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->xacc = xacc; // int16_t:X acceleration (raw) + p->yacc = yacc; // int16_t:Y acceleration (raw) + p->zacc = zacc; // int16_t:Z acceleration (raw) + p->xgyro = xgyro; // int16_t:Angular speed around X axis (raw) + p->ygyro = ygyro; // int16_t:Angular speed around Y axis (raw) + p->zgyro = zgyro; // int16_t:Angular speed around Z axis (raw) + p->xmag = xmag; // int16_t:X Magnetic field (raw) + p->ymag = ymag; // int16_t:Y Magnetic field (raw) + p->zmag = zmag; // int16_t:Z Magnetic field (raw) - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RAW_IMU_LEN); } /** @@ -121,13 +121,44 @@ static inline uint16_t mavlink_msg_raw_imu_encode(uint8_t system_id, uint8_t com * @param ymag Y Magnetic field (raw) * @param zmag Z Magnetic field (raw) */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_raw_imu_send(mavlink_channel_t chan, uint64_t usec, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) { - mavlink_message_t msg; - mavlink_msg_raw_imu_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, usec, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_raw_imu_t payload; + uint16_t checksum; + mavlink_raw_imu_t *p = &payload; + + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->xacc = xacc; // int16_t:X acceleration (raw) + p->yacc = yacc; // int16_t:Y acceleration (raw) + p->zacc = zacc; // int16_t:Z acceleration (raw) + p->xgyro = xgyro; // int16_t:Angular speed around X axis (raw) + p->ygyro = ygyro; // int16_t:Angular speed around Y axis (raw) + p->zgyro = zgyro; // int16_t:Angular speed around Z axis (raw) + p->xmag = xmag; // int16_t:X Magnetic field (raw) + p->ymag = ymag; // int16_t:Y Magnetic field (raw) + p->zmag = zmag; // int16_t:Z Magnetic field (raw) + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_RAW_IMU_LEN; + hdr.msgid = MAVLINK_MSG_ID_RAW_IMU; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -140,16 +171,8 @@ static inline void mavlink_msg_raw_imu_send(mavlink_channel_t chan, uint64_t use */ static inline uint64_t mavlink_msg_raw_imu_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; + mavlink_raw_imu_t *p = (mavlink_raw_imu_t *)&msg->payload[0]; + return (uint64_t)(p->usec); } /** @@ -159,10 +182,8 @@ static inline uint64_t mavlink_msg_raw_imu_get_usec(const mavlink_message_t* msg */ static inline int16_t mavlink_msg_raw_imu_get_xacc(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t))[1]; - return (int16_t)r.s; + mavlink_raw_imu_t *p = (mavlink_raw_imu_t *)&msg->payload[0]; + return (int16_t)(p->xacc); } /** @@ -172,10 +193,8 @@ static inline int16_t mavlink_msg_raw_imu_get_xacc(const mavlink_message_t* msg) */ static inline int16_t mavlink_msg_raw_imu_get_yacc(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_raw_imu_t *p = (mavlink_raw_imu_t *)&msg->payload[0]; + return (int16_t)(p->yacc); } /** @@ -185,10 +204,8 @@ static inline int16_t mavlink_msg_raw_imu_get_yacc(const mavlink_message_t* msg) */ static inline int16_t mavlink_msg_raw_imu_get_zacc(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_raw_imu_t *p = (mavlink_raw_imu_t *)&msg->payload[0]; + return (int16_t)(p->zacc); } /** @@ -198,10 +215,8 @@ static inline int16_t mavlink_msg_raw_imu_get_zacc(const mavlink_message_t* msg) */ static inline int16_t mavlink_msg_raw_imu_get_xgyro(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_raw_imu_t *p = (mavlink_raw_imu_t *)&msg->payload[0]; + return (int16_t)(p->xgyro); } /** @@ -211,10 +226,8 @@ static inline int16_t mavlink_msg_raw_imu_get_xgyro(const mavlink_message_t* msg */ static inline int16_t mavlink_msg_raw_imu_get_ygyro(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_raw_imu_t *p = (mavlink_raw_imu_t *)&msg->payload[0]; + return (int16_t)(p->ygyro); } /** @@ -224,10 +237,8 @@ static inline int16_t mavlink_msg_raw_imu_get_ygyro(const mavlink_message_t* msg */ static inline int16_t mavlink_msg_raw_imu_get_zgyro(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_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(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_raw_imu_t *p = (mavlink_raw_imu_t *)&msg->payload[0]; + return (int16_t)(p->zgyro); } /** @@ -237,10 +248,8 @@ static inline int16_t mavlink_msg_raw_imu_get_zgyro(const mavlink_message_t* msg */ static inline int16_t mavlink_msg_raw_imu_get_xmag(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(int16_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(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_raw_imu_t *p = (mavlink_raw_imu_t *)&msg->payload[0]; + return (int16_t)(p->xmag); } /** @@ -250,10 +259,8 @@ static inline int16_t mavlink_msg_raw_imu_get_xmag(const mavlink_message_t* msg) */ static inline int16_t mavlink_msg_raw_imu_get_ymag(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_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(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_raw_imu_t *p = (mavlink_raw_imu_t *)&msg->payload[0]; + return (int16_t)(p->ymag); } /** @@ -263,10 +270,8 @@ static inline int16_t mavlink_msg_raw_imu_get_ymag(const mavlink_message_t* msg) */ static inline int16_t mavlink_msg_raw_imu_get_zmag(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_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(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_raw_imu_t *p = (mavlink_raw_imu_t *)&msg->payload[0]; + return (int16_t)(p->zmag); } /** @@ -277,14 +282,5 @@ static inline int16_t mavlink_msg_raw_imu_get_zmag(const mavlink_message_t* msg) */ static inline void mavlink_msg_raw_imu_decode(const mavlink_message_t* msg, mavlink_raw_imu_t* raw_imu) { - raw_imu->usec = mavlink_msg_raw_imu_get_usec(msg); - raw_imu->xacc = mavlink_msg_raw_imu_get_xacc(msg); - raw_imu->yacc = mavlink_msg_raw_imu_get_yacc(msg); - raw_imu->zacc = mavlink_msg_raw_imu_get_zacc(msg); - raw_imu->xgyro = mavlink_msg_raw_imu_get_xgyro(msg); - raw_imu->ygyro = mavlink_msg_raw_imu_get_ygyro(msg); - raw_imu->zgyro = mavlink_msg_raw_imu_get_zgyro(msg); - raw_imu->xmag = mavlink_msg_raw_imu_get_xmag(msg); - raw_imu->ymag = mavlink_msg_raw_imu_get_ymag(msg); - raw_imu->zmag = mavlink_msg_raw_imu_get_zmag(msg); + memcpy( raw_imu, msg->payload, sizeof(mavlink_raw_imu_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_raw_pressure.h b/thirdParty/mavlink/include/common/mavlink_msg_raw_pressure.h index 8346ac6286be8e1d7e7c04d704956a480c2ffd83..4d3a9ba2585e3bc39f63397c23deda90ea9c2f4b 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_raw_pressure.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_raw_pressure.h @@ -1,6 +1,8 @@ // MESSAGE RAW_PRESSURE PACKING #define MAVLINK_MSG_ID_RAW_PRESSURE 29 +#define MAVLINK_MSG_ID_RAW_PRESSURE_LEN 16 +#define MAVLINK_MSG_29_LEN 16 typedef struct __mavlink_raw_pressure_t { @@ -12,8 +14,6 @@ typedef struct __mavlink_raw_pressure_t } mavlink_raw_pressure_t; - - /** * @brief Pack a raw_pressure message * @param system_id ID of this system @@ -29,16 +29,16 @@ typedef struct __mavlink_raw_pressure_t */ static inline uint16_t mavlink_msg_raw_pressure_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t usec, int16_t press_abs, int16_t press_diff1, int16_t press_diff2, int16_t temperature) { - uint16_t i = 0; + mavlink_raw_pressure_t *p = (mavlink_raw_pressure_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_RAW_PRESSURE; - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - i += put_int16_t_by_index(press_abs, i, msg->payload); // Absolute pressure (raw) - i += put_int16_t_by_index(press_diff1, i, msg->payload); // Differential pressure 1 (raw) - i += put_int16_t_by_index(press_diff2, i, msg->payload); // Differential pressure 2 (raw) - i += put_int16_t_by_index(temperature, i, msg->payload); // Raw Temperature measurement (raw) + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->press_abs = press_abs; // int16_t:Absolute pressure (raw) + p->press_diff1 = press_diff1; // int16_t:Differential pressure 1 (raw) + p->press_diff2 = press_diff2; // int16_t:Differential pressure 2 (raw) + p->temperature = temperature; // int16_t:Raw Temperature measurement (raw) - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RAW_PRESSURE_LEN); } /** @@ -56,16 +56,16 @@ static inline uint16_t mavlink_msg_raw_pressure_pack(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_raw_pressure_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t usec, int16_t press_abs, int16_t press_diff1, int16_t press_diff2, int16_t temperature) { - uint16_t i = 0; + mavlink_raw_pressure_t *p = (mavlink_raw_pressure_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_RAW_PRESSURE; - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - i += put_int16_t_by_index(press_abs, i, msg->payload); // Absolute pressure (raw) - i += put_int16_t_by_index(press_diff1, i, msg->payload); // Differential pressure 1 (raw) - i += put_int16_t_by_index(press_diff2, i, msg->payload); // Differential pressure 2 (raw) - i += put_int16_t_by_index(temperature, i, msg->payload); // Raw Temperature measurement (raw) + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->press_abs = press_abs; // int16_t:Absolute pressure (raw) + p->press_diff1 = press_diff1; // int16_t:Differential pressure 1 (raw) + p->press_diff2 = press_diff2; // int16_t:Differential pressure 2 (raw) + p->temperature = temperature; // int16_t:Raw Temperature measurement (raw) - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RAW_PRESSURE_LEN); } /** @@ -91,13 +91,39 @@ static inline uint16_t mavlink_msg_raw_pressure_encode(uint8_t system_id, uint8_ * @param press_diff2 Differential pressure 2 (raw) * @param temperature Raw Temperature measurement (raw) */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_raw_pressure_send(mavlink_channel_t chan, uint64_t usec, int16_t press_abs, int16_t press_diff1, int16_t press_diff2, int16_t temperature) { - mavlink_message_t msg; - mavlink_msg_raw_pressure_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, usec, press_abs, press_diff1, press_diff2, temperature); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_raw_pressure_t payload; + uint16_t checksum; + mavlink_raw_pressure_t *p = &payload; + + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->press_abs = press_abs; // int16_t:Absolute pressure (raw) + p->press_diff1 = press_diff1; // int16_t:Differential pressure 1 (raw) + p->press_diff2 = press_diff2; // int16_t:Differential pressure 2 (raw) + p->temperature = temperature; // int16_t:Raw Temperature measurement (raw) + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_RAW_PRESSURE_LEN; + hdr.msgid = MAVLINK_MSG_ID_RAW_PRESSURE; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -110,16 +136,8 @@ static inline void mavlink_msg_raw_pressure_send(mavlink_channel_t chan, uint64_ */ static inline uint64_t mavlink_msg_raw_pressure_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; + mavlink_raw_pressure_t *p = (mavlink_raw_pressure_t *)&msg->payload[0]; + return (uint64_t)(p->usec); } /** @@ -129,10 +147,8 @@ static inline uint64_t mavlink_msg_raw_pressure_get_usec(const mavlink_message_t */ static inline int16_t mavlink_msg_raw_pressure_get_press_abs(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t))[1]; - return (int16_t)r.s; + mavlink_raw_pressure_t *p = (mavlink_raw_pressure_t *)&msg->payload[0]; + return (int16_t)(p->press_abs); } /** @@ -142,10 +158,8 @@ static inline int16_t mavlink_msg_raw_pressure_get_press_abs(const mavlink_messa */ static inline int16_t mavlink_msg_raw_pressure_get_press_diff1(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_raw_pressure_t *p = (mavlink_raw_pressure_t *)&msg->payload[0]; + return (int16_t)(p->press_diff1); } /** @@ -155,10 +169,8 @@ static inline int16_t mavlink_msg_raw_pressure_get_press_diff1(const mavlink_mes */ static inline int16_t mavlink_msg_raw_pressure_get_press_diff2(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_raw_pressure_t *p = (mavlink_raw_pressure_t *)&msg->payload[0]; + return (int16_t)(p->press_diff2); } /** @@ -168,10 +180,8 @@ static inline int16_t mavlink_msg_raw_pressure_get_press_diff2(const mavlink_mes */ static inline int16_t mavlink_msg_raw_pressure_get_temperature(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_raw_pressure_t *p = (mavlink_raw_pressure_t *)&msg->payload[0]; + return (int16_t)(p->temperature); } /** @@ -182,9 +192,5 @@ static inline int16_t mavlink_msg_raw_pressure_get_temperature(const mavlink_mes */ static inline void mavlink_msg_raw_pressure_decode(const mavlink_message_t* msg, mavlink_raw_pressure_t* raw_pressure) { - raw_pressure->usec = mavlink_msg_raw_pressure_get_usec(msg); - raw_pressure->press_abs = mavlink_msg_raw_pressure_get_press_abs(msg); - raw_pressure->press_diff1 = mavlink_msg_raw_pressure_get_press_diff1(msg); - raw_pressure->press_diff2 = mavlink_msg_raw_pressure_get_press_diff2(msg); - raw_pressure->temperature = mavlink_msg_raw_pressure_get_temperature(msg); + memcpy( raw_pressure, msg->payload, sizeof(mavlink_raw_pressure_t)); } 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..b9dfe85e9629eda7b9d13988d317380187c94e10 --- /dev/null +++ b/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_override.h @@ -0,0 +1,286 @@ +// MESSAGE RC_CHANNELS_OVERRIDE PACKING + +#define MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE 70 +#define MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN 18 +#define MAVLINK_MSG_70_LEN 18 + +typedef struct __mavlink_rc_channels_override_t +{ + 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 + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + +} 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) +{ + mavlink_rc_channels_override_t *p = (mavlink_rc_channels_override_t *)&msg->payload[0]; + msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->chan1_raw = chan1_raw; // uint16_t:RC channel 1 value, in microseconds + p->chan2_raw = chan2_raw; // uint16_t:RC channel 2 value, in microseconds + p->chan3_raw = chan3_raw; // uint16_t:RC channel 3 value, in microseconds + p->chan4_raw = chan4_raw; // uint16_t:RC channel 4 value, in microseconds + p->chan5_raw = chan5_raw; // uint16_t:RC channel 5 value, in microseconds + p->chan6_raw = chan6_raw; // uint16_t:RC channel 6 value, in microseconds + p->chan7_raw = chan7_raw; // uint16_t:RC channel 7 value, in microseconds + p->chan8_raw = chan8_raw; // uint16_t:RC channel 8 value, in microseconds + + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN); +} + +/** + * @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) +{ + mavlink_rc_channels_override_t *p = (mavlink_rc_channels_override_t *)&msg->payload[0]; + msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->chan1_raw = chan1_raw; // uint16_t:RC channel 1 value, in microseconds + p->chan2_raw = chan2_raw; // uint16_t:RC channel 2 value, in microseconds + p->chan3_raw = chan3_raw; // uint16_t:RC channel 3 value, in microseconds + p->chan4_raw = chan4_raw; // uint16_t:RC channel 4 value, in microseconds + p->chan5_raw = chan5_raw; // uint16_t:RC channel 5 value, in microseconds + p->chan6_raw = chan6_raw; // uint16_t:RC channel 6 value, in microseconds + p->chan7_raw = chan7_raw; // uint16_t:RC channel 7 value, in microseconds + p->chan8_raw = chan8_raw; // uint16_t:RC channel 8 value, in microseconds + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN); +} + +/** + * @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_header_t hdr; + mavlink_rc_channels_override_t payload; + uint16_t checksum; + mavlink_rc_channels_override_t *p = &payload; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->chan1_raw = chan1_raw; // uint16_t:RC channel 1 value, in microseconds + p->chan2_raw = chan2_raw; // uint16_t:RC channel 2 value, in microseconds + p->chan3_raw = chan3_raw; // uint16_t:RC channel 3 value, in microseconds + p->chan4_raw = chan4_raw; // uint16_t:RC channel 4 value, in microseconds + p->chan5_raw = chan5_raw; // uint16_t:RC channel 5 value, in microseconds + p->chan6_raw = chan6_raw; // uint16_t:RC channel 6 value, in microseconds + p->chan7_raw = chan7_raw; // uint16_t:RC channel 7 value, in microseconds + p->chan8_raw = chan8_raw; // uint16_t:RC channel 8 value, in microseconds + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN; + hdr.msgid = MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); +} + +#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) +{ + mavlink_rc_channels_override_t *p = (mavlink_rc_channels_override_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); +} + +/** + * @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) +{ + mavlink_rc_channels_override_t *p = (mavlink_rc_channels_override_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); +} + +/** + * @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) +{ + mavlink_rc_channels_override_t *p = (mavlink_rc_channels_override_t *)&msg->payload[0]; + return (uint16_t)(p->chan1_raw); +} + +/** + * @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) +{ + mavlink_rc_channels_override_t *p = (mavlink_rc_channels_override_t *)&msg->payload[0]; + return (uint16_t)(p->chan2_raw); +} + +/** + * @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) +{ + mavlink_rc_channels_override_t *p = (mavlink_rc_channels_override_t *)&msg->payload[0]; + return (uint16_t)(p->chan3_raw); +} + +/** + * @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) +{ + mavlink_rc_channels_override_t *p = (mavlink_rc_channels_override_t *)&msg->payload[0]; + return (uint16_t)(p->chan4_raw); +} + +/** + * @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) +{ + mavlink_rc_channels_override_t *p = (mavlink_rc_channels_override_t *)&msg->payload[0]; + return (uint16_t)(p->chan5_raw); +} + +/** + * @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) +{ + mavlink_rc_channels_override_t *p = (mavlink_rc_channels_override_t *)&msg->payload[0]; + return (uint16_t)(p->chan6_raw); +} + +/** + * @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) +{ + mavlink_rc_channels_override_t *p = (mavlink_rc_channels_override_t *)&msg->payload[0]; + return (uint16_t)(p->chan7_raw); +} + +/** + * @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) +{ + mavlink_rc_channels_override_t *p = (mavlink_rc_channels_override_t *)&msg->payload[0]; + return (uint16_t)(p->chan8_raw); +} + +/** + * @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) +{ + memcpy( rc_channels_override, msg->payload, sizeof(mavlink_rc_channels_override_t)); +} diff --git a/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_raw.h b/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_raw.h index 0443838bdcda0b7f5f41ffeb0080c0e0f5051608..56de4d83c80e33ada97acdcd1d74c8c5af8e5afa 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_raw.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_raw.h @@ -1,6 +1,8 @@ // MESSAGE RC_CHANNELS_RAW PACKING #define MAVLINK_MSG_ID_RC_CHANNELS_RAW 35 +#define MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN 17 +#define MAVLINK_MSG_35_LEN 17 typedef struct __mavlink_rc_channels_raw_t { @@ -16,8 +18,6 @@ typedef struct __mavlink_rc_channels_raw_t } mavlink_rc_channels_raw_t; - - /** * @brief Pack a rc_channels_raw message * @param system_id ID of this system @@ -37,20 +37,20 @@ typedef struct __mavlink_rc_channels_raw_t */ static inline uint16_t mavlink_msg_rc_channels_raw_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, 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, uint8_t rssi) { - uint16_t i = 0; + mavlink_rc_channels_raw_t *p = (mavlink_rc_channels_raw_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_RAW; - 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 - i += put_uint8_t_by_index(rssi, i, msg->payload); // Receive signal strength indicator, 0: 0%, 255: 100% + p->chan1_raw = chan1_raw; // uint16_t:RC channel 1 value, in microseconds + p->chan2_raw = chan2_raw; // uint16_t:RC channel 2 value, in microseconds + p->chan3_raw = chan3_raw; // uint16_t:RC channel 3 value, in microseconds + p->chan4_raw = chan4_raw; // uint16_t:RC channel 4 value, in microseconds + p->chan5_raw = chan5_raw; // uint16_t:RC channel 5 value, in microseconds + p->chan6_raw = chan6_raw; // uint16_t:RC channel 6 value, in microseconds + p->chan7_raw = chan7_raw; // uint16_t:RC channel 7 value, in microseconds + p->chan8_raw = chan8_raw; // uint16_t:RC channel 8 value, in microseconds + p->rssi = rssi; // uint8_t:Receive signal strength indicator, 0: 0%, 255: 100% - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN); } /** @@ -72,20 +72,20 @@ static inline uint16_t mavlink_msg_rc_channels_raw_pack(uint8_t system_id, uint8 */ static inline uint16_t mavlink_msg_rc_channels_raw_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, 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, uint8_t rssi) { - uint16_t i = 0; + mavlink_rc_channels_raw_t *p = (mavlink_rc_channels_raw_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_RAW; - 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 - i += put_uint8_t_by_index(rssi, i, msg->payload); // Receive signal strength indicator, 0: 0%, 255: 100% + p->chan1_raw = chan1_raw; // uint16_t:RC channel 1 value, in microseconds + p->chan2_raw = chan2_raw; // uint16_t:RC channel 2 value, in microseconds + p->chan3_raw = chan3_raw; // uint16_t:RC channel 3 value, in microseconds + p->chan4_raw = chan4_raw; // uint16_t:RC channel 4 value, in microseconds + p->chan5_raw = chan5_raw; // uint16_t:RC channel 5 value, in microseconds + p->chan6_raw = chan6_raw; // uint16_t:RC channel 6 value, in microseconds + p->chan7_raw = chan7_raw; // uint16_t:RC channel 7 value, in microseconds + p->chan8_raw = chan8_raw; // uint16_t:RC channel 8 value, in microseconds + p->rssi = rssi; // uint8_t:Receive signal strength indicator, 0: 0%, 255: 100% - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN); } /** @@ -115,13 +115,43 @@ static inline uint16_t mavlink_msg_rc_channels_raw_encode(uint8_t system_id, uin * @param chan8_raw RC channel 8 value, in microseconds * @param rssi Receive signal strength indicator, 0: 0%, 255: 100% */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_rc_channels_raw_send(mavlink_channel_t chan, 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, uint8_t rssi) { - mavlink_message_t msg; - mavlink_msg_rc_channels_raw_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw, rssi); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_rc_channels_raw_t payload; + uint16_t checksum; + mavlink_rc_channels_raw_t *p = &payload; + + p->chan1_raw = chan1_raw; // uint16_t:RC channel 1 value, in microseconds + p->chan2_raw = chan2_raw; // uint16_t:RC channel 2 value, in microseconds + p->chan3_raw = chan3_raw; // uint16_t:RC channel 3 value, in microseconds + p->chan4_raw = chan4_raw; // uint16_t:RC channel 4 value, in microseconds + p->chan5_raw = chan5_raw; // uint16_t:RC channel 5 value, in microseconds + p->chan6_raw = chan6_raw; // uint16_t:RC channel 6 value, in microseconds + p->chan7_raw = chan7_raw; // uint16_t:RC channel 7 value, in microseconds + p->chan8_raw = chan8_raw; // uint16_t:RC channel 8 value, in microseconds + p->rssi = rssi; // uint8_t:Receive signal strength indicator, 0: 0%, 255: 100% + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN; + hdr.msgid = MAVLINK_MSG_ID_RC_CHANNELS_RAW; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -134,10 +164,8 @@ static inline void mavlink_msg_rc_channels_raw_send(mavlink_channel_t chan, uint */ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan1_raw(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload)[0]; - r.b[0] = (msg->payload)[1]; - return (uint16_t)r.s; + mavlink_rc_channels_raw_t *p = (mavlink_rc_channels_raw_t *)&msg->payload[0]; + return (uint16_t)(p->chan1_raw); } /** @@ -147,10 +175,8 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan1_raw(const mavlink_m */ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan2_raw(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_rc_channels_raw_t *p = (mavlink_rc_channels_raw_t *)&msg->payload[0]; + return (uint16_t)(p->chan2_raw); } /** @@ -160,10 +186,8 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan2_raw(const mavlink_m */ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan3_raw(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_rc_channels_raw_t *p = (mavlink_rc_channels_raw_t *)&msg->payload[0]; + return (uint16_t)(p->chan3_raw); } /** @@ -173,10 +197,8 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan3_raw(const mavlink_m */ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan4_raw(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_rc_channels_raw_t *p = (mavlink_rc_channels_raw_t *)&msg->payload[0]; + return (uint16_t)(p->chan4_raw); } /** @@ -186,10 +208,8 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan4_raw(const mavlink_m */ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan5_raw(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_rc_channels_raw_t *p = (mavlink_rc_channels_raw_t *)&msg->payload[0]; + return (uint16_t)(p->chan5_raw); } /** @@ -199,10 +219,8 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan5_raw(const mavlink_m */ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan6_raw(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_rc_channels_raw_t *p = (mavlink_rc_channels_raw_t *)&msg->payload[0]; + return (uint16_t)(p->chan6_raw); } /** @@ -212,10 +230,8 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan6_raw(const mavlink_m */ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan7_raw(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+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(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_rc_channels_raw_t *p = (mavlink_rc_channels_raw_t *)&msg->payload[0]; + return (uint16_t)(p->chan7_raw); } /** @@ -225,10 +241,8 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan7_raw(const mavlink_m */ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan8_raw(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+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(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; + mavlink_rc_channels_raw_t *p = (mavlink_rc_channels_raw_t *)&msg->payload[0]; + return (uint16_t)(p->chan8_raw); } /** @@ -238,7 +252,8 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan8_raw(const mavlink_m */ static inline uint8_t mavlink_msg_rc_channels_raw_get_rssi(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; + mavlink_rc_channels_raw_t *p = (mavlink_rc_channels_raw_t *)&msg->payload[0]; + return (uint8_t)(p->rssi); } /** @@ -249,13 +264,5 @@ static inline uint8_t mavlink_msg_rc_channels_raw_get_rssi(const mavlink_message */ static inline void mavlink_msg_rc_channels_raw_decode(const mavlink_message_t* msg, mavlink_rc_channels_raw_t* rc_channels_raw) { - rc_channels_raw->chan1_raw = mavlink_msg_rc_channels_raw_get_chan1_raw(msg); - rc_channels_raw->chan2_raw = mavlink_msg_rc_channels_raw_get_chan2_raw(msg); - rc_channels_raw->chan3_raw = mavlink_msg_rc_channels_raw_get_chan3_raw(msg); - rc_channels_raw->chan4_raw = mavlink_msg_rc_channels_raw_get_chan4_raw(msg); - rc_channels_raw->chan5_raw = mavlink_msg_rc_channels_raw_get_chan5_raw(msg); - rc_channels_raw->chan6_raw = mavlink_msg_rc_channels_raw_get_chan6_raw(msg); - rc_channels_raw->chan7_raw = mavlink_msg_rc_channels_raw_get_chan7_raw(msg); - rc_channels_raw->chan8_raw = mavlink_msg_rc_channels_raw_get_chan8_raw(msg); - rc_channels_raw->rssi = mavlink_msg_rc_channels_raw_get_rssi(msg); + memcpy( rc_channels_raw, msg->payload, sizeof(mavlink_rc_channels_raw_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_scaled.h b/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_scaled.h index 526482b4e0395acbd2bd2141fa923a5244249b4e..fd2e57ad4cf10b9f66f2bc6c56d47e3b570ba2ae 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_scaled.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_scaled.h @@ -1,6 +1,8 @@ // MESSAGE RC_CHANNELS_SCALED PACKING #define MAVLINK_MSG_ID_RC_CHANNELS_SCALED 36 +#define MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN 17 +#define MAVLINK_MSG_36_LEN 17 typedef struct __mavlink_rc_channels_scaled_t { @@ -16,8 +18,6 @@ typedef struct __mavlink_rc_channels_scaled_t } mavlink_rc_channels_scaled_t; - - /** * @brief Pack a rc_channels_scaled message * @param system_id ID of this system @@ -37,20 +37,20 @@ typedef struct __mavlink_rc_channels_scaled_t */ static inline uint16_t mavlink_msg_rc_channels_scaled_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, int16_t chan1_scaled, int16_t chan2_scaled, int16_t chan3_scaled, int16_t chan4_scaled, int16_t chan5_scaled, int16_t chan6_scaled, int16_t chan7_scaled, int16_t chan8_scaled, uint8_t rssi) { - uint16_t i = 0; + mavlink_rc_channels_scaled_t *p = (mavlink_rc_channels_scaled_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_SCALED; - i += put_int16_t_by_index(chan1_scaled, i, msg->payload); // RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - i += put_int16_t_by_index(chan2_scaled, i, msg->payload); // RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - i += put_int16_t_by_index(chan3_scaled, i, msg->payload); // RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - i += put_int16_t_by_index(chan4_scaled, i, msg->payload); // RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - i += put_int16_t_by_index(chan5_scaled, i, msg->payload); // RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - i += put_int16_t_by_index(chan6_scaled, i, msg->payload); // RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - i += put_int16_t_by_index(chan7_scaled, i, msg->payload); // RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - i += put_int16_t_by_index(chan8_scaled, i, msg->payload); // RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - i += put_uint8_t_by_index(rssi, i, msg->payload); // Receive signal strength indicator, 0: 0%, 255: 100% + p->chan1_scaled = chan1_scaled; // int16_t:RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan2_scaled = chan2_scaled; // int16_t:RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan3_scaled = chan3_scaled; // int16_t:RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan4_scaled = chan4_scaled; // int16_t:RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan5_scaled = chan5_scaled; // int16_t:RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan6_scaled = chan6_scaled; // int16_t:RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan7_scaled = chan7_scaled; // int16_t:RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan8_scaled = chan8_scaled; // int16_t:RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->rssi = rssi; // uint8_t:Receive signal strength indicator, 0: 0%, 255: 100% - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN); } /** @@ -72,20 +72,20 @@ static inline uint16_t mavlink_msg_rc_channels_scaled_pack(uint8_t system_id, ui */ static inline uint16_t mavlink_msg_rc_channels_scaled_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, int16_t chan1_scaled, int16_t chan2_scaled, int16_t chan3_scaled, int16_t chan4_scaled, int16_t chan5_scaled, int16_t chan6_scaled, int16_t chan7_scaled, int16_t chan8_scaled, uint8_t rssi) { - uint16_t i = 0; + mavlink_rc_channels_scaled_t *p = (mavlink_rc_channels_scaled_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_SCALED; - i += put_int16_t_by_index(chan1_scaled, i, msg->payload); // RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - i += put_int16_t_by_index(chan2_scaled, i, msg->payload); // RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - i += put_int16_t_by_index(chan3_scaled, i, msg->payload); // RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - i += put_int16_t_by_index(chan4_scaled, i, msg->payload); // RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - i += put_int16_t_by_index(chan5_scaled, i, msg->payload); // RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - i += put_int16_t_by_index(chan6_scaled, i, msg->payload); // RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - i += put_int16_t_by_index(chan7_scaled, i, msg->payload); // RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - i += put_int16_t_by_index(chan8_scaled, i, msg->payload); // RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - i += put_uint8_t_by_index(rssi, i, msg->payload); // Receive signal strength indicator, 0: 0%, 255: 100% + p->chan1_scaled = chan1_scaled; // int16_t:RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan2_scaled = chan2_scaled; // int16_t:RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan3_scaled = chan3_scaled; // int16_t:RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan4_scaled = chan4_scaled; // int16_t:RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan5_scaled = chan5_scaled; // int16_t:RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan6_scaled = chan6_scaled; // int16_t:RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan7_scaled = chan7_scaled; // int16_t:RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan8_scaled = chan8_scaled; // int16_t:RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->rssi = rssi; // uint8_t:Receive signal strength indicator, 0: 0%, 255: 100% - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN); } /** @@ -115,13 +115,43 @@ static inline uint16_t mavlink_msg_rc_channels_scaled_encode(uint8_t system_id, * @param chan8_scaled RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 * @param rssi Receive signal strength indicator, 0: 0%, 255: 100% */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_rc_channels_scaled_send(mavlink_channel_t chan, int16_t chan1_scaled, int16_t chan2_scaled, int16_t chan3_scaled, int16_t chan4_scaled, int16_t chan5_scaled, int16_t chan6_scaled, int16_t chan7_scaled, int16_t chan8_scaled, uint8_t rssi) { - mavlink_message_t msg; - mavlink_msg_rc_channels_scaled_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, chan1_scaled, chan2_scaled, chan3_scaled, chan4_scaled, chan5_scaled, chan6_scaled, chan7_scaled, chan8_scaled, rssi); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_rc_channels_scaled_t payload; + uint16_t checksum; + mavlink_rc_channels_scaled_t *p = &payload; + + p->chan1_scaled = chan1_scaled; // int16_t:RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan2_scaled = chan2_scaled; // int16_t:RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan3_scaled = chan3_scaled; // int16_t:RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan4_scaled = chan4_scaled; // int16_t:RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan5_scaled = chan5_scaled; // int16_t:RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan6_scaled = chan6_scaled; // int16_t:RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan7_scaled = chan7_scaled; // int16_t:RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan8_scaled = chan8_scaled; // int16_t:RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->rssi = rssi; // uint8_t:Receive signal strength indicator, 0: 0%, 255: 100% + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN; + hdr.msgid = MAVLINK_MSG_ID_RC_CHANNELS_SCALED; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -134,10 +164,8 @@ static inline void mavlink_msg_rc_channels_scaled_send(mavlink_channel_t chan, i */ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan1_scaled(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; + mavlink_rc_channels_scaled_t *p = (mavlink_rc_channels_scaled_t *)&msg->payload[0]; + return (int16_t)(p->chan1_scaled); } /** @@ -147,10 +175,8 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan1_scaled(const mavl */ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan2_scaled(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; + mavlink_rc_channels_scaled_t *p = (mavlink_rc_channels_scaled_t *)&msg->payload[0]; + return (int16_t)(p->chan2_scaled); } /** @@ -160,10 +186,8 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan2_scaled(const mavl */ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan3_scaled(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; + mavlink_rc_channels_scaled_t *p = (mavlink_rc_channels_scaled_t *)&msg->payload[0]; + return (int16_t)(p->chan3_scaled); } /** @@ -173,10 +197,8 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan3_scaled(const mavl */ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan4_scaled(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_rc_channels_scaled_t *p = (mavlink_rc_channels_scaled_t *)&msg->payload[0]; + return (int16_t)(p->chan4_scaled); } /** @@ -186,10 +208,8 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan4_scaled(const mavl */ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan5_scaled(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_rc_channels_scaled_t *p = (mavlink_rc_channels_scaled_t *)&msg->payload[0]; + return (int16_t)(p->chan5_scaled); } /** @@ -199,10 +219,8 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan5_scaled(const mavl */ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan6_scaled(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_rc_channels_scaled_t *p = (mavlink_rc_channels_scaled_t *)&msg->payload[0]; + return (int16_t)(p->chan6_scaled); } /** @@ -212,10 +230,8 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan6_scaled(const mavl */ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan7_scaled(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_rc_channels_scaled_t *p = (mavlink_rc_channels_scaled_t *)&msg->payload[0]; + return (int16_t)(p->chan7_scaled); } /** @@ -225,10 +241,8 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan7_scaled(const mavl */ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan8_scaled(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_rc_channels_scaled_t *p = (mavlink_rc_channels_scaled_t *)&msg->payload[0]; + return (int16_t)(p->chan8_scaled); } /** @@ -238,7 +252,8 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan8_scaled(const mavl */ static inline uint8_t mavlink_msg_rc_channels_scaled_get_rssi(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; + mavlink_rc_channels_scaled_t *p = (mavlink_rc_channels_scaled_t *)&msg->payload[0]; + return (uint8_t)(p->rssi); } /** @@ -249,13 +264,5 @@ static inline uint8_t mavlink_msg_rc_channels_scaled_get_rssi(const mavlink_mess */ static inline void mavlink_msg_rc_channels_scaled_decode(const mavlink_message_t* msg, mavlink_rc_channels_scaled_t* rc_channels_scaled) { - rc_channels_scaled->chan1_scaled = mavlink_msg_rc_channels_scaled_get_chan1_scaled(msg); - rc_channels_scaled->chan2_scaled = mavlink_msg_rc_channels_scaled_get_chan2_scaled(msg); - rc_channels_scaled->chan3_scaled = mavlink_msg_rc_channels_scaled_get_chan3_scaled(msg); - rc_channels_scaled->chan4_scaled = mavlink_msg_rc_channels_scaled_get_chan4_scaled(msg); - rc_channels_scaled->chan5_scaled = mavlink_msg_rc_channels_scaled_get_chan5_scaled(msg); - rc_channels_scaled->chan6_scaled = mavlink_msg_rc_channels_scaled_get_chan6_scaled(msg); - rc_channels_scaled->chan7_scaled = mavlink_msg_rc_channels_scaled_get_chan7_scaled(msg); - rc_channels_scaled->chan8_scaled = mavlink_msg_rc_channels_scaled_get_chan8_scaled(msg); - rc_channels_scaled->rssi = mavlink_msg_rc_channels_scaled_get_rssi(msg); + memcpy( rc_channels_scaled, msg->payload, sizeof(mavlink_rc_channels_scaled_t)); } 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..b779e267e801d8d7fcf2c050caaeb94163e4d683 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_request_data_stream.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_request_data_stream.h @@ -1,19 +1,19 @@ // MESSAGE REQUEST_DATA_STREAM PACKING #define MAVLINK_MSG_ID_REQUEST_DATA_STREAM 66 +#define MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN 6 +#define MAVLINK_MSG_66_LEN 6 typedef struct __mavlink_request_data_stream_t { + uint16_t req_message_rate; ///< The requested interval between two messages of this type 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 uint8_t start_stop; ///< 1 to start sending, 0 to stop sending. } mavlink_request_data_stream_t; - - /** * @brief Pack a request_data_stream message * @param system_id ID of this system @@ -29,16 +29,16 @@ typedef struct __mavlink_request_data_stream_t */ static inline uint16_t mavlink_msg_request_data_stream_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint8_t req_stream_id, uint16_t req_message_rate, uint8_t start_stop) { - uint16_t i = 0; + mavlink_request_data_stream_t *p = (mavlink_request_data_stream_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_REQUEST_DATA_STREAM; - 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_uint8_t_by_index(start_stop, i, msg->payload); // 1 to start sending, 0 to stop sending. + p->target_system = target_system; // uint8_t:The target requested to send the message stream. + p->target_component = target_component; // uint8_t:The target requested to send the message stream. + p->req_stream_id = req_stream_id; // uint8_t:The ID of the requested message type + p->req_message_rate = req_message_rate; // uint16_t:The requested interval between two messages of this type + p->start_stop = start_stop; // uint8_t:1 to start sending, 0 to stop sending. - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN); } /** @@ -56,16 +56,16 @@ static inline uint16_t mavlink_msg_request_data_stream_pack(uint8_t system_id, u */ static inline uint16_t mavlink_msg_request_data_stream_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, uint8_t req_stream_id, uint16_t req_message_rate, uint8_t start_stop) { - uint16_t i = 0; + mavlink_request_data_stream_t *p = (mavlink_request_data_stream_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_REQUEST_DATA_STREAM; - 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_uint8_t_by_index(start_stop, i, msg->payload); // 1 to start sending, 0 to stop sending. + p->target_system = target_system; // uint8_t:The target requested to send the message stream. + p->target_component = target_component; // uint8_t:The target requested to send the message stream. + p->req_stream_id = req_stream_id; // uint8_t:The ID of the requested message type + p->req_message_rate = req_message_rate; // uint16_t:The requested interval between two messages of this type + p->start_stop = start_stop; // uint8_t:1 to start sending, 0 to stop sending. - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN); } /** @@ -91,13 +91,39 @@ static inline uint16_t mavlink_msg_request_data_stream_encode(uint8_t system_id, * @param req_message_rate The requested interval between two messages of this type * @param start_stop 1 to start sending, 0 to stop sending. */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_request_data_stream_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t req_stream_id, uint16_t req_message_rate, uint8_t start_stop) { - mavlink_message_t msg; - mavlink_msg_request_data_stream_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, req_stream_id, req_message_rate, start_stop); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_request_data_stream_t payload; + uint16_t checksum; + mavlink_request_data_stream_t *p = &payload; + + p->target_system = target_system; // uint8_t:The target requested to send the message stream. + p->target_component = target_component; // uint8_t:The target requested to send the message stream. + p->req_stream_id = req_stream_id; // uint8_t:The ID of the requested message type + p->req_message_rate = req_message_rate; // uint16_t:The requested interval between two messages of this type + p->start_stop = start_stop; // uint8_t:1 to start sending, 0 to stop sending. + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN; + hdr.msgid = MAVLINK_MSG_ID_REQUEST_DATA_STREAM; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -110,7 +136,8 @@ static inline void mavlink_msg_request_data_stream_send(mavlink_channel_t chan, */ static inline uint8_t mavlink_msg_request_data_stream_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_request_data_stream_t *p = (mavlink_request_data_stream_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); } /** @@ -120,7 +147,8 @@ static inline uint8_t mavlink_msg_request_data_stream_get_target_system(const ma */ static inline uint8_t mavlink_msg_request_data_stream_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_request_data_stream_t *p = (mavlink_request_data_stream_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); } /** @@ -130,7 +158,8 @@ static inline uint8_t mavlink_msg_request_data_stream_get_target_component(const */ static inline uint8_t mavlink_msg_request_data_stream_get_req_stream_id(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_request_data_stream_t *p = (mavlink_request_data_stream_t *)&msg->payload[0]; + return (uint8_t)(p->req_stream_id); } /** @@ -140,10 +169,8 @@ static inline uint8_t mavlink_msg_request_data_stream_get_req_stream_id(const ma */ static inline uint16_t mavlink_msg_request_data_stream_get_req_message_rate(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[1]; - return (uint16_t)r.s; + mavlink_request_data_stream_t *p = (mavlink_request_data_stream_t *)&msg->payload[0]; + return (uint16_t)(p->req_message_rate); } /** @@ -153,7 +180,8 @@ static inline uint16_t mavlink_msg_request_data_stream_get_req_message_rate(cons */ static inline uint8_t mavlink_msg_request_data_stream_get_start_stop(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[0]; + mavlink_request_data_stream_t *p = (mavlink_request_data_stream_t *)&msg->payload[0]; + return (uint8_t)(p->start_stop); } /** @@ -164,9 +192,5 @@ static inline uint8_t mavlink_msg_request_data_stream_get_start_stop(const mavli */ static inline void mavlink_msg_request_data_stream_decode(const mavlink_message_t* msg, mavlink_request_data_stream_t* request_data_stream) { - request_data_stream->target_system = mavlink_msg_request_data_stream_get_target_system(msg); - request_data_stream->target_component = mavlink_msg_request_data_stream_get_target_component(msg); - request_data_stream->req_stream_id = mavlink_msg_request_data_stream_get_req_stream_id(msg); - request_data_stream->req_message_rate = mavlink_msg_request_data_stream_get_req_message_rate(msg); - request_data_stream->start_stop = mavlink_msg_request_data_stream_get_start_stop(msg); + memcpy( request_data_stream, msg->payload, sizeof(mavlink_request_data_stream_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_setpoint.h b/thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_setpoint.h new file mode 100644 index 0000000000000000000000000000000000000000..41befa9202e937c1709fe3eead98821517ecc1a3 --- /dev/null +++ b/thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_setpoint.h @@ -0,0 +1,178 @@ +// MESSAGE ROLL_PITCH_YAW_SETPOINT PACKING + +#define MAVLINK_MSG_ID_ROLL_PITCH_YAW_SETPOINT 57 +#define MAVLINK_MSG_ID_ROLL_PITCH_YAW_SETPOINT_LEN 16 +#define MAVLINK_MSG_57_LEN 16 + +typedef struct __mavlink_roll_pitch_yaw_setpoint_t +{ + uint32_t time_ms; ///< Timestamp in milliseconds since system boot + float roll; ///< Desired roll angle in radians + float pitch; ///< Desired pitch angle in radians + float yaw; ///< Desired yaw angle in radians + +} mavlink_roll_pitch_yaw_setpoint_t; + +/** + * @brief Pack a roll_pitch_yaw_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_ms Timestamp in milliseconds since system boot + * @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_roll_pitch_yaw_setpoint_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint32_t time_ms, float roll, float pitch, float yaw) +{ + mavlink_roll_pitch_yaw_setpoint_t *p = (mavlink_roll_pitch_yaw_setpoint_t *)&msg->payload[0]; + msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_SETPOINT; + + p->time_ms = time_ms; // uint32_t:Timestamp in milliseconds since system boot + p->roll = roll; // float:Desired roll angle in radians + p->pitch = pitch; // float:Desired pitch angle in radians + p->yaw = yaw; // float:Desired yaw angle in radians + + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SETPOINT_LEN); +} + +/** + * @brief Pack a roll_pitch_yaw_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_ms Timestamp in milliseconds since system boot + * @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_roll_pitch_yaw_setpoint_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint32_t time_ms, float roll, float pitch, float yaw) +{ + mavlink_roll_pitch_yaw_setpoint_t *p = (mavlink_roll_pitch_yaw_setpoint_t *)&msg->payload[0]; + msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_SETPOINT; + + p->time_ms = time_ms; // uint32_t:Timestamp in milliseconds since system boot + p->roll = roll; // float:Desired roll angle in radians + p->pitch = pitch; // float:Desired pitch angle in radians + p->yaw = yaw; // float:Desired yaw angle in radians + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SETPOINT_LEN); +} + +/** + * @brief Encode a roll_pitch_yaw_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_setpoint C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_roll_pitch_yaw_setpoint_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_roll_pitch_yaw_setpoint_t* roll_pitch_yaw_setpoint) +{ + return mavlink_msg_roll_pitch_yaw_setpoint_pack(system_id, component_id, msg, roll_pitch_yaw_setpoint->time_ms, roll_pitch_yaw_setpoint->roll, roll_pitch_yaw_setpoint->pitch, roll_pitch_yaw_setpoint->yaw); +} + +/** + * @brief Send a roll_pitch_yaw_setpoint message + * @param chan MAVLink channel to send the message + * + * @param time_ms Timestamp in milliseconds since system boot + * @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_roll_pitch_yaw_setpoint_send(mavlink_channel_t chan, uint32_t time_ms, float roll, float pitch, float yaw) +{ + mavlink_header_t hdr; + mavlink_roll_pitch_yaw_setpoint_t payload; + uint16_t checksum; + mavlink_roll_pitch_yaw_setpoint_t *p = &payload; + + p->time_ms = time_ms; // uint32_t:Timestamp in milliseconds since system boot + p->roll = roll; // float:Desired roll angle in radians + p->pitch = pitch; // float:Desired pitch angle in radians + p->yaw = yaw; // float:Desired yaw angle in radians + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_ROLL_PITCH_YAW_SETPOINT_LEN; + hdr.msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_SETPOINT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); +} + +#endif +// MESSAGE ROLL_PITCH_YAW_SETPOINT UNPACKING + +/** + * @brief Get field time_ms from roll_pitch_yaw_setpoint message + * + * @return Timestamp in milliseconds since system boot + */ +static inline uint32_t mavlink_msg_roll_pitch_yaw_setpoint_get_time_ms(const mavlink_message_t* msg) +{ + mavlink_roll_pitch_yaw_setpoint_t *p = (mavlink_roll_pitch_yaw_setpoint_t *)&msg->payload[0]; + return (uint32_t)(p->time_ms); +} + +/** + * @brief Get field roll from roll_pitch_yaw_setpoint message + * + * @return Desired roll angle in radians + */ +static inline float mavlink_msg_roll_pitch_yaw_setpoint_get_roll(const mavlink_message_t* msg) +{ + mavlink_roll_pitch_yaw_setpoint_t *p = (mavlink_roll_pitch_yaw_setpoint_t *)&msg->payload[0]; + return (float)(p->roll); +} + +/** + * @brief Get field pitch from roll_pitch_yaw_setpoint message + * + * @return Desired pitch angle in radians + */ +static inline float mavlink_msg_roll_pitch_yaw_setpoint_get_pitch(const mavlink_message_t* msg) +{ + mavlink_roll_pitch_yaw_setpoint_t *p = (mavlink_roll_pitch_yaw_setpoint_t *)&msg->payload[0]; + return (float)(p->pitch); +} + +/** + * @brief Get field yaw from roll_pitch_yaw_setpoint message + * + * @return Desired yaw angle in radians + */ +static inline float mavlink_msg_roll_pitch_yaw_setpoint_get_yaw(const mavlink_message_t* msg) +{ + mavlink_roll_pitch_yaw_setpoint_t *p = (mavlink_roll_pitch_yaw_setpoint_t *)&msg->payload[0]; + return (float)(p->yaw); +} + +/** + * @brief Decode a roll_pitch_yaw_setpoint message into a struct + * + * @param msg The message to decode + * @param roll_pitch_yaw_setpoint C-struct to decode the message contents into + */ +static inline void mavlink_msg_roll_pitch_yaw_setpoint_decode(const mavlink_message_t* msg, mavlink_roll_pitch_yaw_setpoint_t* roll_pitch_yaw_setpoint) +{ + memcpy( roll_pitch_yaw_setpoint, msg->payload, sizeof(mavlink_roll_pitch_yaw_setpoint_t)); +} diff --git a/thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_speed_setpoint.h b/thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_speed_setpoint.h new file mode 100644 index 0000000000000000000000000000000000000000..c920eaf346ef9767d6f3802f59ecd4c92c80c247 --- /dev/null +++ b/thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_speed_setpoint.h @@ -0,0 +1,178 @@ +// MESSAGE ROLL_PITCH_YAW_SPEED_SETPOINT PACKING + +#define MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_SETPOINT 58 +#define MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_SETPOINT_LEN 16 +#define MAVLINK_MSG_58_LEN 16 + +typedef struct __mavlink_roll_pitch_yaw_speed_setpoint_t +{ + uint32_t time_ms; ///< Timestamp in milliseconds since system boot + 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_roll_pitch_yaw_speed_setpoint_t; + +/** + * @brief Pack a roll_pitch_yaw_speed_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_ms Timestamp in milliseconds since system boot + * @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_roll_pitch_yaw_speed_setpoint_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint32_t time_ms, float roll_speed, float pitch_speed, float yaw_speed) +{ + mavlink_roll_pitch_yaw_speed_setpoint_t *p = (mavlink_roll_pitch_yaw_speed_setpoint_t *)&msg->payload[0]; + msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_SETPOINT; + + p->time_ms = time_ms; // uint32_t:Timestamp in milliseconds since system boot + p->roll_speed = roll_speed; // float:Desired roll angular speed in rad/s + p->pitch_speed = pitch_speed; // float:Desired pitch angular speed in rad/s + p->yaw_speed = yaw_speed; // float:Desired yaw angular speed in rad/s + + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_SETPOINT_LEN); +} + +/** + * @brief Pack a roll_pitch_yaw_speed_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_ms Timestamp in milliseconds since system boot + * @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_roll_pitch_yaw_speed_setpoint_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint32_t time_ms, float roll_speed, float pitch_speed, float yaw_speed) +{ + mavlink_roll_pitch_yaw_speed_setpoint_t *p = (mavlink_roll_pitch_yaw_speed_setpoint_t *)&msg->payload[0]; + msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_SETPOINT; + + p->time_ms = time_ms; // uint32_t:Timestamp in milliseconds since system boot + p->roll_speed = roll_speed; // float:Desired roll angular speed in rad/s + p->pitch_speed = pitch_speed; // float:Desired pitch angular speed in rad/s + p->yaw_speed = yaw_speed; // float:Desired yaw angular speed in rad/s + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_SETPOINT_LEN); +} + +/** + * @brief Encode a roll_pitch_yaw_speed_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_setpoint C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_setpoint_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_roll_pitch_yaw_speed_setpoint_t* roll_pitch_yaw_speed_setpoint) +{ + return mavlink_msg_roll_pitch_yaw_speed_setpoint_pack(system_id, component_id, msg, roll_pitch_yaw_speed_setpoint->time_ms, roll_pitch_yaw_speed_setpoint->roll_speed, roll_pitch_yaw_speed_setpoint->pitch_speed, roll_pitch_yaw_speed_setpoint->yaw_speed); +} + +/** + * @brief Send a roll_pitch_yaw_speed_setpoint message + * @param chan MAVLink channel to send the message + * + * @param time_ms Timestamp in milliseconds since system boot + * @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_roll_pitch_yaw_speed_setpoint_send(mavlink_channel_t chan, uint32_t time_ms, float roll_speed, float pitch_speed, float yaw_speed) +{ + mavlink_header_t hdr; + mavlink_roll_pitch_yaw_speed_setpoint_t payload; + uint16_t checksum; + mavlink_roll_pitch_yaw_speed_setpoint_t *p = &payload; + + p->time_ms = time_ms; // uint32_t:Timestamp in milliseconds since system boot + p->roll_speed = roll_speed; // float:Desired roll angular speed in rad/s + p->pitch_speed = pitch_speed; // float:Desired pitch angular speed in rad/s + p->yaw_speed = yaw_speed; // float:Desired yaw angular speed in rad/s + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_SETPOINT_LEN; + hdr.msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_SETPOINT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); +} + +#endif +// MESSAGE ROLL_PITCH_YAW_SPEED_SETPOINT UNPACKING + +/** + * @brief Get field time_ms from roll_pitch_yaw_speed_setpoint message + * + * @return Timestamp in milliseconds since system boot + */ +static inline uint32_t mavlink_msg_roll_pitch_yaw_speed_setpoint_get_time_ms(const mavlink_message_t* msg) +{ + mavlink_roll_pitch_yaw_speed_setpoint_t *p = (mavlink_roll_pitch_yaw_speed_setpoint_t *)&msg->payload[0]; + return (uint32_t)(p->time_ms); +} + +/** + * @brief Get field roll_speed from roll_pitch_yaw_speed_setpoint message + * + * @return Desired roll angular speed in rad/s + */ +static inline float mavlink_msg_roll_pitch_yaw_speed_setpoint_get_roll_speed(const mavlink_message_t* msg) +{ + mavlink_roll_pitch_yaw_speed_setpoint_t *p = (mavlink_roll_pitch_yaw_speed_setpoint_t *)&msg->payload[0]; + return (float)(p->roll_speed); +} + +/** + * @brief Get field pitch_speed from roll_pitch_yaw_speed_setpoint message + * + * @return Desired pitch angular speed in rad/s + */ +static inline float mavlink_msg_roll_pitch_yaw_speed_setpoint_get_pitch_speed(const mavlink_message_t* msg) +{ + mavlink_roll_pitch_yaw_speed_setpoint_t *p = (mavlink_roll_pitch_yaw_speed_setpoint_t *)&msg->payload[0]; + return (float)(p->pitch_speed); +} + +/** + * @brief Get field yaw_speed from roll_pitch_yaw_speed_setpoint message + * + * @return Desired yaw angular speed in rad/s + */ +static inline float mavlink_msg_roll_pitch_yaw_speed_setpoint_get_yaw_speed(const mavlink_message_t* msg) +{ + mavlink_roll_pitch_yaw_speed_setpoint_t *p = (mavlink_roll_pitch_yaw_speed_setpoint_t *)&msg->payload[0]; + return (float)(p->yaw_speed); +} + +/** + * @brief Decode a roll_pitch_yaw_speed_setpoint message into a struct + * + * @param msg The message to decode + * @param roll_pitch_yaw_speed_setpoint C-struct to decode the message contents into + */ +static inline void mavlink_msg_roll_pitch_yaw_speed_setpoint_decode(const mavlink_message_t* msg, mavlink_roll_pitch_yaw_speed_setpoint_t* roll_pitch_yaw_speed_setpoint) +{ + memcpy( roll_pitch_yaw_speed_setpoint, msg->payload, sizeof(mavlink_roll_pitch_yaw_speed_setpoint_t)); +} diff --git a/thirdParty/mavlink/include/common/mavlink_msg_safety_allowed_area.h b/thirdParty/mavlink/include/common/mavlink_msg_safety_allowed_area.h index e1f9cc6b12d25925ca4cd15334d9e6174e702dde..a660e8bed59d688e5237939274202646aa494099 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_safety_allowed_area.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_safety_allowed_area.h @@ -1,21 +1,21 @@ // MESSAGE SAFETY_ALLOWED_AREA PACKING #define MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA 54 +#define MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN 25 +#define MAVLINK_MSG_54_LEN 25 typedef struct __mavlink_safety_allowed_area_t { - uint8_t frame; ///< 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. float p1x; ///< x position 1 / Latitude 1 float p1y; ///< y position 1 / Longitude 1 float p1z; ///< z position 1 / Altitude 1 float p2x; ///< x position 2 / Latitude 2 float p2y; ///< y position 2 / Longitude 2 float p2z; ///< z position 2 / Altitude 2 + uint8_t frame; ///< 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. } mavlink_safety_allowed_area_t; - - /** * @brief Pack a safety_allowed_area message * @param system_id ID of this system @@ -33,18 +33,18 @@ typedef struct __mavlink_safety_allowed_area_t */ static inline uint16_t mavlink_msg_safety_allowed_area_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z) { - uint16_t i = 0; + mavlink_safety_allowed_area_t *p = (mavlink_safety_allowed_area_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA; - i += put_uint8_t_by_index(frame, i, msg->payload); // 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. - i += put_float_by_index(p1x, i, msg->payload); // x position 1 / Latitude 1 - i += put_float_by_index(p1y, i, msg->payload); // y position 1 / Longitude 1 - i += put_float_by_index(p1z, i, msg->payload); // z position 1 / Altitude 1 - i += put_float_by_index(p2x, i, msg->payload); // x position 2 / Latitude 2 - i += put_float_by_index(p2y, i, msg->payload); // y position 2 / Longitude 2 - i += put_float_by_index(p2z, i, msg->payload); // z position 2 / Altitude 2 + p->frame = frame; // uint8_t: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. + p->p1x = p1x; // float:x position 1 / Latitude 1 + p->p1y = p1y; // float:y position 1 / Longitude 1 + p->p1z = p1z; // float:z position 1 / Altitude 1 + p->p2x = p2x; // float:x position 2 / Latitude 2 + p->p2y = p2y; // float:y position 2 / Longitude 2 + p->p2z = p2z; // float:z position 2 / Altitude 2 - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN); } /** @@ -64,18 +64,18 @@ static inline uint16_t mavlink_msg_safety_allowed_area_pack(uint8_t system_id, u */ static inline uint16_t mavlink_msg_safety_allowed_area_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z) { - uint16_t i = 0; + mavlink_safety_allowed_area_t *p = (mavlink_safety_allowed_area_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA; - i += put_uint8_t_by_index(frame, i, msg->payload); // 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. - i += put_float_by_index(p1x, i, msg->payload); // x position 1 / Latitude 1 - i += put_float_by_index(p1y, i, msg->payload); // y position 1 / Longitude 1 - i += put_float_by_index(p1z, i, msg->payload); // z position 1 / Altitude 1 - i += put_float_by_index(p2x, i, msg->payload); // x position 2 / Latitude 2 - i += put_float_by_index(p2y, i, msg->payload); // y position 2 / Longitude 2 - i += put_float_by_index(p2z, i, msg->payload); // z position 2 / Altitude 2 + p->frame = frame; // uint8_t: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. + p->p1x = p1x; // float:x position 1 / Latitude 1 + p->p1y = p1y; // float:y position 1 / Longitude 1 + p->p1z = p1z; // float:z position 1 / Altitude 1 + p->p2x = p2x; // float:x position 2 / Latitude 2 + p->p2y = p2y; // float:y position 2 / Longitude 2 + p->p2z = p2z; // float:z position 2 / Altitude 2 - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN); } /** @@ -103,13 +103,41 @@ static inline uint16_t mavlink_msg_safety_allowed_area_encode(uint8_t system_id, * @param p2y y position 2 / Longitude 2 * @param p2z z position 2 / Altitude 2 */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_safety_allowed_area_send(mavlink_channel_t chan, uint8_t frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z) { - mavlink_message_t msg; - mavlink_msg_safety_allowed_area_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, frame, p1x, p1y, p1z, p2x, p2y, p2z); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_safety_allowed_area_t payload; + uint16_t checksum; + mavlink_safety_allowed_area_t *p = &payload; + + p->frame = frame; // uint8_t: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. + p->p1x = p1x; // float:x position 1 / Latitude 1 + p->p1y = p1y; // float:y position 1 / Longitude 1 + p->p1z = p1z; // float:z position 1 / Altitude 1 + p->p2x = p2x; // float:x position 2 / Latitude 2 + p->p2y = p2y; // float:y position 2 / Longitude 2 + p->p2z = p2z; // float:z position 2 / Altitude 2 + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN; + hdr.msgid = MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -122,7 +150,8 @@ static inline void mavlink_msg_safety_allowed_area_send(mavlink_channel_t chan, */ static inline uint8_t mavlink_msg_safety_allowed_area_get_frame(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_safety_allowed_area_t *p = (mavlink_safety_allowed_area_t *)&msg->payload[0]; + return (uint8_t)(p->frame); } /** @@ -132,12 +161,8 @@ static inline uint8_t mavlink_msg_safety_allowed_area_get_frame(const mavlink_me */ static inline float mavlink_msg_safety_allowed_area_get_p1x(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t))[3]; - return (float)r.f; + mavlink_safety_allowed_area_t *p = (mavlink_safety_allowed_area_t *)&msg->payload[0]; + return (float)(p->p1x); } /** @@ -147,12 +172,8 @@ static inline float mavlink_msg_safety_allowed_area_get_p1x(const mavlink_messag */ static inline float mavlink_msg_safety_allowed_area_get_p1y(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(float))[3]; - return (float)r.f; + mavlink_safety_allowed_area_t *p = (mavlink_safety_allowed_area_t *)&msg->payload[0]; + return (float)(p->p1y); } /** @@ -162,12 +183,8 @@ static inline float mavlink_msg_safety_allowed_area_get_p1y(const mavlink_messag */ static inline float mavlink_msg_safety_allowed_area_get_p1z(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_safety_allowed_area_t *p = (mavlink_safety_allowed_area_t *)&msg->payload[0]; + return (float)(p->p1z); } /** @@ -177,12 +194,8 @@ static inline float mavlink_msg_safety_allowed_area_get_p1z(const mavlink_messag */ static inline float mavlink_msg_safety_allowed_area_get_p2x(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_safety_allowed_area_t *p = (mavlink_safety_allowed_area_t *)&msg->payload[0]; + return (float)(p->p2x); } /** @@ -192,12 +205,8 @@ static inline float mavlink_msg_safety_allowed_area_get_p2x(const mavlink_messag */ static inline float mavlink_msg_safety_allowed_area_get_p2y(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_safety_allowed_area_t *p = (mavlink_safety_allowed_area_t *)&msg->payload[0]; + return (float)(p->p2y); } /** @@ -207,12 +216,8 @@ static inline float mavlink_msg_safety_allowed_area_get_p2y(const mavlink_messag */ static inline float mavlink_msg_safety_allowed_area_get_p2z(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_safety_allowed_area_t *p = (mavlink_safety_allowed_area_t *)&msg->payload[0]; + return (float)(p->p2z); } /** @@ -223,11 +228,5 @@ static inline float mavlink_msg_safety_allowed_area_get_p2z(const mavlink_messag */ static inline void mavlink_msg_safety_allowed_area_decode(const mavlink_message_t* msg, mavlink_safety_allowed_area_t* safety_allowed_area) { - safety_allowed_area->frame = mavlink_msg_safety_allowed_area_get_frame(msg); - safety_allowed_area->p1x = mavlink_msg_safety_allowed_area_get_p1x(msg); - safety_allowed_area->p1y = mavlink_msg_safety_allowed_area_get_p1y(msg); - safety_allowed_area->p1z = mavlink_msg_safety_allowed_area_get_p1z(msg); - safety_allowed_area->p2x = mavlink_msg_safety_allowed_area_get_p2x(msg); - safety_allowed_area->p2y = mavlink_msg_safety_allowed_area_get_p2y(msg); - safety_allowed_area->p2z = mavlink_msg_safety_allowed_area_get_p2z(msg); + memcpy( safety_allowed_area, msg->payload, sizeof(mavlink_safety_allowed_area_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_safety_set_allowed_area.h b/thirdParty/mavlink/include/common/mavlink_msg_safety_set_allowed_area.h index da571e8e7210dc711558278d0bce952f0cafc729..9e8ef1e44b765cab582d82e94f07accf5349fe59 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_safety_set_allowed_area.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_safety_set_allowed_area.h @@ -1,23 +1,23 @@ // MESSAGE SAFETY_SET_ALLOWED_AREA PACKING #define MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA 53 +#define MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN 27 +#define MAVLINK_MSG_53_LEN 27 typedef struct __mavlink_safety_set_allowed_area_t { - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - uint8_t frame; ///< 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. float p1x; ///< x position 1 / Latitude 1 float p1y; ///< y position 1 / Longitude 1 float p1z; ///< z position 1 / Altitude 1 float p2x; ///< x position 2 / Latitude 2 float p2y; ///< y position 2 / Longitude 2 float p2z; ///< z position 2 / Altitude 2 + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + uint8_t frame; ///< 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. } mavlink_safety_set_allowed_area_t; - - /** * @brief Pack a safety_set_allowed_area message * @param system_id ID of this system @@ -37,20 +37,20 @@ typedef struct __mavlink_safety_set_allowed_area_t */ static inline uint16_t mavlink_msg_safety_set_allowed_area_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint8_t frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z) { - uint16_t i = 0; + mavlink_safety_set_allowed_area_t *p = (mavlink_safety_set_allowed_area_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA; - 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_uint8_t_by_index(frame, i, msg->payload); // 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. - i += put_float_by_index(p1x, i, msg->payload); // x position 1 / Latitude 1 - i += put_float_by_index(p1y, i, msg->payload); // y position 1 / Longitude 1 - i += put_float_by_index(p1z, i, msg->payload); // z position 1 / Altitude 1 - i += put_float_by_index(p2x, i, msg->payload); // x position 2 / Latitude 2 - i += put_float_by_index(p2y, i, msg->payload); // y position 2 / Longitude 2 - i += put_float_by_index(p2z, i, msg->payload); // z position 2 / Altitude 2 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->frame = frame; // uint8_t: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. + p->p1x = p1x; // float:x position 1 / Latitude 1 + p->p1y = p1y; // float:y position 1 / Longitude 1 + p->p1z = p1z; // float:z position 1 / Altitude 1 + p->p2x = p2x; // float:x position 2 / Latitude 2 + p->p2y = p2y; // float:y position 2 / Longitude 2 + p->p2z = p2z; // float:z position 2 / Altitude 2 - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN); } /** @@ -72,20 +72,20 @@ static inline uint16_t mavlink_msg_safety_set_allowed_area_pack(uint8_t system_i */ static inline uint16_t mavlink_msg_safety_set_allowed_area_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, uint8_t frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z) { - uint16_t i = 0; + mavlink_safety_set_allowed_area_t *p = (mavlink_safety_set_allowed_area_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA; - 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_uint8_t_by_index(frame, i, msg->payload); // 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. - i += put_float_by_index(p1x, i, msg->payload); // x position 1 / Latitude 1 - i += put_float_by_index(p1y, i, msg->payload); // y position 1 / Longitude 1 - i += put_float_by_index(p1z, i, msg->payload); // z position 1 / Altitude 1 - i += put_float_by_index(p2x, i, msg->payload); // x position 2 / Latitude 2 - i += put_float_by_index(p2y, i, msg->payload); // y position 2 / Longitude 2 - i += put_float_by_index(p2z, i, msg->payload); // z position 2 / Altitude 2 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->frame = frame; // uint8_t: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. + p->p1x = p1x; // float:x position 1 / Latitude 1 + p->p1y = p1y; // float:y position 1 / Longitude 1 + p->p1z = p1z; // float:z position 1 / Altitude 1 + p->p2x = p2x; // float:x position 2 / Latitude 2 + p->p2y = p2y; // float:y position 2 / Longitude 2 + p->p2z = p2z; // float:z position 2 / Altitude 2 - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN); } /** @@ -115,13 +115,43 @@ static inline uint16_t mavlink_msg_safety_set_allowed_area_encode(uint8_t system * @param p2y y position 2 / Longitude 2 * @param p2z z position 2 / Altitude 2 */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_safety_set_allowed_area_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z) { - mavlink_message_t msg; - mavlink_msg_safety_set_allowed_area_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, frame, p1x, p1y, p1z, p2x, p2y, p2z); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_safety_set_allowed_area_t payload; + uint16_t checksum; + mavlink_safety_set_allowed_area_t *p = &payload; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->frame = frame; // uint8_t: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. + p->p1x = p1x; // float:x position 1 / Latitude 1 + p->p1y = p1y; // float:y position 1 / Longitude 1 + p->p1z = p1z; // float:z position 1 / Altitude 1 + p->p2x = p2x; // float:x position 2 / Latitude 2 + p->p2y = p2y; // float:y position 2 / Longitude 2 + p->p2z = p2z; // float:z position 2 / Altitude 2 + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN; + hdr.msgid = MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -134,7 +164,8 @@ static inline void mavlink_msg_safety_set_allowed_area_send(mavlink_channel_t ch */ static inline uint8_t mavlink_msg_safety_set_allowed_area_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_safety_set_allowed_area_t *p = (mavlink_safety_set_allowed_area_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); } /** @@ -144,7 +175,8 @@ static inline uint8_t mavlink_msg_safety_set_allowed_area_get_target_system(cons */ static inline uint8_t mavlink_msg_safety_set_allowed_area_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_safety_set_allowed_area_t *p = (mavlink_safety_set_allowed_area_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); } /** @@ -154,7 +186,8 @@ static inline uint8_t mavlink_msg_safety_set_allowed_area_get_target_component(c */ static inline uint8_t mavlink_msg_safety_set_allowed_area_get_frame(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_safety_set_allowed_area_t *p = (mavlink_safety_set_allowed_area_t *)&msg->payload[0]; + return (uint8_t)(p->frame); } /** @@ -164,12 +197,8 @@ static inline uint8_t mavlink_msg_safety_set_allowed_area_get_frame(const mavlin */ static inline float mavlink_msg_safety_set_allowed_area_get_p1x(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[3]; - return (float)r.f; + mavlink_safety_set_allowed_area_t *p = (mavlink_safety_set_allowed_area_t *)&msg->payload[0]; + return (float)(p->p1x); } /** @@ -179,12 +208,8 @@ static inline float mavlink_msg_safety_set_allowed_area_get_p1x(const mavlink_me */ static inline float mavlink_msg_safety_set_allowed_area_get_p1y(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[3]; - return (float)r.f; + mavlink_safety_set_allowed_area_t *p = (mavlink_safety_set_allowed_area_t *)&msg->payload[0]; + return (float)(p->p1y); } /** @@ -194,12 +219,8 @@ static inline float mavlink_msg_safety_set_allowed_area_get_p1y(const mavlink_me */ static inline float mavlink_msg_safety_set_allowed_area_get_p1z(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_safety_set_allowed_area_t *p = (mavlink_safety_set_allowed_area_t *)&msg->payload[0]; + return (float)(p->p1z); } /** @@ -209,12 +230,8 @@ static inline float mavlink_msg_safety_set_allowed_area_get_p1z(const mavlink_me */ static inline float mavlink_msg_safety_set_allowed_area_get_p2x(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+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(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+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(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_safety_set_allowed_area_t *p = (mavlink_safety_set_allowed_area_t *)&msg->payload[0]; + return (float)(p->p2x); } /** @@ -224,12 +241,8 @@ static inline float mavlink_msg_safety_set_allowed_area_get_p2x(const mavlink_me */ static inline float mavlink_msg_safety_set_allowed_area_get_p2y(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_safety_set_allowed_area_t *p = (mavlink_safety_set_allowed_area_t *)&msg->payload[0]; + return (float)(p->p2y); } /** @@ -239,12 +252,8 @@ static inline float mavlink_msg_safety_set_allowed_area_get_p2y(const mavlink_me */ static inline float mavlink_msg_safety_set_allowed_area_get_p2z(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_safety_set_allowed_area_t *p = (mavlink_safety_set_allowed_area_t *)&msg->payload[0]; + return (float)(p->p2z); } /** @@ -255,13 +264,5 @@ static inline float mavlink_msg_safety_set_allowed_area_get_p2z(const mavlink_me */ static inline void mavlink_msg_safety_set_allowed_area_decode(const mavlink_message_t* msg, mavlink_safety_set_allowed_area_t* safety_set_allowed_area) { - safety_set_allowed_area->target_system = mavlink_msg_safety_set_allowed_area_get_target_system(msg); - safety_set_allowed_area->target_component = mavlink_msg_safety_set_allowed_area_get_target_component(msg); - safety_set_allowed_area->frame = mavlink_msg_safety_set_allowed_area_get_frame(msg); - safety_set_allowed_area->p1x = mavlink_msg_safety_set_allowed_area_get_p1x(msg); - safety_set_allowed_area->p1y = mavlink_msg_safety_set_allowed_area_get_p1y(msg); - safety_set_allowed_area->p1z = mavlink_msg_safety_set_allowed_area_get_p1z(msg); - safety_set_allowed_area->p2x = mavlink_msg_safety_set_allowed_area_get_p2x(msg); - safety_set_allowed_area->p2y = mavlink_msg_safety_set_allowed_area_get_p2y(msg); - safety_set_allowed_area->p2z = mavlink_msg_safety_set_allowed_area_get_p2z(msg); + memcpy( safety_set_allowed_area, msg->payload, sizeof(mavlink_safety_set_allowed_area_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_scaled_imu.h b/thirdParty/mavlink/include/common/mavlink_msg_scaled_imu.h index 77637286ff7a45adb6920079d79c2312a5c2758c..8f381f22c151c50cef642e9cf70125f62a43b8ba 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_scaled_imu.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_scaled_imu.h @@ -1,6 +1,8 @@ // MESSAGE SCALED_IMU PACKING #define MAVLINK_MSG_ID_SCALED_IMU 26 +#define MAVLINK_MSG_ID_SCALED_IMU_LEN 26 +#define MAVLINK_MSG_26_LEN 26 typedef struct __mavlink_scaled_imu_t { @@ -17,8 +19,6 @@ typedef struct __mavlink_scaled_imu_t } mavlink_scaled_imu_t; - - /** * @brief Pack a scaled_imu message * @param system_id ID of this system @@ -39,21 +39,21 @@ typedef struct __mavlink_scaled_imu_t */ static inline uint16_t mavlink_msg_scaled_imu_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t usec, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) { - uint16_t i = 0; + mavlink_scaled_imu_t *p = (mavlink_scaled_imu_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SCALED_IMU; - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - 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) - i += put_int16_t_by_index(xgyro, i, msg->payload); // Angular speed around X axis (millirad /sec) - i += put_int16_t_by_index(ygyro, i, msg->payload); // Angular speed around Y axis (millirad /sec) - i += put_int16_t_by_index(zgyro, i, msg->payload); // Angular speed around Z axis (millirad /sec) - i += put_int16_t_by_index(xmag, i, msg->payload); // X Magnetic field (milli tesla) - i += put_int16_t_by_index(ymag, i, msg->payload); // Y Magnetic field (milli tesla) - i += put_int16_t_by_index(zmag, i, msg->payload); // Z Magnetic field (milli tesla) + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->xacc = xacc; // int16_t:X acceleration (mg) + p->yacc = yacc; // int16_t:Y acceleration (mg) + p->zacc = zacc; // int16_t:Z acceleration (mg) + p->xgyro = xgyro; // int16_t:Angular speed around X axis (millirad /sec) + p->ygyro = ygyro; // int16_t:Angular speed around Y axis (millirad /sec) + p->zgyro = zgyro; // int16_t:Angular speed around Z axis (millirad /sec) + p->xmag = xmag; // int16_t:X Magnetic field (milli tesla) + p->ymag = ymag; // int16_t:Y Magnetic field (milli tesla) + p->zmag = zmag; // int16_t:Z Magnetic field (milli tesla) - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SCALED_IMU_LEN); } /** @@ -76,21 +76,21 @@ static inline uint16_t mavlink_msg_scaled_imu_pack(uint8_t system_id, uint8_t co */ static inline uint16_t mavlink_msg_scaled_imu_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t usec, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) { - uint16_t i = 0; + mavlink_scaled_imu_t *p = (mavlink_scaled_imu_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SCALED_IMU; - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - 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) - i += put_int16_t_by_index(xgyro, i, msg->payload); // Angular speed around X axis (millirad /sec) - i += put_int16_t_by_index(ygyro, i, msg->payload); // Angular speed around Y axis (millirad /sec) - i += put_int16_t_by_index(zgyro, i, msg->payload); // Angular speed around Z axis (millirad /sec) - i += put_int16_t_by_index(xmag, i, msg->payload); // X Magnetic field (milli tesla) - i += put_int16_t_by_index(ymag, i, msg->payload); // Y Magnetic field (milli tesla) - i += put_int16_t_by_index(zmag, i, msg->payload); // Z Magnetic field (milli tesla) + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->xacc = xacc; // int16_t:X acceleration (mg) + p->yacc = yacc; // int16_t:Y acceleration (mg) + p->zacc = zacc; // int16_t:Z acceleration (mg) + p->xgyro = xgyro; // int16_t:Angular speed around X axis (millirad /sec) + p->ygyro = ygyro; // int16_t:Angular speed around Y axis (millirad /sec) + p->zgyro = zgyro; // int16_t:Angular speed around Z axis (millirad /sec) + p->xmag = xmag; // int16_t:X Magnetic field (milli tesla) + p->ymag = ymag; // int16_t:Y Magnetic field (milli tesla) + p->zmag = zmag; // int16_t:Z Magnetic field (milli tesla) - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SCALED_IMU_LEN); } /** @@ -121,13 +121,44 @@ static inline uint16_t mavlink_msg_scaled_imu_encode(uint8_t system_id, uint8_t * @param ymag Y Magnetic field (milli tesla) * @param zmag Z Magnetic field (milli tesla) */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_scaled_imu_send(mavlink_channel_t chan, uint64_t usec, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) { - mavlink_message_t msg; - mavlink_msg_scaled_imu_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, usec, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_scaled_imu_t payload; + uint16_t checksum; + mavlink_scaled_imu_t *p = &payload; + + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->xacc = xacc; // int16_t:X acceleration (mg) + p->yacc = yacc; // int16_t:Y acceleration (mg) + p->zacc = zacc; // int16_t:Z acceleration (mg) + p->xgyro = xgyro; // int16_t:Angular speed around X axis (millirad /sec) + p->ygyro = ygyro; // int16_t:Angular speed around Y axis (millirad /sec) + p->zgyro = zgyro; // int16_t:Angular speed around Z axis (millirad /sec) + p->xmag = xmag; // int16_t:X Magnetic field (milli tesla) + p->ymag = ymag; // int16_t:Y Magnetic field (milli tesla) + p->zmag = zmag; // int16_t:Z Magnetic field (milli tesla) + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_SCALED_IMU_LEN; + hdr.msgid = MAVLINK_MSG_ID_SCALED_IMU; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -140,16 +171,8 @@ static inline void mavlink_msg_scaled_imu_send(mavlink_channel_t chan, uint64_t */ static inline uint64_t mavlink_msg_scaled_imu_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; + mavlink_scaled_imu_t *p = (mavlink_scaled_imu_t *)&msg->payload[0]; + return (uint64_t)(p->usec); } /** @@ -159,10 +182,8 @@ static inline uint64_t mavlink_msg_scaled_imu_get_usec(const mavlink_message_t* */ static inline int16_t mavlink_msg_scaled_imu_get_xacc(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t))[1]; - return (int16_t)r.s; + mavlink_scaled_imu_t *p = (mavlink_scaled_imu_t *)&msg->payload[0]; + return (int16_t)(p->xacc); } /** @@ -172,10 +193,8 @@ static inline int16_t mavlink_msg_scaled_imu_get_xacc(const mavlink_message_t* m */ static inline int16_t mavlink_msg_scaled_imu_get_yacc(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_scaled_imu_t *p = (mavlink_scaled_imu_t *)&msg->payload[0]; + return (int16_t)(p->yacc); } /** @@ -185,10 +204,8 @@ static inline int16_t mavlink_msg_scaled_imu_get_yacc(const mavlink_message_t* m */ static inline int16_t mavlink_msg_scaled_imu_get_zacc(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_scaled_imu_t *p = (mavlink_scaled_imu_t *)&msg->payload[0]; + return (int16_t)(p->zacc); } /** @@ -198,10 +215,8 @@ static inline int16_t mavlink_msg_scaled_imu_get_zacc(const mavlink_message_t* m */ static inline int16_t mavlink_msg_scaled_imu_get_xgyro(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_scaled_imu_t *p = (mavlink_scaled_imu_t *)&msg->payload[0]; + return (int16_t)(p->xgyro); } /** @@ -211,10 +226,8 @@ static inline int16_t mavlink_msg_scaled_imu_get_xgyro(const mavlink_message_t* */ static inline int16_t mavlink_msg_scaled_imu_get_ygyro(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_scaled_imu_t *p = (mavlink_scaled_imu_t *)&msg->payload[0]; + return (int16_t)(p->ygyro); } /** @@ -224,10 +237,8 @@ static inline int16_t mavlink_msg_scaled_imu_get_ygyro(const mavlink_message_t* */ static inline int16_t mavlink_msg_scaled_imu_get_zgyro(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_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(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_scaled_imu_t *p = (mavlink_scaled_imu_t *)&msg->payload[0]; + return (int16_t)(p->zgyro); } /** @@ -237,10 +248,8 @@ static inline int16_t mavlink_msg_scaled_imu_get_zgyro(const mavlink_message_t* */ static inline int16_t mavlink_msg_scaled_imu_get_xmag(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(int16_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(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_scaled_imu_t *p = (mavlink_scaled_imu_t *)&msg->payload[0]; + return (int16_t)(p->xmag); } /** @@ -250,10 +259,8 @@ static inline int16_t mavlink_msg_scaled_imu_get_xmag(const mavlink_message_t* m */ static inline int16_t mavlink_msg_scaled_imu_get_ymag(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_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(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_scaled_imu_t *p = (mavlink_scaled_imu_t *)&msg->payload[0]; + return (int16_t)(p->ymag); } /** @@ -263,10 +270,8 @@ static inline int16_t mavlink_msg_scaled_imu_get_ymag(const mavlink_message_t* m */ static inline int16_t mavlink_msg_scaled_imu_get_zmag(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_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(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_scaled_imu_t *p = (mavlink_scaled_imu_t *)&msg->payload[0]; + return (int16_t)(p->zmag); } /** @@ -277,14 +282,5 @@ static inline int16_t mavlink_msg_scaled_imu_get_zmag(const mavlink_message_t* m */ static inline void mavlink_msg_scaled_imu_decode(const mavlink_message_t* msg, mavlink_scaled_imu_t* scaled_imu) { - scaled_imu->usec = mavlink_msg_scaled_imu_get_usec(msg); - scaled_imu->xacc = mavlink_msg_scaled_imu_get_xacc(msg); - scaled_imu->yacc = mavlink_msg_scaled_imu_get_yacc(msg); - scaled_imu->zacc = mavlink_msg_scaled_imu_get_zacc(msg); - scaled_imu->xgyro = mavlink_msg_scaled_imu_get_xgyro(msg); - scaled_imu->ygyro = mavlink_msg_scaled_imu_get_ygyro(msg); - scaled_imu->zgyro = mavlink_msg_scaled_imu_get_zgyro(msg); - scaled_imu->xmag = mavlink_msg_scaled_imu_get_xmag(msg); - scaled_imu->ymag = mavlink_msg_scaled_imu_get_ymag(msg); - scaled_imu->zmag = mavlink_msg_scaled_imu_get_zmag(msg); + memcpy( scaled_imu, msg->payload, sizeof(mavlink_scaled_imu_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_scaled_pressure.h b/thirdParty/mavlink/include/common/mavlink_msg_scaled_pressure.h index 08a9bedd37110bd1fb1717d2a8a2a510bf2306d3..39d29440f2e92afedc5aacfc298a48a36dc96680 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_scaled_pressure.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_scaled_pressure.h @@ -1,6 +1,8 @@ // MESSAGE SCALED_PRESSURE PACKING #define MAVLINK_MSG_ID_SCALED_PRESSURE 38 +#define MAVLINK_MSG_ID_SCALED_PRESSURE_LEN 18 +#define MAVLINK_MSG_38_LEN 18 typedef struct __mavlink_scaled_pressure_t { @@ -11,8 +13,6 @@ typedef struct __mavlink_scaled_pressure_t } mavlink_scaled_pressure_t; - - /** * @brief Pack a scaled_pressure message * @param system_id ID of this system @@ -27,15 +27,15 @@ typedef struct __mavlink_scaled_pressure_t */ static inline uint16_t mavlink_msg_scaled_pressure_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t usec, float press_abs, float press_diff, int16_t temperature) { - uint16_t i = 0; + mavlink_scaled_pressure_t *p = (mavlink_scaled_pressure_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SCALED_PRESSURE; - 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(press_abs, i, msg->payload); // Absolute pressure (hectopascal) - i += put_float_by_index(press_diff, i, msg->payload); // Differential pressure 1 (hectopascal) - i += put_int16_t_by_index(temperature, i, msg->payload); // Temperature measurement (0.01 degrees celsius) + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->press_abs = press_abs; // float:Absolute pressure (hectopascal) + p->press_diff = press_diff; // float:Differential pressure 1 (hectopascal) + p->temperature = temperature; // int16_t:Temperature measurement (0.01 degrees celsius) - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN); } /** @@ -52,15 +52,15 @@ static inline uint16_t mavlink_msg_scaled_pressure_pack(uint8_t system_id, uint8 */ static inline uint16_t mavlink_msg_scaled_pressure_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t usec, float press_abs, float press_diff, int16_t temperature) { - uint16_t i = 0; + mavlink_scaled_pressure_t *p = (mavlink_scaled_pressure_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SCALED_PRESSURE; - 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(press_abs, i, msg->payload); // Absolute pressure (hectopascal) - i += put_float_by_index(press_diff, i, msg->payload); // Differential pressure 1 (hectopascal) - i += put_int16_t_by_index(temperature, i, msg->payload); // Temperature measurement (0.01 degrees celsius) + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->press_abs = press_abs; // float:Absolute pressure (hectopascal) + p->press_diff = press_diff; // float:Differential pressure 1 (hectopascal) + p->temperature = temperature; // int16_t:Temperature measurement (0.01 degrees celsius) - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN); } /** @@ -85,13 +85,38 @@ static inline uint16_t mavlink_msg_scaled_pressure_encode(uint8_t system_id, uin * @param press_diff Differential pressure 1 (hectopascal) * @param temperature Temperature measurement (0.01 degrees celsius) */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_scaled_pressure_send(mavlink_channel_t chan, uint64_t usec, float press_abs, float press_diff, int16_t temperature) { - mavlink_message_t msg; - mavlink_msg_scaled_pressure_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, usec, press_abs, press_diff, temperature); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_scaled_pressure_t payload; + uint16_t checksum; + mavlink_scaled_pressure_t *p = &payload; + + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->press_abs = press_abs; // float:Absolute pressure (hectopascal) + p->press_diff = press_diff; // float:Differential pressure 1 (hectopascal) + p->temperature = temperature; // int16_t:Temperature measurement (0.01 degrees celsius) + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_SCALED_PRESSURE_LEN; + hdr.msgid = MAVLINK_MSG_ID_SCALED_PRESSURE; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -104,16 +129,8 @@ static inline void mavlink_msg_scaled_pressure_send(mavlink_channel_t chan, uint */ static inline uint64_t mavlink_msg_scaled_pressure_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; + mavlink_scaled_pressure_t *p = (mavlink_scaled_pressure_t *)&msg->payload[0]; + return (uint64_t)(p->usec); } /** @@ -123,12 +140,8 @@ static inline uint64_t mavlink_msg_scaled_pressure_get_usec(const mavlink_messag */ static inline float mavlink_msg_scaled_pressure_get_press_abs(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; + mavlink_scaled_pressure_t *p = (mavlink_scaled_pressure_t *)&msg->payload[0]; + return (float)(p->press_abs); } /** @@ -138,12 +151,8 @@ static inline float mavlink_msg_scaled_pressure_get_press_abs(const mavlink_mess */ static inline float mavlink_msg_scaled_pressure_get_press_diff(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; + mavlink_scaled_pressure_t *p = (mavlink_scaled_pressure_t *)&msg->payload[0]; + return (float)(p->press_diff); } /** @@ -153,10 +162,8 @@ static inline float mavlink_msg_scaled_pressure_get_press_diff(const mavlink_mes */ static inline int16_t mavlink_msg_scaled_pressure_get_temperature(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[1]; - return (int16_t)r.s; + mavlink_scaled_pressure_t *p = (mavlink_scaled_pressure_t *)&msg->payload[0]; + return (int16_t)(p->temperature); } /** @@ -167,8 +174,5 @@ static inline int16_t mavlink_msg_scaled_pressure_get_temperature(const mavlink_ */ static inline void mavlink_msg_scaled_pressure_decode(const mavlink_message_t* msg, mavlink_scaled_pressure_t* scaled_pressure) { - scaled_pressure->usec = mavlink_msg_scaled_pressure_get_usec(msg); - scaled_pressure->press_abs = mavlink_msg_scaled_pressure_get_press_abs(msg); - scaled_pressure->press_diff = mavlink_msg_scaled_pressure_get_press_diff(msg); - scaled_pressure->temperature = mavlink_msg_scaled_pressure_get_temperature(msg); + memcpy( scaled_pressure, msg->payload, sizeof(mavlink_scaled_pressure_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_servo_output_raw.h b/thirdParty/mavlink/include/common/mavlink_msg_servo_output_raw.h index 751afcb80a3c3c7f8db4a559549efd4c389d91ae..5aa3cc8d0da32490d31e53999d08ce8dd0c48fe7 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_servo_output_raw.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_servo_output_raw.h @@ -1,6 +1,8 @@ // MESSAGE SERVO_OUTPUT_RAW PACKING #define MAVLINK_MSG_ID_SERVO_OUTPUT_RAW 37 +#define MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN 16 +#define MAVLINK_MSG_37_LEN 16 typedef struct __mavlink_servo_output_raw_t { @@ -15,8 +17,6 @@ typedef struct __mavlink_servo_output_raw_t } mavlink_servo_output_raw_t; - - /** * @brief Pack a servo_output_raw message * @param system_id ID of this system @@ -35,19 +35,19 @@ typedef struct __mavlink_servo_output_raw_t */ static inline uint16_t mavlink_msg_servo_output_raw_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint16_t servo1_raw, uint16_t servo2_raw, uint16_t servo3_raw, uint16_t servo4_raw, uint16_t servo5_raw, uint16_t servo6_raw, uint16_t servo7_raw, uint16_t servo8_raw) { - uint16_t i = 0; + mavlink_servo_output_raw_t *p = (mavlink_servo_output_raw_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SERVO_OUTPUT_RAW; - i += put_uint16_t_by_index(servo1_raw, i, msg->payload); // Servo output 1 value, in microseconds - i += put_uint16_t_by_index(servo2_raw, i, msg->payload); // Servo output 2 value, in microseconds - i += put_uint16_t_by_index(servo3_raw, i, msg->payload); // Servo output 3 value, in microseconds - i += put_uint16_t_by_index(servo4_raw, i, msg->payload); // Servo output 4 value, in microseconds - i += put_uint16_t_by_index(servo5_raw, i, msg->payload); // Servo output 5 value, in microseconds - i += put_uint16_t_by_index(servo6_raw, i, msg->payload); // Servo output 6 value, in microseconds - i += put_uint16_t_by_index(servo7_raw, i, msg->payload); // Servo output 7 value, in microseconds - i += put_uint16_t_by_index(servo8_raw, i, msg->payload); // Servo output 8 value, in microseconds + p->servo1_raw = servo1_raw; // uint16_t:Servo output 1 value, in microseconds + p->servo2_raw = servo2_raw; // uint16_t:Servo output 2 value, in microseconds + p->servo3_raw = servo3_raw; // uint16_t:Servo output 3 value, in microseconds + p->servo4_raw = servo4_raw; // uint16_t:Servo output 4 value, in microseconds + p->servo5_raw = servo5_raw; // uint16_t:Servo output 5 value, in microseconds + p->servo6_raw = servo6_raw; // uint16_t:Servo output 6 value, in microseconds + p->servo7_raw = servo7_raw; // uint16_t:Servo output 7 value, in microseconds + p->servo8_raw = servo8_raw; // uint16_t:Servo output 8 value, in microseconds - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN); } /** @@ -68,19 +68,19 @@ static inline uint16_t mavlink_msg_servo_output_raw_pack(uint8_t system_id, uint */ static inline uint16_t mavlink_msg_servo_output_raw_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint16_t servo1_raw, uint16_t servo2_raw, uint16_t servo3_raw, uint16_t servo4_raw, uint16_t servo5_raw, uint16_t servo6_raw, uint16_t servo7_raw, uint16_t servo8_raw) { - uint16_t i = 0; + mavlink_servo_output_raw_t *p = (mavlink_servo_output_raw_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SERVO_OUTPUT_RAW; - i += put_uint16_t_by_index(servo1_raw, i, msg->payload); // Servo output 1 value, in microseconds - i += put_uint16_t_by_index(servo2_raw, i, msg->payload); // Servo output 2 value, in microseconds - i += put_uint16_t_by_index(servo3_raw, i, msg->payload); // Servo output 3 value, in microseconds - i += put_uint16_t_by_index(servo4_raw, i, msg->payload); // Servo output 4 value, in microseconds - i += put_uint16_t_by_index(servo5_raw, i, msg->payload); // Servo output 5 value, in microseconds - i += put_uint16_t_by_index(servo6_raw, i, msg->payload); // Servo output 6 value, in microseconds - i += put_uint16_t_by_index(servo7_raw, i, msg->payload); // Servo output 7 value, in microseconds - i += put_uint16_t_by_index(servo8_raw, i, msg->payload); // Servo output 8 value, in microseconds + p->servo1_raw = servo1_raw; // uint16_t:Servo output 1 value, in microseconds + p->servo2_raw = servo2_raw; // uint16_t:Servo output 2 value, in microseconds + p->servo3_raw = servo3_raw; // uint16_t:Servo output 3 value, in microseconds + p->servo4_raw = servo4_raw; // uint16_t:Servo output 4 value, in microseconds + p->servo5_raw = servo5_raw; // uint16_t:Servo output 5 value, in microseconds + p->servo6_raw = servo6_raw; // uint16_t:Servo output 6 value, in microseconds + p->servo7_raw = servo7_raw; // uint16_t:Servo output 7 value, in microseconds + p->servo8_raw = servo8_raw; // uint16_t:Servo output 8 value, in microseconds - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN); } /** @@ -109,13 +109,42 @@ static inline uint16_t mavlink_msg_servo_output_raw_encode(uint8_t system_id, ui * @param servo7_raw Servo output 7 value, in microseconds * @param servo8_raw Servo output 8 value, in microseconds */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_servo_output_raw_send(mavlink_channel_t chan, uint16_t servo1_raw, uint16_t servo2_raw, uint16_t servo3_raw, uint16_t servo4_raw, uint16_t servo5_raw, uint16_t servo6_raw, uint16_t servo7_raw, uint16_t servo8_raw) { - mavlink_message_t msg; - mavlink_msg_servo_output_raw_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, servo1_raw, servo2_raw, servo3_raw, servo4_raw, servo5_raw, servo6_raw, servo7_raw, servo8_raw); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_servo_output_raw_t payload; + uint16_t checksum; + mavlink_servo_output_raw_t *p = &payload; + + p->servo1_raw = servo1_raw; // uint16_t:Servo output 1 value, in microseconds + p->servo2_raw = servo2_raw; // uint16_t:Servo output 2 value, in microseconds + p->servo3_raw = servo3_raw; // uint16_t:Servo output 3 value, in microseconds + p->servo4_raw = servo4_raw; // uint16_t:Servo output 4 value, in microseconds + p->servo5_raw = servo5_raw; // uint16_t:Servo output 5 value, in microseconds + p->servo6_raw = servo6_raw; // uint16_t:Servo output 6 value, in microseconds + p->servo7_raw = servo7_raw; // uint16_t:Servo output 7 value, in microseconds + p->servo8_raw = servo8_raw; // uint16_t:Servo output 8 value, in microseconds + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN; + hdr.msgid = MAVLINK_MSG_ID_SERVO_OUTPUT_RAW; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -128,10 +157,8 @@ static inline void mavlink_msg_servo_output_raw_send(mavlink_channel_t chan, uin */ static inline uint16_t mavlink_msg_servo_output_raw_get_servo1_raw(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload)[0]; - r.b[0] = (msg->payload)[1]; - return (uint16_t)r.s; + mavlink_servo_output_raw_t *p = (mavlink_servo_output_raw_t *)&msg->payload[0]; + return (uint16_t)(p->servo1_raw); } /** @@ -141,10 +168,8 @@ static inline uint16_t mavlink_msg_servo_output_raw_get_servo1_raw(const mavlink */ static inline uint16_t mavlink_msg_servo_output_raw_get_servo2_raw(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_servo_output_raw_t *p = (mavlink_servo_output_raw_t *)&msg->payload[0]; + return (uint16_t)(p->servo2_raw); } /** @@ -154,10 +179,8 @@ static inline uint16_t mavlink_msg_servo_output_raw_get_servo2_raw(const mavlink */ static inline uint16_t mavlink_msg_servo_output_raw_get_servo3_raw(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_servo_output_raw_t *p = (mavlink_servo_output_raw_t *)&msg->payload[0]; + return (uint16_t)(p->servo3_raw); } /** @@ -167,10 +190,8 @@ static inline uint16_t mavlink_msg_servo_output_raw_get_servo3_raw(const mavlink */ static inline uint16_t mavlink_msg_servo_output_raw_get_servo4_raw(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_servo_output_raw_t *p = (mavlink_servo_output_raw_t *)&msg->payload[0]; + return (uint16_t)(p->servo4_raw); } /** @@ -180,10 +201,8 @@ static inline uint16_t mavlink_msg_servo_output_raw_get_servo4_raw(const mavlink */ static inline uint16_t mavlink_msg_servo_output_raw_get_servo5_raw(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_servo_output_raw_t *p = (mavlink_servo_output_raw_t *)&msg->payload[0]; + return (uint16_t)(p->servo5_raw); } /** @@ -193,10 +212,8 @@ static inline uint16_t mavlink_msg_servo_output_raw_get_servo5_raw(const mavlink */ static inline uint16_t mavlink_msg_servo_output_raw_get_servo6_raw(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_servo_output_raw_t *p = (mavlink_servo_output_raw_t *)&msg->payload[0]; + return (uint16_t)(p->servo6_raw); } /** @@ -206,10 +223,8 @@ static inline uint16_t mavlink_msg_servo_output_raw_get_servo6_raw(const mavlink */ static inline uint16_t mavlink_msg_servo_output_raw_get_servo7_raw(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+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(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_servo_output_raw_t *p = (mavlink_servo_output_raw_t *)&msg->payload[0]; + return (uint16_t)(p->servo7_raw); } /** @@ -219,10 +234,8 @@ static inline uint16_t mavlink_msg_servo_output_raw_get_servo7_raw(const mavlink */ static inline uint16_t mavlink_msg_servo_output_raw_get_servo8_raw(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+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(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; + mavlink_servo_output_raw_t *p = (mavlink_servo_output_raw_t *)&msg->payload[0]; + return (uint16_t)(p->servo8_raw); } /** @@ -233,12 +246,5 @@ static inline uint16_t mavlink_msg_servo_output_raw_get_servo8_raw(const mavlink */ static inline void mavlink_msg_servo_output_raw_decode(const mavlink_message_t* msg, mavlink_servo_output_raw_t* servo_output_raw) { - servo_output_raw->servo1_raw = mavlink_msg_servo_output_raw_get_servo1_raw(msg); - servo_output_raw->servo2_raw = mavlink_msg_servo_output_raw_get_servo2_raw(msg); - servo_output_raw->servo3_raw = mavlink_msg_servo_output_raw_get_servo3_raw(msg); - servo_output_raw->servo4_raw = mavlink_msg_servo_output_raw_get_servo4_raw(msg); - servo_output_raw->servo5_raw = mavlink_msg_servo_output_raw_get_servo5_raw(msg); - servo_output_raw->servo6_raw = mavlink_msg_servo_output_raw_get_servo6_raw(msg); - servo_output_raw->servo7_raw = mavlink_msg_servo_output_raw_get_servo7_raw(msg); - servo_output_raw->servo8_raw = mavlink_msg_servo_output_raw_get_servo8_raw(msg); + memcpy( servo_output_raw, msg->payload, sizeof(mavlink_servo_output_raw_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_set_altitude.h b/thirdParty/mavlink/include/common/mavlink_msg_set_altitude.h index 775db90e343bb67430894f9281f5ad063761f7dd..9b50027f1d1562ad6d81353f96a439e49eb3962a 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_set_altitude.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_set_altitude.h @@ -1,16 +1,16 @@ // MESSAGE SET_ALTITUDE PACKING #define MAVLINK_MSG_ID_SET_ALTITUDE 65 +#define MAVLINK_MSG_ID_SET_ALTITUDE_LEN 5 +#define MAVLINK_MSG_65_LEN 5 typedef struct __mavlink_set_altitude_t { - uint8_t target; ///< The system setting the altitude uint32_t mode; ///< The new altitude in meters + uint8_t target; ///< The system setting the altitude } mavlink_set_altitude_t; - - /** * @brief Pack a set_altitude message * @param system_id ID of this system @@ -23,13 +23,13 @@ typedef struct __mavlink_set_altitude_t */ static inline uint16_t mavlink_msg_set_altitude_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target, uint32_t mode) { - uint16_t i = 0; + mavlink_set_altitude_t *p = (mavlink_set_altitude_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SET_ALTITUDE; - i += put_uint8_t_by_index(target, i, msg->payload); // The system setting the altitude - i += put_uint32_t_by_index(mode, i, msg->payload); // The new altitude in meters + p->target = target; // uint8_t:The system setting the altitude + p->mode = mode; // uint32_t:The new altitude in meters - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_ALTITUDE_LEN); } /** @@ -44,13 +44,13 @@ static inline uint16_t mavlink_msg_set_altitude_pack(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_set_altitude_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target, uint32_t mode) { - uint16_t i = 0; + mavlink_set_altitude_t *p = (mavlink_set_altitude_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SET_ALTITUDE; - i += put_uint8_t_by_index(target, i, msg->payload); // The system setting the altitude - i += put_uint32_t_by_index(mode, i, msg->payload); // The new altitude in meters + p->target = target; // uint8_t:The system setting the altitude + p->mode = mode; // uint32_t:The new altitude in meters - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_ALTITUDE_LEN); } /** @@ -73,13 +73,36 @@ static inline uint16_t mavlink_msg_set_altitude_encode(uint8_t system_id, uint8_ * @param target The system setting the altitude * @param mode The new altitude in meters */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_set_altitude_send(mavlink_channel_t chan, uint8_t target, uint32_t mode) { - mavlink_message_t msg; - mavlink_msg_set_altitude_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target, mode); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_set_altitude_t payload; + uint16_t checksum; + mavlink_set_altitude_t *p = &payload; + + p->target = target; // uint8_t:The system setting the altitude + p->mode = mode; // uint32_t:The new altitude in meters + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_SET_ALTITUDE_LEN; + hdr.msgid = MAVLINK_MSG_ID_SET_ALTITUDE; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -92,7 +115,8 @@ static inline void mavlink_msg_set_altitude_send(mavlink_channel_t chan, uint8_t */ static inline uint8_t mavlink_msg_set_altitude_get_target(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_set_altitude_t *p = (mavlink_set_altitude_t *)&msg->payload[0]; + return (uint8_t)(p->target); } /** @@ -102,12 +126,8 @@ static inline uint8_t mavlink_msg_set_altitude_get_target(const mavlink_message_ */ static inline uint32_t mavlink_msg_set_altitude_get_mode(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t))[3]; - return (uint32_t)r.i; + mavlink_set_altitude_t *p = (mavlink_set_altitude_t *)&msg->payload[0]; + return (uint32_t)(p->mode); } /** @@ -118,6 +138,5 @@ static inline uint32_t mavlink_msg_set_altitude_get_mode(const mavlink_message_t */ static inline void mavlink_msg_set_altitude_decode(const mavlink_message_t* msg, mavlink_set_altitude_t* set_altitude) { - set_altitude->target = mavlink_msg_set_altitude_get_target(msg); - set_altitude->mode = mavlink_msg_set_altitude_get_mode(msg); + memcpy( set_altitude, msg->payload, sizeof(mavlink_set_altitude_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_set_mode.h b/thirdParty/mavlink/include/common/mavlink_msg_set_mode.h index efeea6509f070903e55a2728f123cce48810ae73..ad796f1e0ee65632a954cb139579d4850ebe1388 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_set_mode.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_set_mode.h @@ -1,6 +1,8 @@ // MESSAGE SET_MODE PACKING #define MAVLINK_MSG_ID_SET_MODE 11 +#define MAVLINK_MSG_ID_SET_MODE_LEN 2 +#define MAVLINK_MSG_11_LEN 2 typedef struct __mavlink_set_mode_t { @@ -9,8 +11,6 @@ typedef struct __mavlink_set_mode_t } mavlink_set_mode_t; - - /** * @brief Pack a set_mode message * @param system_id ID of this system @@ -23,13 +23,13 @@ typedef struct __mavlink_set_mode_t */ static inline uint16_t mavlink_msg_set_mode_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target, uint8_t mode) { - uint16_t i = 0; + mavlink_set_mode_t *p = (mavlink_set_mode_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SET_MODE; - i += put_uint8_t_by_index(target, i, msg->payload); // The system setting the mode - i += put_uint8_t_by_index(mode, i, msg->payload); // The new mode + p->target = target; // uint8_t:The system setting the mode + p->mode = mode; // uint8_t:The new mode - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_MODE_LEN); } /** @@ -44,13 +44,13 @@ static inline uint16_t mavlink_msg_set_mode_pack(uint8_t system_id, uint8_t comp */ static inline uint16_t mavlink_msg_set_mode_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target, uint8_t mode) { - uint16_t i = 0; + mavlink_set_mode_t *p = (mavlink_set_mode_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SET_MODE; - i += put_uint8_t_by_index(target, i, msg->payload); // The system setting the mode - i += put_uint8_t_by_index(mode, i, msg->payload); // The new mode + p->target = target; // uint8_t:The system setting the mode + p->mode = mode; // uint8_t:The new mode - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_MODE_LEN); } /** @@ -73,13 +73,36 @@ static inline uint16_t mavlink_msg_set_mode_encode(uint8_t system_id, uint8_t co * @param target The system setting the mode * @param mode The new mode */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_set_mode_send(mavlink_channel_t chan, uint8_t target, uint8_t mode) { - mavlink_message_t msg; - mavlink_msg_set_mode_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target, mode); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_set_mode_t payload; + uint16_t checksum; + mavlink_set_mode_t *p = &payload; + + p->target = target; // uint8_t:The system setting the mode + p->mode = mode; // uint8_t:The new mode + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_SET_MODE_LEN; + hdr.msgid = MAVLINK_MSG_ID_SET_MODE; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -92,7 +115,8 @@ static inline void mavlink_msg_set_mode_send(mavlink_channel_t chan, uint8_t tar */ static inline uint8_t mavlink_msg_set_mode_get_target(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_set_mode_t *p = (mavlink_set_mode_t *)&msg->payload[0]; + return (uint8_t)(p->target); } /** @@ -102,7 +126,8 @@ static inline uint8_t mavlink_msg_set_mode_get_target(const mavlink_message_t* m */ static inline uint8_t mavlink_msg_set_mode_get_mode(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_set_mode_t *p = (mavlink_set_mode_t *)&msg->payload[0]; + return (uint8_t)(p->mode); } /** @@ -113,6 +138,5 @@ static inline uint8_t mavlink_msg_set_mode_get_mode(const mavlink_message_t* msg */ static inline void mavlink_msg_set_mode_decode(const mavlink_message_t* msg, mavlink_set_mode_t* set_mode) { - set_mode->target = mavlink_msg_set_mode_get_target(msg); - set_mode->mode = mavlink_msg_set_mode_get_mode(msg); + memcpy( set_mode, msg->payload, sizeof(mavlink_set_mode_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_set_nav_mode.h b/thirdParty/mavlink/include/common/mavlink_msg_set_nav_mode.h index fe514b2ea53cebb352fc8747e8477d4797e23713..11474400976281d370fa834c4de25ec3ce7a5f8c 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_set_nav_mode.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_set_nav_mode.h @@ -1,6 +1,8 @@ // MESSAGE SET_NAV_MODE PACKING #define MAVLINK_MSG_ID_SET_NAV_MODE 12 +#define MAVLINK_MSG_ID_SET_NAV_MODE_LEN 2 +#define MAVLINK_MSG_12_LEN 2 typedef struct __mavlink_set_nav_mode_t { @@ -9,8 +11,6 @@ typedef struct __mavlink_set_nav_mode_t } mavlink_set_nav_mode_t; - - /** * @brief Pack a set_nav_mode message * @param system_id ID of this system @@ -23,13 +23,13 @@ typedef struct __mavlink_set_nav_mode_t */ static inline uint16_t mavlink_msg_set_nav_mode_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target, uint8_t nav_mode) { - uint16_t i = 0; + mavlink_set_nav_mode_t *p = (mavlink_set_nav_mode_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SET_NAV_MODE; - i += put_uint8_t_by_index(target, i, msg->payload); // The system setting the mode - i += put_uint8_t_by_index(nav_mode, i, msg->payload); // The new navigation mode + p->target = target; // uint8_t:The system setting the mode + p->nav_mode = nav_mode; // uint8_t:The new navigation mode - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_NAV_MODE_LEN); } /** @@ -44,13 +44,13 @@ static inline uint16_t mavlink_msg_set_nav_mode_pack(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_set_nav_mode_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target, uint8_t nav_mode) { - uint16_t i = 0; + mavlink_set_nav_mode_t *p = (mavlink_set_nav_mode_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SET_NAV_MODE; - i += put_uint8_t_by_index(target, i, msg->payload); // The system setting the mode - i += put_uint8_t_by_index(nav_mode, i, msg->payload); // The new navigation mode + p->target = target; // uint8_t:The system setting the mode + p->nav_mode = nav_mode; // uint8_t:The new navigation mode - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_NAV_MODE_LEN); } /** @@ -73,13 +73,36 @@ static inline uint16_t mavlink_msg_set_nav_mode_encode(uint8_t system_id, uint8_ * @param target The system setting the mode * @param nav_mode The new navigation mode */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_set_nav_mode_send(mavlink_channel_t chan, uint8_t target, uint8_t nav_mode) { - mavlink_message_t msg; - mavlink_msg_set_nav_mode_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target, nav_mode); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_set_nav_mode_t payload; + uint16_t checksum; + mavlink_set_nav_mode_t *p = &payload; + + p->target = target; // uint8_t:The system setting the mode + p->nav_mode = nav_mode; // uint8_t:The new navigation mode + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_SET_NAV_MODE_LEN; + hdr.msgid = MAVLINK_MSG_ID_SET_NAV_MODE; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -92,7 +115,8 @@ static inline void mavlink_msg_set_nav_mode_send(mavlink_channel_t chan, uint8_t */ static inline uint8_t mavlink_msg_set_nav_mode_get_target(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_set_nav_mode_t *p = (mavlink_set_nav_mode_t *)&msg->payload[0]; + return (uint8_t)(p->target); } /** @@ -102,7 +126,8 @@ static inline uint8_t mavlink_msg_set_nav_mode_get_target(const mavlink_message_ */ static inline uint8_t mavlink_msg_set_nav_mode_get_nav_mode(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_set_nav_mode_t *p = (mavlink_set_nav_mode_t *)&msg->payload[0]; + return (uint8_t)(p->nav_mode); } /** @@ -113,6 +138,5 @@ static inline uint8_t mavlink_msg_set_nav_mode_get_nav_mode(const mavlink_messag */ static inline void mavlink_msg_set_nav_mode_decode(const mavlink_message_t* msg, mavlink_set_nav_mode_t* set_nav_mode) { - set_nav_mode->target = mavlink_msg_set_nav_mode_get_target(msg); - set_nav_mode->nav_mode = mavlink_msg_set_nav_mode_get_nav_mode(msg); + memcpy( set_nav_mode, msg->payload, sizeof(mavlink_set_nav_mode_t)); } 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..1fc909fc4b205cef981233967cf303166bb3e50f --- /dev/null +++ b/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw.h @@ -0,0 +1,196 @@ +// MESSAGE SET_ROLL_PITCH_YAW PACKING + +#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW 55 +#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_LEN 14 +#define MAVLINK_MSG_55_LEN 14 + +typedef struct __mavlink_set_roll_pitch_yaw_t +{ + float roll; ///< Desired roll angle in radians + float pitch; ///< Desired pitch angle in radians + float yaw; ///< Desired yaw angle in radians + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + +} 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) +{ + mavlink_set_roll_pitch_yaw_t *p = (mavlink_set_roll_pitch_yaw_t *)&msg->payload[0]; + msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->roll = roll; // float:Desired roll angle in radians + p->pitch = pitch; // float:Desired pitch angle in radians + p->yaw = yaw; // float:Desired yaw angle in radians + + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_LEN); +} + +/** + * @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) +{ + mavlink_set_roll_pitch_yaw_t *p = (mavlink_set_roll_pitch_yaw_t *)&msg->payload[0]; + msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->roll = roll; // float:Desired roll angle in radians + p->pitch = pitch; // float:Desired pitch angle in radians + p->yaw = yaw; // float:Desired yaw angle in radians + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_LEN); +} + +/** + * @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_header_t hdr; + mavlink_set_roll_pitch_yaw_t payload; + uint16_t checksum; + mavlink_set_roll_pitch_yaw_t *p = &payload; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->roll = roll; // float:Desired roll angle in radians + p->pitch = pitch; // float:Desired pitch angle in radians + p->yaw = yaw; // float:Desired yaw angle in radians + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_LEN; + hdr.msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); +} + +#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) +{ + mavlink_set_roll_pitch_yaw_t *p = (mavlink_set_roll_pitch_yaw_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); +} + +/** + * @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) +{ + mavlink_set_roll_pitch_yaw_t *p = (mavlink_set_roll_pitch_yaw_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); +} + +/** + * @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) +{ + mavlink_set_roll_pitch_yaw_t *p = (mavlink_set_roll_pitch_yaw_t *)&msg->payload[0]; + return (float)(p->roll); +} + +/** + * @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) +{ + mavlink_set_roll_pitch_yaw_t *p = (mavlink_set_roll_pitch_yaw_t *)&msg->payload[0]; + return (float)(p->pitch); +} + +/** + * @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) +{ + mavlink_set_roll_pitch_yaw_t *p = (mavlink_set_roll_pitch_yaw_t *)&msg->payload[0]; + return (float)(p->yaw); +} + +/** + * @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) +{ + memcpy( set_roll_pitch_yaw, msg->payload, sizeof(mavlink_set_roll_pitch_yaw_t)); +} 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..bb5858356fd0adc1a36090afb20f8212d2a97984 --- /dev/null +++ b/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw_speed.h @@ -0,0 +1,196 @@ +// MESSAGE SET_ROLL_PITCH_YAW_SPEED PACKING + +#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED 56 +#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_LEN 14 +#define MAVLINK_MSG_56_LEN 14 + +typedef struct __mavlink_set_roll_pitch_yaw_speed_t +{ + 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 + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + +} 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) +{ + mavlink_set_roll_pitch_yaw_speed_t *p = (mavlink_set_roll_pitch_yaw_speed_t *)&msg->payload[0]; + msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->roll_speed = roll_speed; // float:Desired roll angular speed in rad/s + p->pitch_speed = pitch_speed; // float:Desired pitch angular speed in rad/s + p->yaw_speed = yaw_speed; // float:Desired yaw angular speed in rad/s + + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_LEN); +} + +/** + * @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) +{ + mavlink_set_roll_pitch_yaw_speed_t *p = (mavlink_set_roll_pitch_yaw_speed_t *)&msg->payload[0]; + msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->roll_speed = roll_speed; // float:Desired roll angular speed in rad/s + p->pitch_speed = pitch_speed; // float:Desired pitch angular speed in rad/s + p->yaw_speed = yaw_speed; // float:Desired yaw angular speed in rad/s + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_LEN); +} + +/** + * @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_header_t hdr; + mavlink_set_roll_pitch_yaw_speed_t payload; + uint16_t checksum; + mavlink_set_roll_pitch_yaw_speed_t *p = &payload; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->roll_speed = roll_speed; // float:Desired roll angular speed in rad/s + p->pitch_speed = pitch_speed; // float:Desired pitch angular speed in rad/s + p->yaw_speed = yaw_speed; // float:Desired yaw angular speed in rad/s + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_LEN; + hdr.msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); +} + +#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) +{ + mavlink_set_roll_pitch_yaw_speed_t *p = (mavlink_set_roll_pitch_yaw_speed_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); +} + +/** + * @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) +{ + mavlink_set_roll_pitch_yaw_speed_t *p = (mavlink_set_roll_pitch_yaw_speed_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); +} + +/** + * @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) +{ + mavlink_set_roll_pitch_yaw_speed_t *p = (mavlink_set_roll_pitch_yaw_speed_t *)&msg->payload[0]; + return (float)(p->roll_speed); +} + +/** + * @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) +{ + mavlink_set_roll_pitch_yaw_speed_t *p = (mavlink_set_roll_pitch_yaw_speed_t *)&msg->payload[0]; + return (float)(p->pitch_speed); +} + +/** + * @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) +{ + mavlink_set_roll_pitch_yaw_speed_t *p = (mavlink_set_roll_pitch_yaw_speed_t *)&msg->payload[0]; + return (float)(p->yaw_speed); +} + +/** + * @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) +{ + memcpy( set_roll_pitch_yaw_speed, msg->payload, sizeof(mavlink_set_roll_pitch_yaw_speed_t)); +} diff --git a/thirdParty/mavlink/include/common/mavlink_msg_state_correction.h b/thirdParty/mavlink/include/common/mavlink_msg_state_correction.h index c4c6b5abc9e36a7d3bbca3e4b3bb7610c30410c1..f101513eb8c301501c3a757c3f5726dde773ed96 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_state_correction.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_state_correction.h @@ -1,6 +1,8 @@ // MESSAGE STATE_CORRECTION PACKING #define MAVLINK_MSG_ID_STATE_CORRECTION 64 +#define MAVLINK_MSG_ID_STATE_CORRECTION_LEN 36 +#define MAVLINK_MSG_64_LEN 36 typedef struct __mavlink_state_correction_t { @@ -16,8 +18,6 @@ typedef struct __mavlink_state_correction_t } mavlink_state_correction_t; - - /** * @brief Pack a state_correction message * @param system_id ID of this system @@ -37,20 +37,20 @@ typedef struct __mavlink_state_correction_t */ static inline uint16_t mavlink_msg_state_correction_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, float xErr, float yErr, float zErr, float rollErr, float pitchErr, float yawErr, float vxErr, float vyErr, float vzErr) { - uint16_t i = 0; + mavlink_state_correction_t *p = (mavlink_state_correction_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_STATE_CORRECTION; - i += put_float_by_index(xErr, i, msg->payload); // x position error - i += put_float_by_index(yErr, i, msg->payload); // y position error - i += put_float_by_index(zErr, i, msg->payload); // z position error - i += put_float_by_index(rollErr, i, msg->payload); // roll error (radians) - i += put_float_by_index(pitchErr, i, msg->payload); // pitch error (radians) - i += put_float_by_index(yawErr, i, msg->payload); // yaw error (radians) - i += put_float_by_index(vxErr, i, msg->payload); // x velocity - i += put_float_by_index(vyErr, i, msg->payload); // y velocity - i += put_float_by_index(vzErr, i, msg->payload); // z velocity + p->xErr = xErr; // float:x position error + p->yErr = yErr; // float:y position error + p->zErr = zErr; // float:z position error + p->rollErr = rollErr; // float:roll error (radians) + p->pitchErr = pitchErr; // float:pitch error (radians) + p->yawErr = yawErr; // float:yaw error (radians) + p->vxErr = vxErr; // float:x velocity + p->vyErr = vyErr; // float:y velocity + p->vzErr = vzErr; // float:z velocity - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_STATE_CORRECTION_LEN); } /** @@ -72,20 +72,20 @@ static inline uint16_t mavlink_msg_state_correction_pack(uint8_t system_id, uint */ static inline uint16_t mavlink_msg_state_correction_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, float xErr, float yErr, float zErr, float rollErr, float pitchErr, float yawErr, float vxErr, float vyErr, float vzErr) { - uint16_t i = 0; + mavlink_state_correction_t *p = (mavlink_state_correction_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_STATE_CORRECTION; - i += put_float_by_index(xErr, i, msg->payload); // x position error - i += put_float_by_index(yErr, i, msg->payload); // y position error - i += put_float_by_index(zErr, i, msg->payload); // z position error - i += put_float_by_index(rollErr, i, msg->payload); // roll error (radians) - i += put_float_by_index(pitchErr, i, msg->payload); // pitch error (radians) - i += put_float_by_index(yawErr, i, msg->payload); // yaw error (radians) - i += put_float_by_index(vxErr, i, msg->payload); // x velocity - i += put_float_by_index(vyErr, i, msg->payload); // y velocity - i += put_float_by_index(vzErr, i, msg->payload); // z velocity + p->xErr = xErr; // float:x position error + p->yErr = yErr; // float:y position error + p->zErr = zErr; // float:z position error + p->rollErr = rollErr; // float:roll error (radians) + p->pitchErr = pitchErr; // float:pitch error (radians) + p->yawErr = yawErr; // float:yaw error (radians) + p->vxErr = vxErr; // float:x velocity + p->vyErr = vyErr; // float:y velocity + p->vzErr = vzErr; // float:z velocity - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_STATE_CORRECTION_LEN); } /** @@ -115,13 +115,43 @@ static inline uint16_t mavlink_msg_state_correction_encode(uint8_t system_id, ui * @param vyErr y velocity * @param vzErr z velocity */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_state_correction_send(mavlink_channel_t chan, float xErr, float yErr, float zErr, float rollErr, float pitchErr, float yawErr, float vxErr, float vyErr, float vzErr) { - mavlink_message_t msg; - mavlink_msg_state_correction_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, xErr, yErr, zErr, rollErr, pitchErr, yawErr, vxErr, vyErr, vzErr); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_state_correction_t payload; + uint16_t checksum; + mavlink_state_correction_t *p = &payload; + + p->xErr = xErr; // float:x position error + p->yErr = yErr; // float:y position error + p->zErr = zErr; // float:z position error + p->rollErr = rollErr; // float:roll error (radians) + p->pitchErr = pitchErr; // float:pitch error (radians) + p->yawErr = yawErr; // float:yaw error (radians) + p->vxErr = vxErr; // float:x velocity + p->vyErr = vyErr; // float:y velocity + p->vzErr = vzErr; // float:z velocity + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_STATE_CORRECTION_LEN; + hdr.msgid = MAVLINK_MSG_ID_STATE_CORRECTION; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -134,12 +164,8 @@ static inline void mavlink_msg_state_correction_send(mavlink_channel_t chan, flo */ static inline float mavlink_msg_state_correction_get_xErr(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 (float)r.f; + mavlink_state_correction_t *p = (mavlink_state_correction_t *)&msg->payload[0]; + return (float)(p->xErr); } /** @@ -149,12 +175,8 @@ static inline float mavlink_msg_state_correction_get_xErr(const mavlink_message_ */ static inline float mavlink_msg_state_correction_get_yErr(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float))[3]; - return (float)r.f; + mavlink_state_correction_t *p = (mavlink_state_correction_t *)&msg->payload[0]; + return (float)(p->yErr); } /** @@ -164,12 +186,8 @@ static inline float mavlink_msg_state_correction_get_yErr(const mavlink_message_ */ static inline float mavlink_msg_state_correction_get_zErr(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_state_correction_t *p = (mavlink_state_correction_t *)&msg->payload[0]; + return (float)(p->zErr); } /** @@ -179,12 +197,8 @@ static inline float mavlink_msg_state_correction_get_zErr(const mavlink_message_ */ static inline float mavlink_msg_state_correction_get_rollErr(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_state_correction_t *p = (mavlink_state_correction_t *)&msg->payload[0]; + return (float)(p->rollErr); } /** @@ -194,12 +208,8 @@ static inline float mavlink_msg_state_correction_get_rollErr(const mavlink_messa */ static inline float mavlink_msg_state_correction_get_pitchErr(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_state_correction_t *p = (mavlink_state_correction_t *)&msg->payload[0]; + return (float)(p->pitchErr); } /** @@ -209,12 +219,8 @@ static inline float mavlink_msg_state_correction_get_pitchErr(const mavlink_mess */ static inline float mavlink_msg_state_correction_get_yawErr(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_state_correction_t *p = (mavlink_state_correction_t *)&msg->payload[0]; + return (float)(p->yawErr); } /** @@ -224,12 +230,8 @@ static inline float mavlink_msg_state_correction_get_yawErr(const mavlink_messag */ static inline float mavlink_msg_state_correction_get_vxErr(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_state_correction_t *p = (mavlink_state_correction_t *)&msg->payload[0]; + return (float)(p->vxErr); } /** @@ -239,12 +241,8 @@ static inline float mavlink_msg_state_correction_get_vxErr(const mavlink_message */ static inline float mavlink_msg_state_correction_get_vyErr(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_state_correction_t *p = (mavlink_state_correction_t *)&msg->payload[0]; + return (float)(p->vyErr); } /** @@ -254,12 +252,8 @@ static inline float mavlink_msg_state_correction_get_vyErr(const mavlink_message */ static inline float mavlink_msg_state_correction_get_vzErr(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_state_correction_t *p = (mavlink_state_correction_t *)&msg->payload[0]; + return (float)(p->vzErr); } /** @@ -270,13 +264,5 @@ static inline float mavlink_msg_state_correction_get_vzErr(const mavlink_message */ static inline void mavlink_msg_state_correction_decode(const mavlink_message_t* msg, mavlink_state_correction_t* state_correction) { - state_correction->xErr = mavlink_msg_state_correction_get_xErr(msg); - state_correction->yErr = mavlink_msg_state_correction_get_yErr(msg); - state_correction->zErr = mavlink_msg_state_correction_get_zErr(msg); - state_correction->rollErr = mavlink_msg_state_correction_get_rollErr(msg); - state_correction->pitchErr = mavlink_msg_state_correction_get_pitchErr(msg); - state_correction->yawErr = mavlink_msg_state_correction_get_yawErr(msg); - state_correction->vxErr = mavlink_msg_state_correction_get_vxErr(msg); - state_correction->vyErr = mavlink_msg_state_correction_get_vyErr(msg); - state_correction->vzErr = mavlink_msg_state_correction_get_vzErr(msg); + memcpy( state_correction, msg->payload, sizeof(mavlink_state_correction_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_statustext.h b/thirdParty/mavlink/include/common/mavlink_msg_statustext.h index 681b659c4d2c75aa1c27de58bdcb0a9efadcab70..e53bc5390ed1f6011b2139a3d083d6e3991909c3 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_statustext.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_statustext.h @@ -1,17 +1,17 @@ // MESSAGE STATUSTEXT PACKING #define MAVLINK_MSG_ID_STATUSTEXT 254 +#define MAVLINK_MSG_ID_STATUSTEXT_LEN 51 +#define MAVLINK_MSG_254_LEN 51 typedef struct __mavlink_statustext_t { uint8_t severity; ///< Severity of status, 0 = info message, 255 = critical fault - int8_t text[50]; ///< Status text message, without null termination character + char text[50]; ///< Status text message, without null termination character } mavlink_statustext_t; - #define MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN 50 - /** * @brief Pack a statustext message * @param system_id ID of this system @@ -22,15 +22,15 @@ typedef struct __mavlink_statustext_t * @param text Status text message, without null termination character * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_statustext_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t severity, const int8_t* text) +static inline uint16_t mavlink_msg_statustext_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t severity, const char* text) { - uint16_t i = 0; + mavlink_statustext_t *p = (mavlink_statustext_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_STATUSTEXT; - i += put_uint8_t_by_index(severity, i, msg->payload); // Severity of status, 0 = info message, 255 = critical fault - i += put_array_by_index((const int8_t*)text, sizeof(int8_t)*50, i, msg->payload); // Status text message, without null termination character + p->severity = severity; // uint8_t:Severity of status, 0 = info message, 255 = critical fault + memcpy(p->text, text, sizeof(p->text)); // char[50]:Status text message, without null termination character - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_STATUSTEXT_LEN); } /** @@ -43,15 +43,15 @@ static inline uint16_t mavlink_msg_statustext_pack(uint8_t system_id, uint8_t co * @param text Status text message, without null termination character * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_statustext_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t severity, const int8_t* text) +static inline uint16_t mavlink_msg_statustext_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t severity, const char* text) { - uint16_t i = 0; + mavlink_statustext_t *p = (mavlink_statustext_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_STATUSTEXT; - i += put_uint8_t_by_index(severity, i, msg->payload); // Severity of status, 0 = info message, 255 = critical fault - i += put_array_by_index((const int8_t*)text, sizeof(int8_t)*50, i, msg->payload); // Status text message, without null termination character + p->severity = severity; // uint8_t:Severity of status, 0 = info message, 255 = critical fault + memcpy(p->text, text, sizeof(p->text)); // char[50]:Status text message, without null termination character - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_STATUSTEXT_LEN); } /** @@ -74,13 +74,36 @@ static inline uint16_t mavlink_msg_statustext_encode(uint8_t system_id, uint8_t * @param severity Severity of status, 0 = info message, 255 = critical fault * @param text Status text message, without null termination character */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_statustext_send(mavlink_channel_t chan, uint8_t severity, const int8_t* text) + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS +static inline void mavlink_msg_statustext_send(mavlink_channel_t chan, uint8_t severity, const char* text) { - mavlink_message_t msg; - mavlink_msg_statustext_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, severity, text); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_statustext_t payload; + uint16_t checksum; + mavlink_statustext_t *p = &payload; + + p->severity = severity; // uint8_t:Severity of status, 0 = info message, 255 = critical fault + memcpy(p->text, text, sizeof(p->text)); // char[50]:Status text message, without null termination character + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_STATUSTEXT_LEN; + hdr.msgid = MAVLINK_MSG_ID_STATUSTEXT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -93,7 +116,8 @@ static inline void mavlink_msg_statustext_send(mavlink_channel_t chan, uint8_t s */ static inline uint8_t mavlink_msg_statustext_get_severity(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_statustext_t *p = (mavlink_statustext_t *)&msg->payload[0]; + return (uint8_t)(p->severity); } /** @@ -101,11 +125,12 @@ static inline uint8_t mavlink_msg_statustext_get_severity(const mavlink_message_ * * @return Status text message, without null termination character */ -static inline uint16_t mavlink_msg_statustext_get_text(const mavlink_message_t* msg, int8_t* r_data) +static inline uint16_t mavlink_msg_statustext_get_text(const mavlink_message_t* msg, char* text) { + mavlink_statustext_t *p = (mavlink_statustext_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint8_t), sizeof(int8_t)*50); - return sizeof(int8_t)*50; + memcpy(text, p->text, sizeof(p->text)); + return sizeof(p->text); } /** @@ -116,6 +141,5 @@ static inline uint16_t mavlink_msg_statustext_get_text(const mavlink_message_t* */ static inline void mavlink_msg_statustext_decode(const mavlink_message_t* msg, mavlink_statustext_t* statustext) { - statustext->severity = mavlink_msg_statustext_get_severity(msg); - mavlink_msg_statustext_get_text(msg, statustext->text); + memcpy( statustext, msg->payload, sizeof(mavlink_statustext_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_sys_status.h b/thirdParty/mavlink/include/common/mavlink_msg_sys_status.h index ef83d844813b07c3a0692b704e232894e58e1510..1ca8cec2ecc945f310bb909c5d9021db728c7899 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_sys_status.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_sys_status.h @@ -1,21 +1,21 @@ // MESSAGE SYS_STATUS PACKING #define MAVLINK_MSG_ID_SYS_STATUS 34 +#define MAVLINK_MSG_ID_SYS_STATUS_LEN 11 +#define MAVLINK_MSG_34_LEN 11 typedef struct __mavlink_sys_status_t { - uint8_t mode; ///< System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h - uint8_t nav_mode; ///< Navigation mode, see MAV_NAV_MODE ENUM - uint8_t status; ///< System status flag, see MAV_STATUS ENUM uint16_t load; ///< Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 uint16_t vbat; ///< Battery voltage, in millivolts (1 = 1 millivolt) uint16_t battery_remaining; ///< Remaining battery energy: (0%: 0, 100%: 1000) uint16_t packet_drop; ///< Dropped packets (packets that were corrupted on reception on the MAV) + uint8_t mode; ///< System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h + uint8_t nav_mode; ///< Navigation mode, see MAV_NAV_MODE ENUM + uint8_t status; ///< System status flag, see MAV_STATUS ENUM } mavlink_sys_status_t; - - /** * @brief Pack a sys_status message * @param system_id ID of this system @@ -33,18 +33,18 @@ typedef struct __mavlink_sys_status_t */ static inline uint16_t mavlink_msg_sys_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t mode, uint8_t nav_mode, uint8_t status, uint16_t load, uint16_t vbat, uint16_t battery_remaining, uint16_t packet_drop) { - uint16_t i = 0; + mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SYS_STATUS; - i += put_uint8_t_by_index(mode, i, msg->payload); // System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h - i += put_uint8_t_by_index(nav_mode, i, msg->payload); // Navigation mode, see MAV_NAV_MODE ENUM - i += put_uint8_t_by_index(status, i, msg->payload); // System status flag, see MAV_STATUS ENUM - i += put_uint16_t_by_index(load, i, msg->payload); // Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - i += put_uint16_t_by_index(vbat, i, msg->payload); // Battery voltage, in millivolts (1 = 1 millivolt) - i += put_uint16_t_by_index(battery_remaining, i, msg->payload); // Remaining battery energy: (0%: 0, 100%: 1000) - i += put_uint16_t_by_index(packet_drop, i, msg->payload); // Dropped packets (packets that were corrupted on reception on the MAV) + p->mode = mode; // uint8_t:System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h + p->nav_mode = nav_mode; // uint8_t:Navigation mode, see MAV_NAV_MODE ENUM + p->status = status; // uint8_t:System status flag, see MAV_STATUS ENUM + p->load = load; // uint16_t:Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + p->vbat = vbat; // uint16_t:Battery voltage, in millivolts (1 = 1 millivolt) + p->battery_remaining = battery_remaining; // uint16_t:Remaining battery energy: (0%: 0, 100%: 1000) + p->packet_drop = packet_drop; // uint16_t:Dropped packets (packets that were corrupted on reception on the MAV) - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SYS_STATUS_LEN); } /** @@ -64,18 +64,18 @@ static inline uint16_t mavlink_msg_sys_status_pack(uint8_t system_id, uint8_t co */ static inline uint16_t mavlink_msg_sys_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t mode, uint8_t nav_mode, uint8_t status, uint16_t load, uint16_t vbat, uint16_t battery_remaining, uint16_t packet_drop) { - uint16_t i = 0; + mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SYS_STATUS; - i += put_uint8_t_by_index(mode, i, msg->payload); // System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h - i += put_uint8_t_by_index(nav_mode, i, msg->payload); // Navigation mode, see MAV_NAV_MODE ENUM - i += put_uint8_t_by_index(status, i, msg->payload); // System status flag, see MAV_STATUS ENUM - i += put_uint16_t_by_index(load, i, msg->payload); // Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - i += put_uint16_t_by_index(vbat, i, msg->payload); // Battery voltage, in millivolts (1 = 1 millivolt) - i += put_uint16_t_by_index(battery_remaining, i, msg->payload); // Remaining battery energy: (0%: 0, 100%: 1000) - i += put_uint16_t_by_index(packet_drop, i, msg->payload); // Dropped packets (packets that were corrupted on reception on the MAV) + p->mode = mode; // uint8_t:System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h + p->nav_mode = nav_mode; // uint8_t:Navigation mode, see MAV_NAV_MODE ENUM + p->status = status; // uint8_t:System status flag, see MAV_STATUS ENUM + p->load = load; // uint16_t:Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + p->vbat = vbat; // uint16_t:Battery voltage, in millivolts (1 = 1 millivolt) + p->battery_remaining = battery_remaining; // uint16_t:Remaining battery energy: (0%: 0, 100%: 1000) + p->packet_drop = packet_drop; // uint16_t:Dropped packets (packets that were corrupted on reception on the MAV) - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SYS_STATUS_LEN); } /** @@ -103,13 +103,41 @@ static inline uint16_t mavlink_msg_sys_status_encode(uint8_t system_id, uint8_t * @param battery_remaining Remaining battery energy: (0%: 0, 100%: 1000) * @param packet_drop Dropped packets (packets that were corrupted on reception on the MAV) */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_sys_status_send(mavlink_channel_t chan, uint8_t mode, uint8_t nav_mode, uint8_t status, uint16_t load, uint16_t vbat, uint16_t battery_remaining, uint16_t packet_drop) { - mavlink_message_t msg; - mavlink_msg_sys_status_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, mode, nav_mode, status, load, vbat, battery_remaining, packet_drop); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_sys_status_t payload; + uint16_t checksum; + mavlink_sys_status_t *p = &payload; + + p->mode = mode; // uint8_t:System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h + p->nav_mode = nav_mode; // uint8_t:Navigation mode, see MAV_NAV_MODE ENUM + p->status = status; // uint8_t:System status flag, see MAV_STATUS ENUM + p->load = load; // uint16_t:Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + p->vbat = vbat; // uint16_t:Battery voltage, in millivolts (1 = 1 millivolt) + p->battery_remaining = battery_remaining; // uint16_t:Remaining battery energy: (0%: 0, 100%: 1000) + p->packet_drop = packet_drop; // uint16_t:Dropped packets (packets that were corrupted on reception on the MAV) + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_SYS_STATUS_LEN; + hdr.msgid = MAVLINK_MSG_ID_SYS_STATUS; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -122,7 +150,8 @@ static inline void mavlink_msg_sys_status_send(mavlink_channel_t chan, uint8_t m */ static inline uint8_t mavlink_msg_sys_status_get_mode(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg->payload[0]; + return (uint8_t)(p->mode); } /** @@ -132,7 +161,8 @@ static inline uint8_t mavlink_msg_sys_status_get_mode(const mavlink_message_t* m */ static inline uint8_t mavlink_msg_sys_status_get_nav_mode(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg->payload[0]; + return (uint8_t)(p->nav_mode); } /** @@ -142,7 +172,8 @@ static inline uint8_t mavlink_msg_sys_status_get_nav_mode(const mavlink_message_ */ static inline uint8_t mavlink_msg_sys_status_get_status(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg->payload[0]; + return (uint8_t)(p->status); } /** @@ -152,10 +183,8 @@ static inline uint8_t mavlink_msg_sys_status_get_status(const mavlink_message_t* */ static inline uint16_t mavlink_msg_sys_status_get_load(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[1]; - return (uint16_t)r.s; + mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg->payload[0]; + return (uint16_t)(p->load); } /** @@ -165,10 +194,8 @@ static inline uint16_t mavlink_msg_sys_status_get_load(const mavlink_message_t* */ static inline uint16_t mavlink_msg_sys_status_get_vbat(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg->payload[0]; + return (uint16_t)(p->vbat); } /** @@ -178,10 +205,8 @@ static inline uint16_t mavlink_msg_sys_status_get_vbat(const mavlink_message_t* */ static inline uint16_t mavlink_msg_sys_status_get_battery_remaining(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+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(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg->payload[0]; + return (uint16_t)(p->battery_remaining); } /** @@ -191,10 +216,8 @@ static inline uint16_t mavlink_msg_sys_status_get_battery_remaining(const mavlin */ static inline uint16_t mavlink_msg_sys_status_get_packet_drop(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+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(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg->payload[0]; + return (uint16_t)(p->packet_drop); } /** @@ -205,11 +228,5 @@ static inline uint16_t mavlink_msg_sys_status_get_packet_drop(const mavlink_mess */ static inline void mavlink_msg_sys_status_decode(const mavlink_message_t* msg, mavlink_sys_status_t* sys_status) { - sys_status->mode = mavlink_msg_sys_status_get_mode(msg); - sys_status->nav_mode = mavlink_msg_sys_status_get_nav_mode(msg); - sys_status->status = mavlink_msg_sys_status_get_status(msg); - sys_status->load = mavlink_msg_sys_status_get_load(msg); - sys_status->vbat = mavlink_msg_sys_status_get_vbat(msg); - sys_status->battery_remaining = mavlink_msg_sys_status_get_battery_remaining(msg); - sys_status->packet_drop = mavlink_msg_sys_status_get_packet_drop(msg); + memcpy( sys_status, msg->payload, sizeof(mavlink_sys_status_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_system_time.h b/thirdParty/mavlink/include/common/mavlink_msg_system_time.h index d293f025bfc5937c253e1e78747c61720b88f436..c2328a68fc7d633cd2e219c5f4daab27263215b5 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_system_time.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_system_time.h @@ -1,6 +1,8 @@ // MESSAGE SYSTEM_TIME PACKING #define MAVLINK_MSG_ID_SYSTEM_TIME 2 +#define MAVLINK_MSG_ID_SYSTEM_TIME_LEN 8 +#define MAVLINK_MSG_2_LEN 8 typedef struct __mavlink_system_time_t { @@ -8,8 +10,6 @@ typedef struct __mavlink_system_time_t } mavlink_system_time_t; - - /** * @brief Pack a system_time message * @param system_id ID of this system @@ -21,12 +21,12 @@ typedef struct __mavlink_system_time_t */ static inline uint16_t mavlink_msg_system_time_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t time_usec) { - uint16_t i = 0; + mavlink_system_time_t *p = (mavlink_system_time_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SYSTEM_TIME; - i += put_uint64_t_by_index(time_usec, i, msg->payload); // Timestamp of the master clock in microseconds since UNIX epoch. + p->time_usec = time_usec; // uint64_t:Timestamp of the master clock in microseconds since UNIX epoch. - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SYSTEM_TIME_LEN); } /** @@ -40,12 +40,12 @@ static inline uint16_t mavlink_msg_system_time_pack(uint8_t system_id, uint8_t c */ static inline uint16_t mavlink_msg_system_time_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t time_usec) { - uint16_t i = 0; + mavlink_system_time_t *p = (mavlink_system_time_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SYSTEM_TIME; - i += put_uint64_t_by_index(time_usec, i, msg->payload); // Timestamp of the master clock in microseconds since UNIX epoch. + p->time_usec = time_usec; // uint64_t:Timestamp of the master clock in microseconds since UNIX epoch. - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SYSTEM_TIME_LEN); } /** @@ -67,13 +67,35 @@ static inline uint16_t mavlink_msg_system_time_encode(uint8_t system_id, uint8_t * * @param time_usec Timestamp of the master clock in microseconds since UNIX epoch. */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_system_time_send(mavlink_channel_t chan, uint64_t time_usec) { - mavlink_message_t msg; - mavlink_msg_system_time_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, time_usec); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_system_time_t payload; + uint16_t checksum; + mavlink_system_time_t *p = &payload; + + p->time_usec = time_usec; // uint64_t:Timestamp of the master clock in microseconds since UNIX epoch. + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_SYSTEM_TIME_LEN; + hdr.msgid = MAVLINK_MSG_ID_SYSTEM_TIME; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -86,16 +108,8 @@ static inline void mavlink_msg_system_time_send(mavlink_channel_t chan, uint64_t */ static inline uint64_t mavlink_msg_system_time_get_time_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; + mavlink_system_time_t *p = (mavlink_system_time_t *)&msg->payload[0]; + return (uint64_t)(p->time_usec); } /** @@ -106,5 +120,5 @@ static inline uint64_t mavlink_msg_system_time_get_time_usec(const mavlink_messa */ static inline void mavlink_msg_system_time_decode(const mavlink_message_t* msg, mavlink_system_time_t* system_time) { - system_time->time_usec = mavlink_msg_system_time_get_time_usec(msg); + memcpy( system_time, msg->payload, sizeof(mavlink_system_time_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_system_time_utc.h b/thirdParty/mavlink/include/common/mavlink_msg_system_time_utc.h index 75c25f1ced937bd7d13bfdfa91f07bf7601200f5..414b4984f0b9f14e0a211714f98496aae8815f7f 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_system_time_utc.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_system_time_utc.h @@ -1,6 +1,8 @@ // MESSAGE SYSTEM_TIME_UTC PACKING #define MAVLINK_MSG_ID_SYSTEM_TIME_UTC 4 +#define MAVLINK_MSG_ID_SYSTEM_TIME_UTC_LEN 8 +#define MAVLINK_MSG_4_LEN 8 typedef struct __mavlink_system_time_utc_t { @@ -9,8 +11,6 @@ typedef struct __mavlink_system_time_utc_t } mavlink_system_time_utc_t; - - /** * @brief Pack a system_time_utc message * @param system_id ID of this system @@ -23,13 +23,13 @@ typedef struct __mavlink_system_time_utc_t */ static inline uint16_t mavlink_msg_system_time_utc_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint32_t utc_date, uint32_t utc_time) { - uint16_t i = 0; + mavlink_system_time_utc_t *p = (mavlink_system_time_utc_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SYSTEM_TIME_UTC; - i += put_uint32_t_by_index(utc_date, i, msg->payload); // GPS UTC date ddmmyy - i += put_uint32_t_by_index(utc_time, i, msg->payload); // GPS UTC time hhmmss + p->utc_date = utc_date; // uint32_t:GPS UTC date ddmmyy + p->utc_time = utc_time; // uint32_t:GPS UTC time hhmmss - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SYSTEM_TIME_UTC_LEN); } /** @@ -44,13 +44,13 @@ static inline uint16_t mavlink_msg_system_time_utc_pack(uint8_t system_id, uint8 */ static inline uint16_t mavlink_msg_system_time_utc_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint32_t utc_date, uint32_t utc_time) { - uint16_t i = 0; + mavlink_system_time_utc_t *p = (mavlink_system_time_utc_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SYSTEM_TIME_UTC; - i += put_uint32_t_by_index(utc_date, i, msg->payload); // GPS UTC date ddmmyy - i += put_uint32_t_by_index(utc_time, i, msg->payload); // GPS UTC time hhmmss + p->utc_date = utc_date; // uint32_t:GPS UTC date ddmmyy + p->utc_time = utc_time; // uint32_t:GPS UTC time hhmmss - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SYSTEM_TIME_UTC_LEN); } /** @@ -73,13 +73,36 @@ static inline uint16_t mavlink_msg_system_time_utc_encode(uint8_t system_id, uin * @param utc_date GPS UTC date ddmmyy * @param utc_time GPS UTC time hhmmss */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_system_time_utc_send(mavlink_channel_t chan, uint32_t utc_date, uint32_t utc_time) { - mavlink_message_t msg; - mavlink_msg_system_time_utc_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, utc_date, utc_time); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_system_time_utc_t payload; + uint16_t checksum; + mavlink_system_time_utc_t *p = &payload; + + p->utc_date = utc_date; // uint32_t:GPS UTC date ddmmyy + p->utc_time = utc_time; // uint32_t:GPS UTC time hhmmss + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_SYSTEM_TIME_UTC_LEN; + hdr.msgid = MAVLINK_MSG_ID_SYSTEM_TIME_UTC; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -92,12 +115,8 @@ static inline void mavlink_msg_system_time_utc_send(mavlink_channel_t chan, uint */ static inline uint32_t mavlink_msg_system_time_utc_get_utc_date(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; + mavlink_system_time_utc_t *p = (mavlink_system_time_utc_t *)&msg->payload[0]; + return (uint32_t)(p->utc_date); } /** @@ -107,12 +126,8 @@ static inline uint32_t mavlink_msg_system_time_utc_get_utc_date(const mavlink_me */ static inline uint32_t mavlink_msg_system_time_utc_get_utc_time(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint32_t))[0]; - r.b[2] = (msg->payload+sizeof(uint32_t))[1]; - r.b[1] = (msg->payload+sizeof(uint32_t))[2]; - r.b[0] = (msg->payload+sizeof(uint32_t))[3]; - return (uint32_t)r.i; + mavlink_system_time_utc_t *p = (mavlink_system_time_utc_t *)&msg->payload[0]; + return (uint32_t)(p->utc_time); } /** @@ -123,6 +138,5 @@ static inline uint32_t mavlink_msg_system_time_utc_get_utc_time(const mavlink_me */ static inline void mavlink_msg_system_time_utc_decode(const mavlink_message_t* msg, mavlink_system_time_utc_t* system_time_utc) { - system_time_utc->utc_date = mavlink_msg_system_time_utc_get_utc_date(msg); - system_time_utc->utc_time = mavlink_msg_system_time_utc_get_utc_time(msg); + memcpy( system_time_utc, msg->payload, sizeof(mavlink_system_time_utc_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_vfr_hud.h b/thirdParty/mavlink/include/common/mavlink_msg_vfr_hud.h index 9f55bdfa70f7b3f754db24eda1c73f720e52daa3..0b757cef6688b264992255588a6f4376e0c85fcb 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_vfr_hud.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_vfr_hud.h @@ -1,20 +1,20 @@ // MESSAGE VFR_HUD PACKING #define MAVLINK_MSG_ID_VFR_HUD 74 +#define MAVLINK_MSG_ID_VFR_HUD_LEN 20 +#define MAVLINK_MSG_74_LEN 20 typedef struct __mavlink_vfr_hud_t { float airspeed; ///< Current airspeed in m/s float groundspeed; ///< Current ground speed in m/s - int16_t heading; ///< Current heading in degrees, in compass units (0..360, 0=north) - uint16_t throttle; ///< Current throttle setting in integer percent, 0 to 100 float alt; ///< Current altitude (MSL), in meters float climb; ///< Current climb rate in meters/second + int16_t heading; ///< Current heading in degrees, in compass units (0..360, 0=north) + uint16_t throttle; ///< Current throttle setting in integer percent, 0 to 100 } mavlink_vfr_hud_t; - - /** * @brief Pack a vfr_hud message * @param system_id ID of this system @@ -31,17 +31,17 @@ typedef struct __mavlink_vfr_hud_t */ static inline uint16_t mavlink_msg_vfr_hud_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, float airspeed, float groundspeed, int16_t heading, uint16_t throttle, float alt, float climb) { - uint16_t i = 0; + mavlink_vfr_hud_t *p = (mavlink_vfr_hud_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_VFR_HUD; - i += put_float_by_index(airspeed, i, msg->payload); // Current airspeed in m/s - i += put_float_by_index(groundspeed, i, msg->payload); // Current ground speed in m/s - i += put_int16_t_by_index(heading, i, msg->payload); // Current heading in degrees, in compass units (0..360, 0=north) - i += put_uint16_t_by_index(throttle, i, msg->payload); // Current throttle setting in integer percent, 0 to 100 - i += put_float_by_index(alt, i, msg->payload); // Current altitude (MSL), in meters - i += put_float_by_index(climb, i, msg->payload); // Current climb rate in meters/second + p->airspeed = airspeed; // float:Current airspeed in m/s + p->groundspeed = groundspeed; // float:Current ground speed in m/s + p->heading = heading; // int16_t:Current heading in degrees, in compass units (0..360, 0=north) + p->throttle = throttle; // uint16_t:Current throttle setting in integer percent, 0 to 100 + p->alt = alt; // float:Current altitude (MSL), in meters + p->climb = climb; // float:Current climb rate in meters/second - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VFR_HUD_LEN); } /** @@ -60,17 +60,17 @@ static inline uint16_t mavlink_msg_vfr_hud_pack(uint8_t system_id, uint8_t compo */ static inline uint16_t mavlink_msg_vfr_hud_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, float airspeed, float groundspeed, int16_t heading, uint16_t throttle, float alt, float climb) { - uint16_t i = 0; + mavlink_vfr_hud_t *p = (mavlink_vfr_hud_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_VFR_HUD; - i += put_float_by_index(airspeed, i, msg->payload); // Current airspeed in m/s - i += put_float_by_index(groundspeed, i, msg->payload); // Current ground speed in m/s - i += put_int16_t_by_index(heading, i, msg->payload); // Current heading in degrees, in compass units (0..360, 0=north) - i += put_uint16_t_by_index(throttle, i, msg->payload); // Current throttle setting in integer percent, 0 to 100 - i += put_float_by_index(alt, i, msg->payload); // Current altitude (MSL), in meters - i += put_float_by_index(climb, i, msg->payload); // Current climb rate in meters/second + p->airspeed = airspeed; // float:Current airspeed in m/s + p->groundspeed = groundspeed; // float:Current ground speed in m/s + p->heading = heading; // int16_t:Current heading in degrees, in compass units (0..360, 0=north) + p->throttle = throttle; // uint16_t:Current throttle setting in integer percent, 0 to 100 + p->alt = alt; // float:Current altitude (MSL), in meters + p->climb = climb; // float:Current climb rate in meters/second - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_VFR_HUD_LEN); } /** @@ -97,13 +97,40 @@ static inline uint16_t mavlink_msg_vfr_hud_encode(uint8_t system_id, uint8_t com * @param alt Current altitude (MSL), in meters * @param climb Current climb rate in meters/second */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_vfr_hud_send(mavlink_channel_t chan, float airspeed, float groundspeed, int16_t heading, uint16_t throttle, float alt, float climb) { - mavlink_message_t msg; - mavlink_msg_vfr_hud_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, airspeed, groundspeed, heading, throttle, alt, climb); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_vfr_hud_t payload; + uint16_t checksum; + mavlink_vfr_hud_t *p = &payload; + + p->airspeed = airspeed; // float:Current airspeed in m/s + p->groundspeed = groundspeed; // float:Current ground speed in m/s + p->heading = heading; // int16_t:Current heading in degrees, in compass units (0..360, 0=north) + p->throttle = throttle; // uint16_t:Current throttle setting in integer percent, 0 to 100 + p->alt = alt; // float:Current altitude (MSL), in meters + p->climb = climb; // float:Current climb rate in meters/second + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_VFR_HUD_LEN; + hdr.msgid = MAVLINK_MSG_ID_VFR_HUD; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -116,12 +143,8 @@ static inline void mavlink_msg_vfr_hud_send(mavlink_channel_t chan, float airspe */ static inline float mavlink_msg_vfr_hud_get_airspeed(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 (float)r.f; + mavlink_vfr_hud_t *p = (mavlink_vfr_hud_t *)&msg->payload[0]; + return (float)(p->airspeed); } /** @@ -131,12 +154,8 @@ static inline float mavlink_msg_vfr_hud_get_airspeed(const mavlink_message_t* ms */ static inline float mavlink_msg_vfr_hud_get_groundspeed(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float))[3]; - return (float)r.f; + mavlink_vfr_hud_t *p = (mavlink_vfr_hud_t *)&msg->payload[0]; + return (float)(p->groundspeed); } /** @@ -146,10 +165,8 @@ static inline float mavlink_msg_vfr_hud_get_groundspeed(const mavlink_message_t* */ static inline int16_t mavlink_msg_vfr_hud_get_heading(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float))[0]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float))[1]; - return (int16_t)r.s; + mavlink_vfr_hud_t *p = (mavlink_vfr_hud_t *)&msg->payload[0]; + return (int16_t)(p->heading); } /** @@ -159,10 +176,8 @@ static inline int16_t mavlink_msg_vfr_hud_get_heading(const mavlink_message_t* m */ static inline uint16_t mavlink_msg_vfr_hud_get_throttle(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t))[1]; - return (uint16_t)r.s; + mavlink_vfr_hud_t *p = (mavlink_vfr_hud_t *)&msg->payload[0]; + return (uint16_t)(p->throttle); } /** @@ -172,12 +187,8 @@ static inline uint16_t mavlink_msg_vfr_hud_get_throttle(const mavlink_message_t* */ static inline float mavlink_msg_vfr_hud_get_alt(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(uint16_t))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(uint16_t))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(uint16_t))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(uint16_t))[3]; - return (float)r.f; + mavlink_vfr_hud_t *p = (mavlink_vfr_hud_t *)&msg->payload[0]; + return (float)(p->alt); } /** @@ -187,12 +198,8 @@ static inline float mavlink_msg_vfr_hud_get_alt(const mavlink_message_t* msg) */ static inline float mavlink_msg_vfr_hud_get_climb(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(uint16_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(uint16_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(uint16_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(uint16_t)+sizeof(float))[3]; - return (float)r.f; + mavlink_vfr_hud_t *p = (mavlink_vfr_hud_t *)&msg->payload[0]; + return (float)(p->climb); } /** @@ -203,10 +210,5 @@ static inline float mavlink_msg_vfr_hud_get_climb(const mavlink_message_t* msg) */ static inline void mavlink_msg_vfr_hud_decode(const mavlink_message_t* msg, mavlink_vfr_hud_t* vfr_hud) { - vfr_hud->airspeed = mavlink_msg_vfr_hud_get_airspeed(msg); - vfr_hud->groundspeed = mavlink_msg_vfr_hud_get_groundspeed(msg); - vfr_hud->heading = mavlink_msg_vfr_hud_get_heading(msg); - vfr_hud->throttle = mavlink_msg_vfr_hud_get_throttle(msg); - vfr_hud->alt = mavlink_msg_vfr_hud_get_alt(msg); - vfr_hud->climb = mavlink_msg_vfr_hud_get_climb(msg); + memcpy( vfr_hud, msg->payload, sizeof(mavlink_vfr_hud_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint.h index ab1c5f836a0b44b4e7139c486eec860dc6b6c90f..a29a26b0ecdf0cdfcacc237f5f2397ad532070e4 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint.h @@ -1,16 +1,11 @@ // MESSAGE WAYPOINT PACKING #define MAVLINK_MSG_ID_WAYPOINT 39 +#define MAVLINK_MSG_ID_WAYPOINT_LEN 36 +#define MAVLINK_MSG_39_LEN 36 typedef struct __mavlink_waypoint_t { - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - uint16_t seq; ///< Sequence - uint8_t frame; ///< The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h - uint8_t command; ///< The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs - uint8_t current; ///< false:0, true:1 - uint8_t autocontinue; ///< autocontinue to next wp float param1; ///< PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters float param2; ///< PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds float param3; ///< 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. @@ -18,11 +13,16 @@ typedef struct __mavlink_waypoint_t float x; ///< PARAM5 / local: x position, global: latitude float y; ///< PARAM6 / y position: global: longitude float z; ///< PARAM7 / z position: global: altitude + uint16_t seq; ///< Sequence + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + uint8_t frame; ///< The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h + uint8_t command; ///< The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs + uint8_t current; ///< false:0, true:1 + uint8_t autocontinue; ///< autocontinue to next wp } mavlink_waypoint_t; - - /** * @brief Pack a waypoint message * @param system_id ID of this system @@ -47,25 +47,25 @@ typedef struct __mavlink_waypoint_t */ static inline uint16_t mavlink_msg_waypoint_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint8_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, float x, float y, float z) { - uint16_t i = 0; + mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT; - 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(seq, i, msg->payload); // Sequence - i += put_uint8_t_by_index(frame, i, msg->payload); // The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h - i += put_uint8_t_by_index(command, i, msg->payload); // The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs - i += put_uint8_t_by_index(current, i, msg->payload); // false:0, true:1 - i += put_uint8_t_by_index(autocontinue, i, msg->payload); // autocontinue to next wp - i += put_float_by_index(param1, i, msg->payload); // PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters - i += put_float_by_index(param2, i, msg->payload); // PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds - i += put_float_by_index(param3, i, msg->payload); // 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. - i += put_float_by_index(param4, i, msg->payload); // PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH - i += put_float_by_index(x, i, msg->payload); // PARAM5 / local: x position, global: latitude - i += put_float_by_index(y, i, msg->payload); // PARAM6 / y position: global: longitude - i += put_float_by_index(z, i, msg->payload); // PARAM7 / z position: global: altitude - - return mavlink_finalize_message(msg, system_id, component_id, i); + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->seq = seq; // uint16_t:Sequence + p->frame = frame; // uint8_t:The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h + p->command = command; // uint8_t:The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs + p->current = current; // uint8_t:false:0, true:1 + p->autocontinue = autocontinue; // uint8_t:autocontinue to next wp + p->param1 = param1; // float:PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters + p->param2 = param2; // float:PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds + p->param3 = param3; // float: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. + p->param4 = param4; // float:PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH + p->x = x; // float:PARAM5 / local: x position, global: latitude + p->y = y; // float:PARAM6 / y position: global: longitude + p->z = z; // float:PARAM7 / z position: global: altitude + + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WAYPOINT_LEN); } /** @@ -92,25 +92,25 @@ static inline uint16_t mavlink_msg_waypoint_pack(uint8_t system_id, uint8_t comp */ static inline uint16_t mavlink_msg_waypoint_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 seq, uint8_t frame, uint8_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, float x, float y, float z) { - uint16_t i = 0; + mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT; - 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(seq, i, msg->payload); // Sequence - i += put_uint8_t_by_index(frame, i, msg->payload); // The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h - i += put_uint8_t_by_index(command, i, msg->payload); // The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs - i += put_uint8_t_by_index(current, i, msg->payload); // false:0, true:1 - i += put_uint8_t_by_index(autocontinue, i, msg->payload); // autocontinue to next wp - i += put_float_by_index(param1, i, msg->payload); // PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters - i += put_float_by_index(param2, i, msg->payload); // PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds - i += put_float_by_index(param3, i, msg->payload); // 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. - i += put_float_by_index(param4, i, msg->payload); // PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH - i += put_float_by_index(x, i, msg->payload); // PARAM5 / local: x position, global: latitude - i += put_float_by_index(y, i, msg->payload); // PARAM6 / y position: global: longitude - i += put_float_by_index(z, i, msg->payload); // PARAM7 / z position: global: altitude - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->seq = seq; // uint16_t:Sequence + p->frame = frame; // uint8_t:The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h + p->command = command; // uint8_t:The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs + p->current = current; // uint8_t:false:0, true:1 + p->autocontinue = autocontinue; // uint8_t:autocontinue to next wp + p->param1 = param1; // float:PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters + p->param2 = param2; // float:PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds + p->param3 = param3; // float: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. + p->param4 = param4; // float:PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH + p->x = x; // float:PARAM5 / local: x position, global: latitude + p->y = y; // float:PARAM6 / y position: global: longitude + p->z = z; // float:PARAM7 / z position: global: altitude + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WAYPOINT_LEN); } /** @@ -145,13 +145,48 @@ static inline uint16_t mavlink_msg_waypoint_encode(uint8_t system_id, uint8_t co * @param y PARAM6 / y position: global: longitude * @param z PARAM7 / z position: global: altitude */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_waypoint_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint8_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, float x, float y, float z) { - mavlink_message_t msg; - mavlink_msg_waypoint_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_waypoint_t payload; + uint16_t checksum; + mavlink_waypoint_t *p = &payload; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->seq = seq; // uint16_t:Sequence + p->frame = frame; // uint8_t:The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h + p->command = command; // uint8_t:The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs + p->current = current; // uint8_t:false:0, true:1 + p->autocontinue = autocontinue; // uint8_t:autocontinue to next wp + p->param1 = param1; // float:PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters + p->param2 = param2; // float:PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds + p->param3 = param3; // float: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. + p->param4 = param4; // float:PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH + p->x = x; // float:PARAM5 / local: x position, global: latitude + p->y = y; // float:PARAM6 / y position: global: longitude + p->z = z; // float:PARAM7 / z position: global: altitude + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_WAYPOINT_LEN; + hdr.msgid = MAVLINK_MSG_ID_WAYPOINT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -164,7 +199,8 @@ static inline void mavlink_msg_waypoint_send(mavlink_channel_t chan, uint8_t tar */ static inline uint8_t mavlink_msg_waypoint_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); } /** @@ -174,7 +210,8 @@ static inline uint8_t mavlink_msg_waypoint_get_target_system(const mavlink_messa */ static inline uint8_t mavlink_msg_waypoint_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); } /** @@ -184,10 +221,8 @@ static inline uint8_t mavlink_msg_waypoint_get_target_component(const mavlink_me */ static inline uint16_t mavlink_msg_waypoint_get_seq(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; + mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; + return (uint16_t)(p->seq); } /** @@ -197,7 +232,8 @@ static inline uint16_t mavlink_msg_waypoint_get_seq(const mavlink_message_t* msg */ static inline uint8_t mavlink_msg_waypoint_get_frame(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[0]; + mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; + return (uint8_t)(p->frame); } /** @@ -207,7 +243,8 @@ static inline uint8_t mavlink_msg_waypoint_get_frame(const mavlink_message_t* ms */ static inline uint8_t mavlink_msg_waypoint_get_command(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t))[0]; + mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; + return (uint8_t)(p->command); } /** @@ -217,7 +254,8 @@ static inline uint8_t mavlink_msg_waypoint_get_command(const mavlink_message_t* */ static inline uint8_t mavlink_msg_waypoint_get_current(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; + return (uint8_t)(p->current); } /** @@ -227,7 +265,8 @@ static inline uint8_t mavlink_msg_waypoint_get_current(const mavlink_message_t* */ static inline uint8_t mavlink_msg_waypoint_get_autocontinue(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; + return (uint8_t)(p->autocontinue); } /** @@ -237,12 +276,8 @@ static inline uint8_t mavlink_msg_waypoint_get_autocontinue(const mavlink_messag */ static inline float mavlink_msg_waypoint_get_param1(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[3]; - return (float)r.f; + mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; + return (float)(p->param1); } /** @@ -252,12 +287,8 @@ static inline float mavlink_msg_waypoint_get_param1(const mavlink_message_t* msg */ static inline float mavlink_msg_waypoint_get_param2(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[3]; - return (float)r.f; + mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; + return (float)(p->param2); } /** @@ -267,12 +298,8 @@ static inline float mavlink_msg_waypoint_get_param2(const mavlink_message_t* msg */ static inline float mavlink_msg_waypoint_get_param3(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; + return (float)(p->param3); } /** @@ -282,12 +309,8 @@ static inline float mavlink_msg_waypoint_get_param3(const mavlink_message_t* msg */ static inline float mavlink_msg_waypoint_get_param4(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+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(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+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(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+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(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; + return (float)(p->param4); } /** @@ -297,12 +320,8 @@ static inline float mavlink_msg_waypoint_get_param4(const mavlink_message_t* msg */ static inline float mavlink_msg_waypoint_get_x(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; + return (float)(p->x); } /** @@ -312,12 +331,8 @@ static inline float mavlink_msg_waypoint_get_x(const mavlink_message_t* msg) */ static inline float mavlink_msg_waypoint_get_y(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; + return (float)(p->y); } /** @@ -327,12 +342,8 @@ static inline float mavlink_msg_waypoint_get_y(const mavlink_message_t* msg) */ static inline float mavlink_msg_waypoint_get_z(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; + return (float)(p->z); } /** @@ -343,18 +354,5 @@ static inline float mavlink_msg_waypoint_get_z(const mavlink_message_t* msg) */ static inline void mavlink_msg_waypoint_decode(const mavlink_message_t* msg, mavlink_waypoint_t* waypoint) { - waypoint->target_system = mavlink_msg_waypoint_get_target_system(msg); - waypoint->target_component = mavlink_msg_waypoint_get_target_component(msg); - waypoint->seq = mavlink_msg_waypoint_get_seq(msg); - waypoint->frame = mavlink_msg_waypoint_get_frame(msg); - waypoint->command = mavlink_msg_waypoint_get_command(msg); - waypoint->current = mavlink_msg_waypoint_get_current(msg); - waypoint->autocontinue = mavlink_msg_waypoint_get_autocontinue(msg); - waypoint->param1 = mavlink_msg_waypoint_get_param1(msg); - waypoint->param2 = mavlink_msg_waypoint_get_param2(msg); - waypoint->param3 = mavlink_msg_waypoint_get_param3(msg); - waypoint->param4 = mavlink_msg_waypoint_get_param4(msg); - waypoint->x = mavlink_msg_waypoint_get_x(msg); - waypoint->y = mavlink_msg_waypoint_get_y(msg); - waypoint->z = mavlink_msg_waypoint_get_z(msg); + memcpy( waypoint, msg->payload, sizeof(mavlink_waypoint_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_ack.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_ack.h index d994eaf49a76a8a6b226311052dbf8451c9e4493..8a5d52ed38d03fb569ef6fd9f9426ae23f0df1fb 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_ack.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_ack.h @@ -1,6 +1,8 @@ // MESSAGE WAYPOINT_ACK PACKING #define MAVLINK_MSG_ID_WAYPOINT_ACK 47 +#define MAVLINK_MSG_ID_WAYPOINT_ACK_LEN 3 +#define MAVLINK_MSG_47_LEN 3 typedef struct __mavlink_waypoint_ack_t { @@ -10,8 +12,6 @@ typedef struct __mavlink_waypoint_ack_t } mavlink_waypoint_ack_t; - - /** * @brief Pack a waypoint_ack message * @param system_id ID of this system @@ -25,14 +25,14 @@ typedef struct __mavlink_waypoint_ack_t */ static inline uint16_t mavlink_msg_waypoint_ack_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint8_t type) { - uint16_t i = 0; + mavlink_waypoint_ack_t *p = (mavlink_waypoint_ack_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_ACK; - 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_uint8_t_by_index(type, i, msg->payload); // 0: OK, 1: Error + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->type = type; // uint8_t:0: OK, 1: Error - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WAYPOINT_ACK_LEN); } /** @@ -48,14 +48,14 @@ static inline uint16_t mavlink_msg_waypoint_ack_pack(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_waypoint_ack_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, uint8_t type) { - uint16_t i = 0; + mavlink_waypoint_ack_t *p = (mavlink_waypoint_ack_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_ACK; - 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_uint8_t_by_index(type, i, msg->payload); // 0: OK, 1: Error + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->type = type; // uint8_t:0: OK, 1: Error - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WAYPOINT_ACK_LEN); } /** @@ -79,13 +79,37 @@ static inline uint16_t mavlink_msg_waypoint_ack_encode(uint8_t system_id, uint8_ * @param target_component Component ID * @param type 0: OK, 1: Error */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_waypoint_ack_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t type) { - mavlink_message_t msg; - mavlink_msg_waypoint_ack_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, type); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_waypoint_ack_t payload; + uint16_t checksum; + mavlink_waypoint_ack_t *p = &payload; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->type = type; // uint8_t:0: OK, 1: Error + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_WAYPOINT_ACK_LEN; + hdr.msgid = MAVLINK_MSG_ID_WAYPOINT_ACK; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -98,7 +122,8 @@ static inline void mavlink_msg_waypoint_ack_send(mavlink_channel_t chan, uint8_t */ static inline uint8_t mavlink_msg_waypoint_ack_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_waypoint_ack_t *p = (mavlink_waypoint_ack_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); } /** @@ -108,7 +133,8 @@ static inline uint8_t mavlink_msg_waypoint_ack_get_target_system(const mavlink_m */ static inline uint8_t mavlink_msg_waypoint_ack_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_waypoint_ack_t *p = (mavlink_waypoint_ack_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); } /** @@ -118,7 +144,8 @@ static inline uint8_t mavlink_msg_waypoint_ack_get_target_component(const mavlin */ static inline uint8_t mavlink_msg_waypoint_ack_get_type(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_waypoint_ack_t *p = (mavlink_waypoint_ack_t *)&msg->payload[0]; + return (uint8_t)(p->type); } /** @@ -129,7 +156,5 @@ static inline uint8_t mavlink_msg_waypoint_ack_get_type(const mavlink_message_t* */ static inline void mavlink_msg_waypoint_ack_decode(const mavlink_message_t* msg, mavlink_waypoint_ack_t* waypoint_ack) { - waypoint_ack->target_system = mavlink_msg_waypoint_ack_get_target_system(msg); - waypoint_ack->target_component = mavlink_msg_waypoint_ack_get_target_component(msg); - waypoint_ack->type = mavlink_msg_waypoint_ack_get_type(msg); + memcpy( waypoint_ack, msg->payload, sizeof(mavlink_waypoint_ack_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_clear_all.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_clear_all.h index 5020fbada8671223344d1fd9056b41259f14f747..b96a80696a4dc98ad8436dfdeacc01f21a2feb5e 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_clear_all.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_clear_all.h @@ -1,6 +1,8 @@ // MESSAGE WAYPOINT_CLEAR_ALL PACKING #define MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL 45 +#define MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL_LEN 2 +#define MAVLINK_MSG_45_LEN 2 typedef struct __mavlink_waypoint_clear_all_t { @@ -9,8 +11,6 @@ typedef struct __mavlink_waypoint_clear_all_t } mavlink_waypoint_clear_all_t; - - /** * @brief Pack a waypoint_clear_all message * @param system_id ID of this system @@ -23,13 +23,13 @@ typedef struct __mavlink_waypoint_clear_all_t */ static inline uint16_t mavlink_msg_waypoint_clear_all_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component) { - uint16_t i = 0; + mavlink_waypoint_clear_all_t *p = (mavlink_waypoint_clear_all_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL; - 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 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL_LEN); } /** @@ -44,13 +44,13 @@ static inline uint16_t mavlink_msg_waypoint_clear_all_pack(uint8_t system_id, ui */ static inline uint16_t mavlink_msg_waypoint_clear_all_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 i = 0; + mavlink_waypoint_clear_all_t *p = (mavlink_waypoint_clear_all_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL; - 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 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL_LEN); } /** @@ -73,13 +73,36 @@ static inline uint16_t mavlink_msg_waypoint_clear_all_encode(uint8_t system_id, * @param target_system System ID * @param target_component Component ID */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_waypoint_clear_all_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) { - mavlink_message_t msg; - mavlink_msg_waypoint_clear_all_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_waypoint_clear_all_t payload; + uint16_t checksum; + mavlink_waypoint_clear_all_t *p = &payload; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL_LEN; + hdr.msgid = MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -92,7 +115,8 @@ static inline void mavlink_msg_waypoint_clear_all_send(mavlink_channel_t chan, u */ static inline uint8_t mavlink_msg_waypoint_clear_all_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_waypoint_clear_all_t *p = (mavlink_waypoint_clear_all_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); } /** @@ -102,7 +126,8 @@ static inline uint8_t mavlink_msg_waypoint_clear_all_get_target_system(const mav */ static inline uint8_t mavlink_msg_waypoint_clear_all_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_waypoint_clear_all_t *p = (mavlink_waypoint_clear_all_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); } /** @@ -113,6 +138,5 @@ static inline uint8_t mavlink_msg_waypoint_clear_all_get_target_component(const */ static inline void mavlink_msg_waypoint_clear_all_decode(const mavlink_message_t* msg, mavlink_waypoint_clear_all_t* waypoint_clear_all) { - waypoint_clear_all->target_system = mavlink_msg_waypoint_clear_all_get_target_system(msg); - waypoint_clear_all->target_component = mavlink_msg_waypoint_clear_all_get_target_component(msg); + memcpy( waypoint_clear_all, msg->payload, sizeof(mavlink_waypoint_clear_all_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_count.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_count.h index 9ceacb08064ee5bfaaf678f43834e76c0c463cde..8856c3c445ff33eb6e00ae3acc633895132b3ee9 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_count.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_count.h @@ -1,17 +1,17 @@ // MESSAGE WAYPOINT_COUNT PACKING #define MAVLINK_MSG_ID_WAYPOINT_COUNT 44 +#define MAVLINK_MSG_ID_WAYPOINT_COUNT_LEN 4 +#define MAVLINK_MSG_44_LEN 4 typedef struct __mavlink_waypoint_count_t { + uint16_t count; ///< Number of Waypoints in the Sequence uint8_t target_system; ///< System ID uint8_t target_component; ///< Component ID - uint16_t count; ///< Number of Waypoints in the Sequence } mavlink_waypoint_count_t; - - /** * @brief Pack a waypoint_count message * @param system_id ID of this system @@ -25,14 +25,14 @@ typedef struct __mavlink_waypoint_count_t */ static inline uint16_t mavlink_msg_waypoint_count_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint16_t count) { - uint16_t i = 0; + mavlink_waypoint_count_t *p = (mavlink_waypoint_count_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_COUNT; - 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(count, i, msg->payload); // Number of Waypoints in the Sequence + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->count = count; // uint16_t:Number of Waypoints in the Sequence - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WAYPOINT_COUNT_LEN); } /** @@ -48,14 +48,14 @@ static inline uint16_t mavlink_msg_waypoint_count_pack(uint8_t system_id, uint8_ */ static inline uint16_t mavlink_msg_waypoint_count_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 count) { - uint16_t i = 0; + mavlink_waypoint_count_t *p = (mavlink_waypoint_count_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_COUNT; - 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(count, i, msg->payload); // Number of Waypoints in the Sequence + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->count = count; // uint16_t:Number of Waypoints in the Sequence - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WAYPOINT_COUNT_LEN); } /** @@ -79,13 +79,37 @@ static inline uint16_t mavlink_msg_waypoint_count_encode(uint8_t system_id, uint * @param target_component Component ID * @param count Number of Waypoints in the Sequence */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_waypoint_count_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t count) { - mavlink_message_t msg; - mavlink_msg_waypoint_count_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, count); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_waypoint_count_t payload; + uint16_t checksum; + mavlink_waypoint_count_t *p = &payload; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->count = count; // uint16_t:Number of Waypoints in the Sequence + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_WAYPOINT_COUNT_LEN; + hdr.msgid = MAVLINK_MSG_ID_WAYPOINT_COUNT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -98,7 +122,8 @@ static inline void mavlink_msg_waypoint_count_send(mavlink_channel_t chan, uint8 */ static inline uint8_t mavlink_msg_waypoint_count_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_waypoint_count_t *p = (mavlink_waypoint_count_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); } /** @@ -108,7 +133,8 @@ static inline uint8_t mavlink_msg_waypoint_count_get_target_system(const mavlink */ static inline uint8_t mavlink_msg_waypoint_count_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_waypoint_count_t *p = (mavlink_waypoint_count_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); } /** @@ -118,10 +144,8 @@ static inline uint8_t mavlink_msg_waypoint_count_get_target_component(const mavl */ static inline uint16_t mavlink_msg_waypoint_count_get_count(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; + mavlink_waypoint_count_t *p = (mavlink_waypoint_count_t *)&msg->payload[0]; + return (uint16_t)(p->count); } /** @@ -132,7 +156,5 @@ static inline uint16_t mavlink_msg_waypoint_count_get_count(const mavlink_messag */ static inline void mavlink_msg_waypoint_count_decode(const mavlink_message_t* msg, mavlink_waypoint_count_t* waypoint_count) { - waypoint_count->target_system = mavlink_msg_waypoint_count_get_target_system(msg); - waypoint_count->target_component = mavlink_msg_waypoint_count_get_target_component(msg); - waypoint_count->count = mavlink_msg_waypoint_count_get_count(msg); + memcpy( waypoint_count, msg->payload, sizeof(mavlink_waypoint_count_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_current.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_current.h index 17f66fc298937fe031c4e02b6ad6e15cfa91f561..9c75639c796813e278fa126f7df9feb984fe6278 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_current.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_current.h @@ -1,6 +1,8 @@ // MESSAGE WAYPOINT_CURRENT PACKING #define MAVLINK_MSG_ID_WAYPOINT_CURRENT 42 +#define MAVLINK_MSG_ID_WAYPOINT_CURRENT_LEN 2 +#define MAVLINK_MSG_42_LEN 2 typedef struct __mavlink_waypoint_current_t { @@ -8,8 +10,6 @@ typedef struct __mavlink_waypoint_current_t } mavlink_waypoint_current_t; - - /** * @brief Pack a waypoint_current message * @param system_id ID of this system @@ -21,12 +21,12 @@ typedef struct __mavlink_waypoint_current_t */ static inline uint16_t mavlink_msg_waypoint_current_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint16_t seq) { - uint16_t i = 0; + mavlink_waypoint_current_t *p = (mavlink_waypoint_current_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_CURRENT; - i += put_uint16_t_by_index(seq, i, msg->payload); // Sequence + p->seq = seq; // uint16_t:Sequence - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WAYPOINT_CURRENT_LEN); } /** @@ -40,12 +40,12 @@ static inline uint16_t mavlink_msg_waypoint_current_pack(uint8_t system_id, uint */ static inline uint16_t mavlink_msg_waypoint_current_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint16_t seq) { - uint16_t i = 0; + mavlink_waypoint_current_t *p = (mavlink_waypoint_current_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_CURRENT; - i += put_uint16_t_by_index(seq, i, msg->payload); // Sequence + p->seq = seq; // uint16_t:Sequence - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WAYPOINT_CURRENT_LEN); } /** @@ -67,13 +67,35 @@ static inline uint16_t mavlink_msg_waypoint_current_encode(uint8_t system_id, ui * * @param seq Sequence */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_waypoint_current_send(mavlink_channel_t chan, uint16_t seq) { - mavlink_message_t msg; - mavlink_msg_waypoint_current_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, seq); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_waypoint_current_t payload; + uint16_t checksum; + mavlink_waypoint_current_t *p = &payload; + + p->seq = seq; // uint16_t:Sequence + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_WAYPOINT_CURRENT_LEN; + hdr.msgid = MAVLINK_MSG_ID_WAYPOINT_CURRENT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -86,10 +108,8 @@ static inline void mavlink_msg_waypoint_current_send(mavlink_channel_t chan, uin */ static inline uint16_t mavlink_msg_waypoint_current_get_seq(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload)[0]; - r.b[0] = (msg->payload)[1]; - return (uint16_t)r.s; + mavlink_waypoint_current_t *p = (mavlink_waypoint_current_t *)&msg->payload[0]; + return (uint16_t)(p->seq); } /** @@ -100,5 +120,5 @@ static inline uint16_t mavlink_msg_waypoint_current_get_seq(const mavlink_messag */ static inline void mavlink_msg_waypoint_current_decode(const mavlink_message_t* msg, mavlink_waypoint_current_t* waypoint_current) { - waypoint_current->seq = mavlink_msg_waypoint_current_get_seq(msg); + memcpy( waypoint_current, msg->payload, sizeof(mavlink_waypoint_current_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_reached.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_reached.h index ffcea52fc94f66b730a41f159629fee62cb14af8..5e1f144e8b68db6543b6c82bbdd836115beec42c 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_reached.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_reached.h @@ -1,6 +1,8 @@ // MESSAGE WAYPOINT_REACHED PACKING #define MAVLINK_MSG_ID_WAYPOINT_REACHED 46 +#define MAVLINK_MSG_ID_WAYPOINT_REACHED_LEN 2 +#define MAVLINK_MSG_46_LEN 2 typedef struct __mavlink_waypoint_reached_t { @@ -8,8 +10,6 @@ typedef struct __mavlink_waypoint_reached_t } mavlink_waypoint_reached_t; - - /** * @brief Pack a waypoint_reached message * @param system_id ID of this system @@ -21,12 +21,12 @@ typedef struct __mavlink_waypoint_reached_t */ static inline uint16_t mavlink_msg_waypoint_reached_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint16_t seq) { - uint16_t i = 0; + mavlink_waypoint_reached_t *p = (mavlink_waypoint_reached_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REACHED; - i += put_uint16_t_by_index(seq, i, msg->payload); // Sequence + p->seq = seq; // uint16_t:Sequence - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WAYPOINT_REACHED_LEN); } /** @@ -40,12 +40,12 @@ static inline uint16_t mavlink_msg_waypoint_reached_pack(uint8_t system_id, uint */ static inline uint16_t mavlink_msg_waypoint_reached_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint16_t seq) { - uint16_t i = 0; + mavlink_waypoint_reached_t *p = (mavlink_waypoint_reached_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REACHED; - i += put_uint16_t_by_index(seq, i, msg->payload); // Sequence + p->seq = seq; // uint16_t:Sequence - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WAYPOINT_REACHED_LEN); } /** @@ -67,13 +67,35 @@ static inline uint16_t mavlink_msg_waypoint_reached_encode(uint8_t system_id, ui * * @param seq Sequence */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_waypoint_reached_send(mavlink_channel_t chan, uint16_t seq) { - mavlink_message_t msg; - mavlink_msg_waypoint_reached_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, seq); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_waypoint_reached_t payload; + uint16_t checksum; + mavlink_waypoint_reached_t *p = &payload; + + p->seq = seq; // uint16_t:Sequence + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_WAYPOINT_REACHED_LEN; + hdr.msgid = MAVLINK_MSG_ID_WAYPOINT_REACHED; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -86,10 +108,8 @@ static inline void mavlink_msg_waypoint_reached_send(mavlink_channel_t chan, uin */ static inline uint16_t mavlink_msg_waypoint_reached_get_seq(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload)[0]; - r.b[0] = (msg->payload)[1]; - return (uint16_t)r.s; + mavlink_waypoint_reached_t *p = (mavlink_waypoint_reached_t *)&msg->payload[0]; + return (uint16_t)(p->seq); } /** @@ -100,5 +120,5 @@ static inline uint16_t mavlink_msg_waypoint_reached_get_seq(const mavlink_messag */ static inline void mavlink_msg_waypoint_reached_decode(const mavlink_message_t* msg, mavlink_waypoint_reached_t* waypoint_reached) { - waypoint_reached->seq = mavlink_msg_waypoint_reached_get_seq(msg); + memcpy( waypoint_reached, msg->payload, sizeof(mavlink_waypoint_reached_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request.h index 234b55c11d4da51f2c897096d2ca0e88f9c3ed2e..9f3ccdbe31565315ecfced445eeea41442388a41 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request.h @@ -1,17 +1,17 @@ // MESSAGE WAYPOINT_REQUEST PACKING #define MAVLINK_MSG_ID_WAYPOINT_REQUEST 40 +#define MAVLINK_MSG_ID_WAYPOINT_REQUEST_LEN 4 +#define MAVLINK_MSG_40_LEN 4 typedef struct __mavlink_waypoint_request_t { + uint16_t seq; ///< Sequence uint8_t target_system; ///< System ID uint8_t target_component; ///< Component ID - uint16_t seq; ///< Sequence } mavlink_waypoint_request_t; - - /** * @brief Pack a waypoint_request message * @param system_id ID of this system @@ -25,14 +25,14 @@ typedef struct __mavlink_waypoint_request_t */ static inline uint16_t mavlink_msg_waypoint_request_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint16_t seq) { - uint16_t i = 0; + mavlink_waypoint_request_t *p = (mavlink_waypoint_request_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST; - 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(seq, i, msg->payload); // Sequence + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->seq = seq; // uint16_t:Sequence - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WAYPOINT_REQUEST_LEN); } /** @@ -48,14 +48,14 @@ static inline uint16_t mavlink_msg_waypoint_request_pack(uint8_t system_id, uint */ static inline uint16_t mavlink_msg_waypoint_request_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 seq) { - uint16_t i = 0; + mavlink_waypoint_request_t *p = (mavlink_waypoint_request_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST; - 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(seq, i, msg->payload); // Sequence + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->seq = seq; // uint16_t:Sequence - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WAYPOINT_REQUEST_LEN); } /** @@ -79,13 +79,37 @@ static inline uint16_t mavlink_msg_waypoint_request_encode(uint8_t system_id, ui * @param target_component Component ID * @param seq Sequence */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_waypoint_request_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq) { - mavlink_message_t msg; - mavlink_msg_waypoint_request_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, seq); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_waypoint_request_t payload; + uint16_t checksum; + mavlink_waypoint_request_t *p = &payload; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->seq = seq; // uint16_t:Sequence + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_WAYPOINT_REQUEST_LEN; + hdr.msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -98,7 +122,8 @@ static inline void mavlink_msg_waypoint_request_send(mavlink_channel_t chan, uin */ static inline uint8_t mavlink_msg_waypoint_request_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_waypoint_request_t *p = (mavlink_waypoint_request_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); } /** @@ -108,7 +133,8 @@ static inline uint8_t mavlink_msg_waypoint_request_get_target_system(const mavli */ static inline uint8_t mavlink_msg_waypoint_request_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_waypoint_request_t *p = (mavlink_waypoint_request_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); } /** @@ -118,10 +144,8 @@ static inline uint8_t mavlink_msg_waypoint_request_get_target_component(const ma */ static inline uint16_t mavlink_msg_waypoint_request_get_seq(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; + mavlink_waypoint_request_t *p = (mavlink_waypoint_request_t *)&msg->payload[0]; + return (uint16_t)(p->seq); } /** @@ -132,7 +156,5 @@ static inline uint16_t mavlink_msg_waypoint_request_get_seq(const mavlink_messag */ static inline void mavlink_msg_waypoint_request_decode(const mavlink_message_t* msg, mavlink_waypoint_request_t* waypoint_request) { - waypoint_request->target_system = mavlink_msg_waypoint_request_get_target_system(msg); - waypoint_request->target_component = mavlink_msg_waypoint_request_get_target_component(msg); - waypoint_request->seq = mavlink_msg_waypoint_request_get_seq(msg); + memcpy( waypoint_request, msg->payload, sizeof(mavlink_waypoint_request_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request_list.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request_list.h index d4f7cf03d5d3c9bc04c86d06f2f490f668f46e12..34d8276d073aebcf62a07fa620bca549ef0b3b15 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request_list.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request_list.h @@ -1,6 +1,8 @@ // MESSAGE WAYPOINT_REQUEST_LIST PACKING #define MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST 43 +#define MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST_LEN 2 +#define MAVLINK_MSG_43_LEN 2 typedef struct __mavlink_waypoint_request_list_t { @@ -9,8 +11,6 @@ typedef struct __mavlink_waypoint_request_list_t } mavlink_waypoint_request_list_t; - - /** * @brief Pack a waypoint_request_list message * @param system_id ID of this system @@ -23,13 +23,13 @@ typedef struct __mavlink_waypoint_request_list_t */ static inline uint16_t mavlink_msg_waypoint_request_list_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component) { - uint16_t i = 0; + mavlink_waypoint_request_list_t *p = (mavlink_waypoint_request_list_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST; - 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 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST_LEN); } /** @@ -44,13 +44,13 @@ static inline uint16_t mavlink_msg_waypoint_request_list_pack(uint8_t system_id, */ static inline uint16_t mavlink_msg_waypoint_request_list_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 i = 0; + mavlink_waypoint_request_list_t *p = (mavlink_waypoint_request_list_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST; - 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 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST_LEN); } /** @@ -73,13 +73,36 @@ static inline uint16_t mavlink_msg_waypoint_request_list_encode(uint8_t system_i * @param target_system System ID * @param target_component Component ID */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_waypoint_request_list_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) { - mavlink_message_t msg; - mavlink_msg_waypoint_request_list_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_waypoint_request_list_t payload; + uint16_t checksum; + mavlink_waypoint_request_list_t *p = &payload; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST_LEN; + hdr.msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -92,7 +115,8 @@ static inline void mavlink_msg_waypoint_request_list_send(mavlink_channel_t chan */ static inline uint8_t mavlink_msg_waypoint_request_list_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_waypoint_request_list_t *p = (mavlink_waypoint_request_list_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); } /** @@ -102,7 +126,8 @@ static inline uint8_t mavlink_msg_waypoint_request_list_get_target_system(const */ static inline uint8_t mavlink_msg_waypoint_request_list_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_waypoint_request_list_t *p = (mavlink_waypoint_request_list_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); } /** @@ -113,6 +138,5 @@ static inline uint8_t mavlink_msg_waypoint_request_list_get_target_component(con */ static inline void mavlink_msg_waypoint_request_list_decode(const mavlink_message_t* msg, mavlink_waypoint_request_list_t* waypoint_request_list) { - waypoint_request_list->target_system = mavlink_msg_waypoint_request_list_get_target_system(msg); - waypoint_request_list->target_component = mavlink_msg_waypoint_request_list_get_target_component(msg); + memcpy( waypoint_request_list, msg->payload, sizeof(mavlink_waypoint_request_list_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_set_current.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_set_current.h index 6570fe4f8297e9b7e860f0f3374d90329dda1875..829f29982b14948a99a0017b1db45d2f057fec76 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_set_current.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_set_current.h @@ -1,17 +1,17 @@ // MESSAGE WAYPOINT_SET_CURRENT PACKING #define MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT 41 +#define MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT_LEN 4 +#define MAVLINK_MSG_41_LEN 4 typedef struct __mavlink_waypoint_set_current_t { + uint16_t seq; ///< Sequence uint8_t target_system; ///< System ID uint8_t target_component; ///< Component ID - uint16_t seq; ///< Sequence } mavlink_waypoint_set_current_t; - - /** * @brief Pack a waypoint_set_current message * @param system_id ID of this system @@ -25,14 +25,14 @@ typedef struct __mavlink_waypoint_set_current_t */ static inline uint16_t mavlink_msg_waypoint_set_current_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint16_t seq) { - uint16_t i = 0; + mavlink_waypoint_set_current_t *p = (mavlink_waypoint_set_current_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT; - 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(seq, i, msg->payload); // Sequence + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->seq = seq; // uint16_t:Sequence - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT_LEN); } /** @@ -48,14 +48,14 @@ static inline uint16_t mavlink_msg_waypoint_set_current_pack(uint8_t system_id, */ static inline uint16_t mavlink_msg_waypoint_set_current_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 seq) { - uint16_t i = 0; + mavlink_waypoint_set_current_t *p = (mavlink_waypoint_set_current_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT; - 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(seq, i, msg->payload); // Sequence + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->seq = seq; // uint16_t:Sequence - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT_LEN); } /** @@ -79,13 +79,37 @@ static inline uint16_t mavlink_msg_waypoint_set_current_encode(uint8_t system_id * @param target_component Component ID * @param seq Sequence */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_waypoint_set_current_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq) { - mavlink_message_t msg; - mavlink_msg_waypoint_set_current_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, seq); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_waypoint_set_current_t payload; + uint16_t checksum; + mavlink_waypoint_set_current_t *p = &payload; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->seq = seq; // uint16_t:Sequence + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT_LEN; + hdr.msgid = MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -98,7 +122,8 @@ static inline void mavlink_msg_waypoint_set_current_send(mavlink_channel_t chan, */ static inline uint8_t mavlink_msg_waypoint_set_current_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_waypoint_set_current_t *p = (mavlink_waypoint_set_current_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); } /** @@ -108,7 +133,8 @@ static inline uint8_t mavlink_msg_waypoint_set_current_get_target_system(const m */ static inline uint8_t mavlink_msg_waypoint_set_current_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_waypoint_set_current_t *p = (mavlink_waypoint_set_current_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); } /** @@ -118,10 +144,8 @@ static inline uint8_t mavlink_msg_waypoint_set_current_get_target_component(cons */ static inline uint16_t mavlink_msg_waypoint_set_current_get_seq(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; + mavlink_waypoint_set_current_t *p = (mavlink_waypoint_set_current_t *)&msg->payload[0]; + return (uint16_t)(p->seq); } /** @@ -132,7 +156,5 @@ static inline uint16_t mavlink_msg_waypoint_set_current_get_seq(const mavlink_me */ static inline void mavlink_msg_waypoint_set_current_decode(const mavlink_message_t* msg, mavlink_waypoint_set_current_t* waypoint_set_current) { - waypoint_set_current->target_system = mavlink_msg_waypoint_set_current_get_target_system(msg); - waypoint_set_current->target_component = mavlink_msg_waypoint_set_current_get_target_component(msg); - waypoint_set_current->seq = mavlink_msg_waypoint_set_current_get_seq(msg); + memcpy( waypoint_set_current, msg->payload, sizeof(mavlink_waypoint_set_current_t)); } diff --git a/thirdParty/mavlink/include/mavlink_checksum.h b/thirdParty/mavlink/include/mavlink_checksum.h new file mode 100755 index 0000000000000000000000000000000000000000..0f4225174122463c3d3094cc26f6a5faa478e6fa --- /dev/null +++ b/thirdParty/mavlink/include/mavlink_checksum.h @@ -0,0 +1,156 @@ +/** @file + * @brief MAVLink comm protocol checksum routines. + * @see http://qgroundcontrol.org/mavlink/ + * Edited on Monday, August 8 2011 + */ + +#ifdef __cplusplus +extern "C" { +#endif + +#ifndef _CHECKSUM_H_ +#define _CHECKSUM_H_ + +#include "inttypes.h" + + +/** + * + * CALCULATE THE CHECKSUM + * + */ + +#define X25_INIT_CRC 0xffff +#define X25_VALIDATE_CRC 0xf0b8 + +/** + * @brief Accumulate the X.25 CRC by adding one char at a time. + * + * The checksum function adds the hash of one char at a time to the + * 16 bit checksum (uint16_t). + * + * @param data new char to hash + * @param crcAccum the already accumulated checksum + **/ +static inline void crc_accumulate(uint8_t data, uint16_t *crcAccum) +{ + /*Accumulate one byte of data into the CRC*/ + uint8_t tmp; + + tmp=data ^ (uint8_t)(*crcAccum &0xff); + tmp^= (tmp<<4); + *crcAccum = (*crcAccum>>8) ^ (tmp<<8) ^ (tmp <<3) ^ (tmp>>4); +// *crcAccum += data; // super simple to test +} + +/** + * @brief Initiliaze the buffer for the X.25 CRC + * + * @param crcAccum the 16 bit X.25 CRC + */ +static inline void crc_init(uint16_t* crcAccum) +{ + *crcAccum = X25_INIT_CRC; +} + +/** + * @brief Initiliaze the buffer for the X.25 CRC to a specified value + * + * @param crcAccum the 16 bit X.25 CRC + */ +static inline void crc_init2(uint16_t* crcAccum, uint16_t crcValue) +{ + *crcAccum = crcValue; +} + + + +/** + * @brief Calculates the X.25 checksum on a byte buffer + * + * @param pBuffer buffer containing the byte array to hash + * @param length length of the byte array + * @return the checksum over the buffer bytes + **/ +static inline uint16_t crc_calculate(uint8_t* pBuffer, int length) +{ + + // For a "message" of length bytes contained in the unsigned char array + // pointed to by pBuffer, calculate the CRC + // crcCalculate(unsigned char* pBuffer, int length, unsigned short* checkConst) < not needed + + uint16_t crcTmp; + //uint16_t tmp; + uint8_t* pTmp; + int i; + + pTmp=pBuffer; + + + /* init crcTmp */ + crc_init(&crcTmp); + + for (i = 0; i < length; i++){ + crc_accumulate(*pTmp++, &crcTmp); + } + + /* This is currently not needed, as only the checksum over payload should be computed + tmp = crcTmp; + crcAccumulate((unsigned char)(~crcTmp & 0xff),&tmp); + crcAccumulate((unsigned char)((~crcTmp>>8)&0xff),&tmp); + *checkConst = tmp; + */ + return(crcTmp); +} + + +/** + * @brief Calculates the X.25 checksum on a msg buffer + * + * @param pMSG buffer containing the msg to hash + * @param length number of bytes to hash + * @return the checksum over the buffer bytes + **/ +static inline uint16_t crc_calculate_msg(mavlink_message_t* pMSG, int length) +{ + + // For a "message" of length bytes contained in the unsigned char array + // pointed to by pBuffer, calculate the CRC + // crcCalculate(unsigned char* pBuffer, int length, unsigned short* checkConst) < not needed + + uint16_t crcTmp; + //uint16_t tmp; + uint8_t* pTmp; + int i; + + pTmp=&pMSG->len; + + /* init crcTmp */ + crc_init(&crcTmp); + + for (i = 0; i < 5; i++){ + crc_accumulate(*pTmp++, &crcTmp); + } + + pTmp=&pMSG->payload[0]; + for (; i < length; i++){ + crc_accumulate(*pTmp++, &crcTmp); + } + + /* This is currently not needed, as only the checksum over payload should be computed + tmp = crcTmp; + crcAccumulate((unsigned char)(~crcTmp & 0xff),&tmp); + crcAccumulate((unsigned char)((~crcTmp>>8)&0xff),&tmp); + *checkConst = tmp; + */ + return(crcTmp); +} + + + + +#endif /* _CHECKSUM_H_ */ + +#ifdef __cplusplus +} +#endif diff --git a/thirdParty/mavlink/include/mavlink_data.h b/thirdParty/mavlink/include/mavlink_data.h new file mode 100755 index 0000000000000000000000000000000000000000..ce51e2b93ae3beca3b7e5769748f7f87c92ec07d --- /dev/null +++ b/thirdParty/mavlink/include/mavlink_data.h @@ -0,0 +1,21 @@ +/** @file + * @brief Main MAVLink comm protocol data. + * @see http://qgroundcontrol.org/mavlink/ + * Edited on Monday, August 8 2011 + */ + +#ifndef _ML_DATA_H_ +#define _ML_DATA_H_ + +#include "mavlink_types.h" + +#ifdef MAVLINK_CHECK_LENGTH +const uint8_t MAVLINK_CONST mavlink_msg_lengths[256] = MAVLINK_MESSAGE_LENGTHS; +#endif + +const uint8_t MAVLINK_CONST mavlink_msg_keys[256] = MAVLINK_MESSAGE_KEYS; + +mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB]; +mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB]; + +#endif \ No newline at end of file diff --git a/thirdParty/mavlink/include/mavlink_options.h b/thirdParty/mavlink/include/mavlink_options.h new file mode 100755 index 0000000000000000000000000000000000000000..bc337f6176d91b52feb991ae8206e656ba7682b6 --- /dev/null +++ b/thirdParty/mavlink/include/mavlink_options.h @@ -0,0 +1,102 @@ +/** @file + * @brief MAVLink comm protocol option constants. + * @see http://qgroundcontrol.org/mavlink/ + * Edited on Monday, August 8 2011 + */ + +#ifdef __cplusplus +extern "C" { +#endif + +#ifndef _ML_OPTIONS_H_ +#define _ML_OPTIONS_H_ + + +/** + * + * Receive message length check option. On receive verify the length field + * as soon as the message ID field is received. Requires a 256 byte const + * table. Comment out the define to leave out the table and code to check it. + * + */ +//#define MAVLINK_CHECK_LENGTH + +/** + * + * Receive message buffer option. This option should be used only when the + * side effects are understood but allows the underlying program access to + * the internal recieve buffer - eliminating the usual double buffering. It + * also REQUIRES changes in the return type of mavlink_parse_char so only + * enable if you make the changes required. Default DISABLED. + * + */ +//#define MAVLINK_STATIC_BUFFER + +/** + * + * Receive message buffers option. This option defines how many msg buffers + * mavlink will define, and thereby how many links it can support. A default + * will be supplied if the symbol is not pre-defined, dependant on the make + * envionment. The default is 16 for a recognised OS envionment and 1 + * otherwise. + * + */ +#if !((defined MAVLINK_COMM_NB) | (MAVLINK_COMM_NB < 1)) +#undef MAVLINK_COMM_NB + #if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32) | (defined __APPLE__) + #define MAVLINK_COMM_NB 16 + #else + #define MAVLINK_COMM_NB 1 + #endif +#endif + + +/** + * + * Data relization option. This option controls inclusion of the file + * mavlink_data.h in the current compile unit - thus defining mavlink's + * variables. Default is ON (not defined) because typically mavlink.h is only + * included once in a system but if it was used in two files there would + * be duplicate variables at link time. Normal practice would be to define + * this symbol outside of this file as defining it here will cause missing + * symbols at link time. In other words in the first file to include mavlink.h + * do not define this sybol, then define this symbol in all other files before + * including mavlink.h + * + */ +//#define MAVLINK_NO_DATA +#ifdef MAVLINK_NO_DATA + #undef MAVLINK_DATA +#else + #define MAVLINK_DATA +#endif + +/** + * + * Custom data const data relization and access options. + * This define is placed in the form + * const uint8_t MAVLINK_CONST name[] = { ... }; + * for the keys table and (if applicable) lengths table to tell the compiler + * were to put the data. The access option is placed in the form + * variable = MAVLINK_CONST_READ( name[i] ); + * in order to allow custom read function's or accessors. + * By default MAVLINK_CONST is defined as nothing and MAVLINK_CONST_READ as + * MAVLINK_CONST_READ( a ) a + * These symbols are only defined if not already defined allowing this file + * to remain unchanged while the actual definitions are maintained in external + * files. + * + */ +#ifndef MAVLINK_CONST +#define MAVLINK_CONST +#endif +#ifndef MAVLINK_CONST_READ +#define MAVLINK_CONST_READ( a ) a +#endif + + +#endif /* _ML_OPTIONS_H_ */ + +#ifdef __cplusplus +} +#endif diff --git a/thirdParty/mavlink/include/mavlink_protocol.h b/thirdParty/mavlink/include/mavlink_protocol.h new file mode 100755 index 0000000000000000000000000000000000000000..fe63af913e863c7718545fe38577ee0c4ea1931e --- /dev/null +++ b/thirdParty/mavlink/include/mavlink_protocol.h @@ -0,0 +1,424 @@ +/** @file + * @brief Main MAVLink comm protocol routines. + * @see http://qgroundcontrol.org/mavlink/ + * Edited on Monday, August 8 2011 + */ + +#ifndef _MAVLINK_PROTOCOL_H_ +#define _MAVLINK_PROTOCOL_H_ + +#include "mavlink_types.h" + +#include "mavlink_checksum.h" + +#ifdef MAVLINK_CHECK_LENGTH +extern const uint8_t MAVLINK_CONST mavlink_msg_lengths[256]; +#endif + +extern const uint8_t MAVLINK_CONST mavlink_msg_keys[256]; + +extern mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB]; +extern mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB]; + + +/** + * @brief Initialize the communication stack + * + * This function has to be called before using commParseBuffer() to initialize the different status registers. + * + * @return Will initialize the different buffers and status registers. + */ +static void mavlink_parse_state_initialize(mavlink_status_t* initStatus) +{ + if ((initStatus->parse_state <= MAVLINK_PARSE_STATE_UNINIT) || (initStatus->parse_state > MAVLINK_PARSE_STATE_GOT_CRC1)) + { + initStatus->ck_a = 0; + initStatus->ck_b = 0; + initStatus->msg_received = 0; + initStatus->buffer_overrun = 0; + initStatus->parse_error = 0; + initStatus->parse_state = MAVLINK_PARSE_STATE_UNINIT; + initStatus->packet_idx = 0; + initStatus->packet_rx_drop_count = 0; + initStatus->packet_rx_success_count = 0; + initStatus->current_rx_seq = 0; + initStatus->current_tx_seq = 0; + } +} + +static inline mavlink_status_t* mavlink_get_channel_status(uint8_t chan) +{ + + return &m_mavlink_status[chan]; +} + +/** + * @brief Finalize a MAVLink message with MAVLINK_COMM_0 as default channel + * + * This function calculates the checksum and sets length and aircraft id correctly. + * It assumes that the message id and the payload are already correctly set. + * + * @warning This function implicitely assumes the message is sent over channel zero. + * if the message is sent over a different channel it will reach the receiver + * without error, BUT the sequence number might be wrong due to the wrong + * channel sequence counter. This will result is wrongly reported excessive + * packet loss. Please use @see mavlink_{pack|encode}_headerless and then + * @see mavlink_finalize_message_chan before sending for a correct channel + * assignment. Please note that the mavlink_msg_xxx_pack and encode functions + * assign channel zero as default and thus induce possible loss counter errors.\ + * They have been left to ensure code compatibility. + * + * @see mavlink_finalize_message_chan + * @param msg Message to finalize + * @param system_id Id of the sending (this) system, 1-127 + * @param length Message length, usually just the counter incremented while packing the message + */ +static inline uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, uint16_t length) +{ + // This code part is the same for all messages; + uint16_t checksum; + msg->len = length; + msg->sysid = system_id; + msg->compid = component_id; + // One sequence number per component + msg->seq = mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq; + mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq = mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq+1; +// checksum = crc_calculate((uint8_t*)((void*)msg), length + MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_msg(msg, length + MAVLINK_CORE_HEADER_LEN); + msg->ck_a = (uint8_t)(checksum & 0xFF); ///< High byte + msg->ck_b = (uint8_t)(checksum >> 8); ///< Low byte + + return length + MAVLINK_NUM_NON_STX_PAYLOAD_BYTES; +} + +/** + * @brief Finalize a MAVLink message with channel assignment + * + * This function calculates the checksum and sets length and aircraft id correctly. + * It assumes that the message id and the payload are already correctly set. This function + * can also be used if the message header has already been written before (as in mavlink_msg_xxx_pack + * instead of mavlink_msg_xxx_pack_headerless), it just introduces little extra overhead. + * + * @param msg Message to finalize + * @param system_id Id of the sending (this) system, 1-127 + * @param length Message length, usually just the counter incremented while packing the message + */ +static inline uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, uint8_t chan, uint16_t length) +{ + // This code part is the same for all messages; + uint16_t checksum; + msg->len = length; + msg->sysid = system_id; + msg->compid = component_id; + // One sequence number per component + msg->seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = mavlink_get_channel_status(chan)->current_tx_seq+1; +// checksum = crc_calculate((uint8_t*)((void*)msg), length + MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_msg(msg, length + MAVLINK_CORE_HEADER_LEN); + msg->ck_a = (uint8_t)(checksum & 0xFF); ///< High byte + msg->ck_b = (uint8_t)(checksum >> 8); ///< Low byte + + return length + MAVLINK_NUM_NON_STX_PAYLOAD_BYTES; +} + +/** + * @brief Pack a message to send it over a serial byte stream + */ +static inline uint16_t mavlink_msg_to_send_buffer(uint8_t* buffer, const mavlink_message_t* msg) +{ + *(buffer+0) = MAVLINK_STX; ///< Start transmit +// memcpy((buffer+1), msg, msg->len + MAVLINK_CORE_HEADER_LEN); ///< Core header plus payload + memcpy((buffer+1), &msg->len, MAVLINK_CORE_HEADER_LEN); ///< Core header + memcpy((buffer+1+MAVLINK_CORE_HEADER_LEN), &msg->payload[0], msg->len); ///< payload + *(buffer + msg->len + MAVLINK_CORE_HEADER_LEN + 1) = msg->ck_a; + *(buffer + msg->len + MAVLINK_CORE_HEADER_LEN + 2) = msg->ck_b; + return msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES; +// return 0; +} + +/** + * @brief Get the required buffer size for this message + */ +static inline uint16_t mavlink_msg_get_send_buffer_length(const mavlink_message_t* msg) +{ + return msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES; +} + +union checksum_ { + uint16_t s; + uint8_t c[2]; +}; + +static inline void mavlink_start_checksum(mavlink_message_t* msg) +{ + union checksum_ ck; + crc_init(&(ck.s)); + msg->ck_a = ck.c[0]; + msg->ck_b = ck.c[1]; +} + +static inline void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c) +{ + union checksum_ ck; + ck.c[0] = msg->ck_a; + ck.c[1] = msg->ck_b; + crc_accumulate(c, &(ck.s)); + msg->ck_a = ck.c[0]; + msg->ck_b = ck.c[1]; +} + +/** + * This is a convenience function which handles the complete MAVLink parsing. + * the function will parse one byte at a time and return the complete packet once + * it could be successfully decoded. Checksum and other failures will be silently + * ignored. + * + * @param chan ID of the current channel. This allows to parse different channels with this function. + * a channel is not a physical message channel like a serial port, but a logic partition of + * the communication streams in this case. COMM_NB is the limit for the number of channels + * on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows + * @param c The char to barse + * + * @param returnMsg NULL if no message could be decoded, the message data else + * @return 0 if no message could be decoded, 1 else + * + * A typical use scenario of this function call is: + * + * @code + * #include // For fixed-width uint8_t type + * + * mavlink_message_t msg; + * int chan = 0; + * + * + * while(serial.bytesAvailable > 0) + * { + * uint8_t byte = serial.getNextByte(); + * if (mavlink_parse_char(chan, byte, &msg)) + * { + * printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid); + * } + * } + * + * + * @endcode + */ +#ifdef MAVLINK_STATIC_BUFFER +static inline mavlink_message_t* mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status) +#else +static inline int16_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status) +#endif +{ + // Initializes only once, values keep unchanged after first initialization + mavlink_parse_state_initialize(mavlink_get_channel_status(chan)); + + mavlink_message_t* rxmsg = &m_mavlink_message[chan]; ///< The currently decoded message + mavlink_status_t* status = mavlink_get_channel_status(chan); ///< The current decode status + int bufferIndex = 0; + + status->msg_received = 0; + + switch (status->parse_state) + { + case MAVLINK_PARSE_STATE_UNINIT: + case MAVLINK_PARSE_STATE_IDLE: + if (c == MAVLINK_STX) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; + mavlink_start_checksum(rxmsg); + } + break; + + case MAVLINK_PARSE_STATE_GOT_STX: + if (status->msg_received) + { + status->buffer_overrun++; + status->parse_error++; + status->msg_received = 0; + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + } + else + { + // NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2 + rxmsg->len = c; + status->packet_idx = 0; + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH; + } + break; + + case MAVLINK_PARSE_STATE_GOT_LENGTH: + rxmsg->seq = c; + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ; + break; + + case MAVLINK_PARSE_STATE_GOT_SEQ: + rxmsg->sysid = c; + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID; + break; + + case MAVLINK_PARSE_STATE_GOT_SYSID: + rxmsg->compid = c; + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID; + break; + + case MAVLINK_PARSE_STATE_GOT_COMPID: + rxmsg->msgid = c; + mavlink_update_checksum(rxmsg, c); +#ifdef MAVLINK_CHECK_LENGTH + if (rxmsg->len != MAVLINK_CONST_READ( mavlink_msg_lengths[c] ) ) + status->parse_state = MAVLINK_PARSE_STATE_IDLE; // abort, not going to understand it anyway + else ; +#endif + if (rxmsg->len == 0) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; + } + else + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID; + } + break; + + case MAVLINK_PARSE_STATE_GOT_MSGID: + rxmsg->payload[status->packet_idx++] = c; + mavlink_update_checksum(rxmsg, c); + if (status->packet_idx == rxmsg->len) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; + mavlink_update_checksum(rxmsg, + MAVLINK_CONST_READ( mavlink_msg_lengths[rxmsg->msgid] ) ); + } + break; + + case MAVLINK_PARSE_STATE_GOT_PAYLOAD: + if (c != rxmsg->ck_a) + { + // Check first checksum byte + status->parse_error++; + status->msg_received = 0; + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + if (c == MAVLINK_STX) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; + mavlink_start_checksum(rxmsg); + } + } + else + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1; + } + break; + + case MAVLINK_PARSE_STATE_GOT_CRC1: + if (c != rxmsg->ck_b) + {// Check second checksum byte + status->parse_error++; + status->msg_received = 0; + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + if (c == MAVLINK_STX) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; + mavlink_start_checksum(rxmsg); + } + } + else + { + // Successfully got message + status->msg_received = 1; + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + if ( r_message != NULL ) + memcpy(r_message, rxmsg, sizeof(mavlink_message_t)); + else ; + } + break; + } + + bufferIndex++; + // If a message has been sucessfully decoded, check index + if (status->msg_received == 1) + { + //while(status->current_seq != rxmsg->seq) + //{ + // status->packet_rx_drop_count++; + // status->current_seq++; + //} + status->current_rx_seq = rxmsg->seq; + // Initial condition: If no packet has been received so far, drop count is undefined + if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0; + // Count this packet as received + status->packet_rx_success_count++; + } + + r_mavlink_status->current_rx_seq = status->current_rx_seq+1; + r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count; + r_mavlink_status->packet_rx_drop_count = status->parse_error; + status->parse_error = 0; + if (status->msg_received == 1) + { + if ( r_message != NULL ) + return r_message; + else return rxmsg; + } else return NULL; +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +// To make MAVLink work on your MCU, define a similar function + +/* + +#include "mavlink_types.h" + +void comm_send_ch(mavlink_channel_t chan, uint8_t ch) +{ + if (chan == MAVLINK_COMM_0) + { + uart0_transmit(ch); + } + if (chan == MAVLINK_COMM_1) + { + uart1_transmit(ch); + } +} + + +static inline void mavlink_send_msg(mavlink_channel_t chan, mavlink_message_t* msg) +{ + // ARM7 MCU board implementation + // Create pointer on message struct + // Send STX + comm_send_ch(chan, MAVLINK_STX); + comm_send_ch(chan, msg->len); + comm_send_ch(chan, msg->seq); + comm_send_ch(chan, msg->sysid); + comm_send_ch(chan, msg->compid); + comm_send_ch(chan, msg->msgid); + for(uint16_t i = 0; i < msg->len; i++) + { + comm_send_ch(chan, msg->payload[i]); + } + comm_send_ch(chan, msg->ck_a); + comm_send_ch(chan, msg->ck_b); +} + +static inline void mavlink_send_mem(mavlink_channel_t chan, (uint8_t *)mem, uint8_t num) +{ + // ARM7 MCU board implementation + // Create pointer on message struct + // Send STX + for(uint16_t i = 0; i < num; i++) + { + comm_send_ch( chan, mem[i] ); + } +} + */ +static inline void mavlink_send_uart(mavlink_channel_t chan, mavlink_message_t* msg); +static inline void mavlink_send_mem(mavlink_channel_t chan, (uint8_t *)mem, uint8_t num); +#define mavlink_send_msg( a, b ) mavlink_send_uart( a, b ) +#endif + +#endif /* _MAVLINK_PROTOCOL_H_ */ diff --git a/thirdParty/mavlink/include/mavlink_types.h b/thirdParty/mavlink/include/mavlink_types.h old mode 100644 new mode 100755 index 67eed17c8ef58db38206917b2867b15817141e0a..36d8d57c5d452e1f2093624567dcfa6fe52e7e10 --- a/thirdParty/mavlink/include/mavlink_types.h +++ b/thirdParty/mavlink/include/mavlink_types.h @@ -1,251 +1,109 @@ -#ifndef MAVLINK_TYPES_H_ -#define MAVLINK_TYPES_H_ - -#include "inttypes.h" - -enum MAV_CLASS -{ - MAV_CLASS_GENERIC = 0, ///< Generic autopilot, full support for everything - MAV_CLASS_PIXHAWK = 1, ///< PIXHAWK autopilot, http://pixhawk.ethz.ch - MAV_CLASS_SLUGS = 2, ///< SLUGS autopilot, http://slugsuav.soe.ucsc.edu - MAV_CLASS_ARDUPILOTMEGA = 3, ///< ArduPilotMega / ArduCopter, http://diydrones.com - MAV_CLASS_OPENPILOT = 4, ///< OpenPilot, http://openpilot.org - MAV_CLASS_GENERIC_MISSION_WAYPOINTS_ONLY = 5, ///< Generic autopilot only supporting simple waypoints - MAV_CLASS_GENERIC_MISSION_NAVIGATION_ONLY = 6, ///< Generic autopilot supporting waypoints and other simple navigation commands - MAV_CLASS_GENERIC_MISSION_FULL = 7, ///< Generic autopilot supporting the full mission command set - MAV_CLASS_NONE = 8, ///< No valid autopilot - MAV_CLASS_NB ///< Number of autopilot classes -}; - -enum MAV_ACTION -{ - MAV_ACTION_HOLD = 0, - MAV_ACTION_MOTORS_START = 1, - MAV_ACTION_LAUNCH = 2, - MAV_ACTION_RETURN = 3, - MAV_ACTION_EMCY_LAND = 4, - MAV_ACTION_EMCY_KILL = 5, - MAV_ACTION_CONFIRM_KILL = 6, - MAV_ACTION_CONTINUE = 7, - MAV_ACTION_MOTORS_STOP = 8, - MAV_ACTION_HALT = 9, - MAV_ACTION_SHUTDOWN = 10, - MAV_ACTION_REBOOT = 11, - MAV_ACTION_SET_MANUAL = 12, - MAV_ACTION_SET_AUTO = 13, - MAV_ACTION_STORAGE_READ = 14, - MAV_ACTION_STORAGE_WRITE = 15, - MAV_ACTION_CALIBRATE_RC = 16, - MAV_ACTION_CALIBRATE_GYRO = 17, - MAV_ACTION_CALIBRATE_MAG = 18, - MAV_ACTION_CALIBRATE_ACC = 19, - MAV_ACTION_CALIBRATE_PRESSURE = 20, - MAV_ACTION_REC_START = 21, - MAV_ACTION_REC_PAUSE = 22, - MAV_ACTION_REC_STOP = 23, - MAV_ACTION_TAKEOFF = 24, - MAV_ACTION_NAVIGATE = 25, - MAV_ACTION_LAND = 26, - MAV_ACTION_LOITER = 27, - MAV_ACTION_SET_ORIGIN = 28, - MAV_ACTION_RELAY_ON = 29, - MAV_ACTION_RELAY_OFF = 30, - MAV_ACTION_GET_IMAGE = 31, - MAV_ACTION_VIDEO_START = 32, - MAV_ACTION_VIDEO_STOP = 33, - MAV_ACTION_RESET_MAP = 34, - MAV_ACTION_RESET_PLAN = 35, - MAV_ACTION_DELAY_BEFORE_COMMAND = 36, - MAV_ACTION_ASCEND_AT_RATE = 37, - MAV_ACTION_CHANGE_MODE = 38, - MAV_ACTION_LOITER_MAX_TURNS = 39, - MAV_ACTION_LOITER_MAX_TIME = 40, - MAV_ACTION_START_HILSIM = 41, - MAV_ACTION_STOP_HILSIM = 42, - MAV_ACTION_NB ///< Number of MAV actions -}; - -enum MAV_MODE -{ - MAV_MODE_UNINIT = 0, ///< System is in undefined state - MAV_MODE_LOCKED = 1, ///< Motors are blocked, system is safe - MAV_MODE_MANUAL = 2, ///< System is allowed to be active, under manual (RC) control - MAV_MODE_GUIDED = 3, ///< System is allowed to be active, under autonomous control, manual setpoint - MAV_MODE_AUTO = 4, ///< System is allowed to be active, under autonomous control and navigation - MAV_MODE_TEST1 = 5, ///< Generic test mode, for custom use - MAV_MODE_TEST2 = 6, ///< Generic test mode, for custom use - MAV_MODE_TEST3 = 7, ///< Generic test mode, for custom use - MAV_MODE_READY = 8, ///< System is ready, motors are unblocked, but controllers are inactive - MAV_MODE_RC_TRAINING = 9 ///< System is blocked, only RC valued are read and reported back -}; - -enum MAV_STATE -{ - MAV_STATE_UNINIT = 0, - MAV_STATE_BOOT, - MAV_STATE_CALIBRATING, - MAV_STATE_STANDBY, - MAV_STATE_ACTIVE, - MAV_STATE_CRITICAL, - MAV_STATE_EMERGENCY, - MAV_STATE_HILSIM, - MAV_STATE_POWEROFF -}; - -enum MAV_NAV -{ - MAV_NAV_GROUNDED = 0, - MAV_NAV_LIFTOFF, - MAV_NAV_HOLD, - MAV_NAV_WAYPOINT, - MAV_NAV_VECTOR, - MAV_NAV_RETURNING, - MAV_NAV_LANDING, - MAV_NAV_LOST, - MAV_NAV_LOITER, - MAV_NAV_FREE_DRIFT -}; - -enum MAV_TYPE -{ - MAV_GENERIC = 0, - MAV_FIXED_WING = 1, - MAV_QUADROTOR = 2, - MAV_COAXIAL = 3, - MAV_HELICOPTER = 4, - MAV_GROUND = 5, - OCU = 6, - MAV_AIRSHIP = 7, - MAV_FREE_BALLOON = 8, - MAV_ROCKET = 9, - UGV_GROUND_ROVER = 10, - UGV_SURFACE_SHIP = 11 -}; - -enum MAV_AUTOPILOT_TYPE -{ - MAV_AUTOPILOT_GENERIC = 0, - MAV_AUTOPILOT_PIXHAWK = 1, - MAV_AUTOPILOT_SLUGS = 2, - MAV_AUTOPILOT_ARDUPILOTMEGA = 3, - MAV_AUTOPILOT_NONE = 4 -}; - -enum MAV_COMPONENT -{ - MAV_COMP_ID_GPS, - MAV_COMP_ID_WAYPOINTPLANNER, - MAV_COMP_ID_BLOBTRACKER, - MAV_COMP_ID_PATHPLANNER, - MAV_COMP_ID_AIRSLAM, - MAV_COMP_ID_MAPPER, - MAV_COMP_ID_CAMERA, - MAV_COMP_ID_IMU = 200, - MAV_COMP_ID_IMU_2 = 201, - MAV_COMP_ID_IMU_3 = 202, - MAV_COMP_ID_UDP_BRIDGE = 240, - MAV_COMP_ID_UART_BRIDGE = 241, - MAV_COMP_ID_SYSTEM_CONTROL = 250 -}; - -enum MAV_FRAME -{ - MAV_FRAME_GLOBAL = 0, - MAV_FRAME_LOCAL = 1, - MAV_FRAME_MISSION = 2, - MAV_FRAME_GLOBAL_RELATIVE_ALT = 3, - MAV_FRAME_LOCAL_ENU = 4 -}; - -enum MAVLINK_DATA_STREAM_TYPE -{ - MAVLINK_DATA_STREAM_IMG_JPEG, - MAVLINK_DATA_STREAM_IMG_BMP, - MAVLINK_DATA_STREAM_IMG_RAW8U, - MAVLINK_DATA_STREAM_IMG_RAW32U, - MAVLINK_DATA_STREAM_IMG_PGM, - MAVLINK_DATA_STREAM_IMG_PNG - -}; - -#define MAVLINK_STX 0x55 ///< Packet start sign -#define MAVLINK_STX_LEN 1 ///< Length of start sign -#define MAVLINK_MAX_PAYLOAD_LEN 255 ///< Maximum payload length - -#define MAVLINK_CORE_HEADER_LEN 5 ///< Length of core header (of the comm. layer): message length (1 byte) + message sequence (1 byte) + message system id (1 byte) + message component id (1 byte) + message type id (1 byte) -#define MAVLINK_NUM_HEADER_BYTES (MAVLINK_CORE_HEADER_LEN + MAVLINK_STX_LEN) ///< Length of all header bytes, including core and checksum -#define MAVLINK_NUM_CHECKSUM_BYTES 2 -#define MAVLINK_NUM_NON_PAYLOAD_BYTES (MAVLINK_NUM_HEADER_BYTES + MAVLINK_NUM_CHECKSUM_BYTES) -#define MAVLINK_NUM_NON_STX_PAYLOAD_BYTES (MAVLINK_NUM_NON_PAYLOAD_BYTES - MAVLINK_STX_LEN) - -#define MAVLINK_MAX_PACKET_LEN (MAVLINK_MAX_PAYLOAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) ///< Maximum packet length -//#define MAVLINK_MAX_DATA_LEN MAVLINK_MAX_PACKET_LEN - MAVLINK_STX_LEN - -typedef struct __mavlink_system { - uint8_t sysid; ///< Used by the MAVLink message_xx_send() convenience function - uint8_t compid; ///< Used by the MAVLink message_xx_send() convenience function - uint8_t type; ///< Unused, can be used by user to store the system's type - uint8_t state; ///< Unused, can be used by user to store the system's state - uint8_t mode; ///< Unused, can be used by user to store the system's mode - uint8_t nav_mode; ///< Unused, can be used by user to store the system's navigation mode -} mavlink_system_t; - -typedef struct __mavlink_message { - uint8_t len; ///< Length of payload - uint8_t seq; ///< Sequence of packet - uint8_t sysid; ///< ID of message sender system/aircraft - uint8_t compid; ///< ID of the message sender component - uint8_t msgid; ///< ID of message in payload - uint8_t payload[MAVLINK_MAX_PAYLOAD_LEN]; ///< Payload data, ALIGNMENT IMPORTANT ON MCU - uint8_t ck_a; ///< Checksum high byte - uint8_t ck_b; ///< Checksum low byte -} mavlink_message_t; - -typedef enum { - MAVLINK_COMM_0, - MAVLINK_COMM_1, - MAVLINK_COMM_2, - MAVLINK_COMM_3 -} mavlink_channel_t; - -/* - * applications can set MAVLINK_COMM_NUM_BUFFERS to the maximum number - * of buffers they will use. If more are used, then the result will be - * a stack overrun - */ -#ifndef MAVLINK_COMM_NUM_BUFFERS -#if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32) -# define MAVLINK_COMM_NUM_BUFFERS 16 -#else -# define MAVLINK_COMM_NUM_BUFFERS 4 -#endif -#endif - -typedef enum { - MAVLINK_PARSE_STATE_UNINIT=0, - MAVLINK_PARSE_STATE_IDLE, - MAVLINK_PARSE_STATE_GOT_STX, - MAVLINK_PARSE_STATE_GOT_SEQ, - MAVLINK_PARSE_STATE_GOT_LENGTH, - MAVLINK_PARSE_STATE_GOT_SYSID, - MAVLINK_PARSE_STATE_GOT_COMPID, - MAVLINK_PARSE_STATE_GOT_MSGID, - MAVLINK_PARSE_STATE_GOT_PAYLOAD, - MAVLINK_PARSE_STATE_GOT_CRC1 -} mavlink_parse_state_t; ///< The state machine for the comm parser - -typedef struct __mavlink_status { - uint8_t ck_a; ///< Checksum byte 1 - uint8_t ck_b; ///< Checksum byte 2 - uint8_t msg_received; ///< Number of received messages - uint8_t buffer_overrun; ///< Number of buffer overruns - uint8_t parse_error; ///< Number of parse errors - mavlink_parse_state_t parse_state; ///< Parsing state machine - uint8_t packet_idx; ///< Index in current packet - uint8_t current_rx_seq; ///< Sequence number of last packet received - uint8_t current_tx_seq; ///< Sequence number of last packet sent - uint16_t packet_rx_success_count; ///< Received packets - uint16_t packet_rx_drop_count; ///< Number of packet drops -} mavlink_status_t; - -#endif /* MAVLINK_TYPES_H_ */ +/** @file + * @brief MAVLink comm protocol enumerations / structures / constants. + * @see http://qgroundcontrol.org/mavlink/ + * Edited on Monday, August 8 2011 + */ + +#ifndef MAVLINK_TYPES_H_ +#define MAVLINK_TYPES_H_ + +#include "inttypes.h" + +#define MAVLINK_STX 0xD5 ///< Packet start sign +#define MAVLINK_STX_LEN 1 ///< Length of start sign +#define MAVLINK_MAX_PAYLOAD_LEN 255 ///< Maximum payload length + +#define MAVLINK_CORE_HEADER_LEN 5 ///< Length of core header (of the comm. layer): message length (1 byte) + message sequence (1 byte) + message system id (1 byte) + message component id (1 byte) + message type id (1 byte) +#define MAVLINK_NUM_HEADER_BYTES (MAVLINK_CORE_HEADER_LEN + MAVLINK_STX_LEN) ///< Length of all header bytes, including core and checksum +#define MAVLINK_NUM_CHECKSUM_BYTES 2 +#define MAVLINK_NUM_NON_PAYLOAD_BYTES (MAVLINK_NUM_HEADER_BYTES + MAVLINK_NUM_CHECKSUM_BYTES) +#define MAVLINK_NUM_NON_STX_PAYLOAD_BYTES (MAVLINK_NUM_NON_PAYLOAD_BYTES - MAVLINK_STX_LEN) + +#define MAVLINK_MAX_PACKET_LEN (MAVLINK_MAX_PAYLOAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) ///< Maximum packet length +//#define MAVLINK_MAX_DATA_LEN MAVLINK_MAX_PACKET_LEN - MAVLINK_STX_LEN + +typedef struct __mavlink_system +{ + uint8_t sysid; ///< Used by the MAVLink message_xx_send() convenience function + uint8_t compid; ///< Used by the MAVLink message_xx_send() convenience function + uint8_t type; ///< Unused, can be used by user to store the system's type + uint8_t state; ///< Unused, can be used by user to store the system's state + uint8_t mode; ///< Unused, can be used by user to store the system's mode + uint8_t nav_mode; ///< Unused, can be used by user to store the system's navigation mode +} mavlink_system_t; + +typedef struct __mavlink_message +{ + union + { + uint16_t ck; ///< Checksum high byte + struct + { + uint8_t ck_a; ///< Checksum low byte + uint8_t ck_b; ///< Checksum high byte + }; + }; + uint8_t STX; ///< start of packet marker + uint8_t len; ///< Length of payload + uint8_t seq; ///< Sequence of packet + uint8_t sysid; ///< ID of message sender system/aircraft + uint8_t compid; ///< ID of the message sender component + uint8_t msgid; ///< ID of message in payload + uint8_t payload[MAVLINK_MAX_PAYLOAD_LEN]; ///< Payload data, ALIGNMENT IMPORTANT ON MCU +} mavlink_message_t; + +typedef struct __mavlink_header +{ + union { + uint16_t ck; ///< Checksum high byte + uint8_t ck_a; ///< Checksum low byte + uint8_t ck_b; ///< Checksum high byte + }; + uint8_t STX; ///< start of packet marker + uint8_t len; ///< Length of payload + uint8_t seq; ///< Sequence of packet + uint8_t sysid; ///< ID of message sender system/aircraft + uint8_t compid; ///< ID of the message sender component + uint8_t msgid; ///< ID of message in payload +} mavlink_header_t; + +typedef enum +{ + MAVLINK_COMM_0, + MAVLINK_COMM_1, + MAVLINK_COMM_2, + MAVLINK_COMM_3, + MAVLINK_COMM_NB, + MAVLINK_COMM_NB_HIGH = 16 +} mavlink_channel_t; + +typedef enum +{ + MAVLINK_PARSE_STATE_UNINIT=0, + MAVLINK_PARSE_STATE_IDLE, + MAVLINK_PARSE_STATE_GOT_STX, + MAVLINK_PARSE_STATE_GOT_SEQ, + MAVLINK_PARSE_STATE_GOT_LENGTH, + MAVLINK_PARSE_STATE_GOT_SYSID, + MAVLINK_PARSE_STATE_GOT_COMPID, + MAVLINK_PARSE_STATE_GOT_MSGID, + MAVLINK_PARSE_STATE_GOT_PAYLOAD, + MAVLINK_PARSE_STATE_GOT_CRC1 +} mavlink_parse_state_t; ///< The state machine for the comm parser + +typedef struct __mavlink_status +{ + uint8_t ck_a; ///< Checksum byte 1 + uint8_t ck_b; ///< Checksum byte 2 + uint8_t msg_received; ///< Number of received messages + uint8_t buffer_overrun; ///< Number of buffer overruns + uint8_t parse_error; ///< Number of parse errors + mavlink_parse_state_t parse_state; ///< Parsing state machine + uint8_t packet_idx; ///< Index in current packet + uint8_t current_rx_seq; ///< Sequence number of last packet received + uint8_t current_tx_seq; ///< Sequence number of last packet sent + uint16_t packet_rx_success_count; ///< Received packets + uint16_t packet_rx_drop_count; ///< Number of packet drops +} mavlink_status_t; + +#endif /* MAVLINK_TYPES_H_ */ diff --git a/thirdParty/mavlink/include/minimal/mavlink.h b/thirdParty/mavlink/include/minimal/mavlink.h index 45a5e9566a8f66618fc3b88cc4469b032ebabc7b..c8cd1940e27e8ed6ae4a1da255b3590e1edb9ae0 100644 --- a/thirdParty/mavlink/include/minimal/mavlink.h +++ b/thirdParty/mavlink/include/minimal/mavlink.h @@ -1,11 +1,16 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Wednesday, July 27 2011, 14:17 UTC + * Generated on Tuesday, August 9 2011, 16:16 UTC */ #ifndef MAVLINK_H #define MAVLINK_H +#pragma pack(push,1) +#include "mavlink_options.h" #include "minimal.h" - +#ifdef MAVLINK_DATA +#include "mavlink_data.h" +#endif +#pragma pack(pop) #endif diff --git a/thirdParty/mavlink/include/minimal/mavlink_msg_heartbeat.h b/thirdParty/mavlink/include/minimal/mavlink_msg_heartbeat.h index 0e5c4db5ca00fb627f2c5072cd8b2d1d7cbd02ad..7c686831eee88b7e0cb4e92e8ffd8f771c7a51ae 100644 --- a/thirdParty/mavlink/include/minimal/mavlink_msg_heartbeat.h +++ b/thirdParty/mavlink/include/minimal/mavlink_msg_heartbeat.h @@ -1,6 +1,8 @@ // MESSAGE HEARTBEAT PACKING #define MAVLINK_MSG_ID_HEARTBEAT 0 +#define MAVLINK_MSG_ID_HEARTBEAT_LEN 3 +#define MAVLINK_MSG_0_LEN 3 typedef struct __mavlink_heartbeat_t { @@ -10,8 +12,6 @@ typedef struct __mavlink_heartbeat_t } mavlink_heartbeat_t; - - /** * @brief Pack a heartbeat message * @param system_id ID of this system @@ -24,14 +24,14 @@ typedef struct __mavlink_heartbeat_t */ static inline uint16_t mavlink_msg_heartbeat_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t type, uint8_t autopilot) { - uint16_t i = 0; + mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_HEARTBEAT; - i += put_uint8_t_by_index(type, i, msg->payload); // Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) - i += put_uint8_t_by_index(autopilot, i, msg->payload); // Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM - i += put_uint8_t_by_index(2, i, msg->payload); // MAVLink version + p->type = type; // uint8_t:Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + p->autopilot = autopilot; // uint8_t:Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM - return mavlink_finalize_message(msg, system_id, component_id, i); + p->mavlink_version = MAVLINK_VERSION; // uint8_t_mavlink_version:MAVLink version + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HEARTBEAT_LEN); } /** @@ -46,14 +46,14 @@ static inline uint16_t mavlink_msg_heartbeat_pack(uint8_t system_id, uint8_t com */ static inline uint16_t mavlink_msg_heartbeat_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t type, uint8_t autopilot) { - uint16_t i = 0; + mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_HEARTBEAT; - i += put_uint8_t_by_index(type, i, msg->payload); // Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) - i += put_uint8_t_by_index(autopilot, i, msg->payload); // Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM - i += put_uint8_t_by_index(2, i, msg->payload); // MAVLink version + p->type = type; // uint8_t:Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + p->autopilot = autopilot; // uint8_t:Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + p->mavlink_version = MAVLINK_VERSION; // uint8_t_mavlink_version:MAVLink version + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HEARTBEAT_LEN); } /** @@ -76,13 +76,37 @@ static inline uint16_t mavlink_msg_heartbeat_encode(uint8_t system_id, uint8_t c * @param type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) * @param autopilot Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t type, uint8_t autopilot) { - mavlink_message_t msg; - mavlink_msg_heartbeat_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, type, autopilot); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_heartbeat_t payload; + uint16_t checksum; + mavlink_heartbeat_t *p = &payload; + + p->type = type; // uint8_t:Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + p->autopilot = autopilot; // uint8_t:Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM + + p->mavlink_version = MAVLINK_VERSION; // uint8_t_mavlink_version:MAVLink version + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_HEARTBEAT_LEN; + hdr.msgid = MAVLINK_MSG_ID_HEARTBEAT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -95,7 +119,8 @@ static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t ty */ static inline uint8_t mavlink_msg_heartbeat_get_type(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg->payload[0]; + return (uint8_t)(p->type); } /** @@ -105,7 +130,8 @@ static inline uint8_t mavlink_msg_heartbeat_get_type(const mavlink_message_t* ms */ static inline uint8_t mavlink_msg_heartbeat_get_autopilot(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg->payload[0]; + return (uint8_t)(p->autopilot); } /** @@ -115,7 +141,8 @@ static inline uint8_t mavlink_msg_heartbeat_get_autopilot(const mavlink_message_ */ static inline uint8_t mavlink_msg_heartbeat_get_mavlink_version(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg->payload[0]; + return (uint8_t)(p->mavlink_version); } /** @@ -126,7 +153,5 @@ static inline uint8_t mavlink_msg_heartbeat_get_mavlink_version(const mavlink_me */ static inline void mavlink_msg_heartbeat_decode(const mavlink_message_t* msg, mavlink_heartbeat_t* heartbeat) { - heartbeat->type = mavlink_msg_heartbeat_get_type(msg); - heartbeat->autopilot = mavlink_msg_heartbeat_get_autopilot(msg); - heartbeat->mavlink_version = mavlink_msg_heartbeat_get_mavlink_version(msg); + memcpy( heartbeat, msg->payload, sizeof(mavlink_heartbeat_t)); } diff --git a/thirdParty/mavlink/include/minimal/minimal.h b/thirdParty/mavlink/include/minimal/minimal.h index 61cd3fe22be39ed8f8bdaaf855398f3d7cce25d2..c3d7e2a1bf9a894711febe2ae471dc87b2389449 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 Tuesday, August 9 2011, 16:16 UTC */ #ifndef MINIMAL_H #define MINIMAL_H @@ -34,10 +34,10 @@ extern "C" { #include "./mavlink_msg_heartbeat.h" -// MESSAGE LENGTHS +// MESSAGE CRC KEYS -#undef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS { } +#undef MAVLINK_MESSAGE_KEYS +#define MAVLINK_MESSAGE_KEYS { } #ifdef __cplusplus } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink.h b/thirdParty/mavlink/include/pixhawk/mavlink.h index 16a37599e1fa218650bf065dbd66ec620892162c..b14edfc4d057db2186964ff76d9ed44e34f149c0 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink.h @@ -1,11 +1,16 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Wednesday, July 27 2011, 14:17 UTC + * Generated on Tuesday, August 9 2011, 16:49 UTC */ #ifndef MAVLINK_H #define MAVLINK_H +#pragma pack(push,1) +#include "mavlink_options.h" #include "pixhawk.h" - +#ifdef MAVLINK_DATA +#include "mavlink_data.h" +#endif +#pragma pack(pop) #endif diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_attitude_control.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_attitude_control.h index 1a8b806f8ab2983c374e2f89f944df9772d48226..fd8503d974ee85d912fd0fe8f19e34824f2efd1c 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_attitude_control.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_attitude_control.h @@ -1,14 +1,16 @@ // MESSAGE ATTITUDE_CONTROL PACKING #define MAVLINK_MSG_ID_ATTITUDE_CONTROL 85 +#define MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN 21 +#define MAVLINK_MSG_85_LEN 21 typedef struct __mavlink_attitude_control_t { - uint8_t target; ///< The system to be controlled float roll; ///< roll float pitch; ///< pitch float yaw; ///< yaw float thrust; ///< thrust + uint8_t target; ///< The system to be controlled uint8_t roll_manual; ///< roll control enabled auto:0, manual:1 uint8_t pitch_manual; ///< pitch auto:0, manual:1 uint8_t yaw_manual; ///< yaw auto:0, manual:1 @@ -16,8 +18,6 @@ typedef struct __mavlink_attitude_control_t } mavlink_attitude_control_t; - - /** * @brief Pack a attitude_control message * @param system_id ID of this system @@ -37,20 +37,20 @@ typedef struct __mavlink_attitude_control_t */ static inline uint16_t mavlink_msg_attitude_control_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target, float roll, float pitch, float yaw, float thrust, uint8_t roll_manual, uint8_t pitch_manual, uint8_t yaw_manual, uint8_t thrust_manual) { - uint16_t i = 0; + mavlink_attitude_control_t *p = (mavlink_attitude_control_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_ATTITUDE_CONTROL; - i += put_uint8_t_by_index(target, i, msg->payload); // The system to be controlled - i += put_float_by_index(roll, i, msg->payload); // roll - i += put_float_by_index(pitch, i, msg->payload); // pitch - i += put_float_by_index(yaw, i, msg->payload); // yaw - i += put_float_by_index(thrust, i, msg->payload); // thrust - i += put_uint8_t_by_index(roll_manual, i, msg->payload); // roll control enabled auto:0, manual:1 - i += put_uint8_t_by_index(pitch_manual, i, msg->payload); // pitch auto:0, manual:1 - i += put_uint8_t_by_index(yaw_manual, i, msg->payload); // yaw auto:0, manual:1 - i += put_uint8_t_by_index(thrust_manual, i, msg->payload); // thrust auto:0, manual:1 + p->target = target; // uint8_t:The system to be controlled + p->roll = roll; // float:roll + p->pitch = pitch; // float:pitch + p->yaw = yaw; // float:yaw + p->thrust = thrust; // float:thrust + p->roll_manual = roll_manual; // uint8_t:roll control enabled auto:0, manual:1 + p->pitch_manual = pitch_manual; // uint8_t:pitch auto:0, manual:1 + p->yaw_manual = yaw_manual; // uint8_t:yaw auto:0, manual:1 + p->thrust_manual = thrust_manual; // uint8_t:thrust auto:0, manual:1 - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN); } /** @@ -72,20 +72,20 @@ static inline uint16_t mavlink_msg_attitude_control_pack(uint8_t system_id, uint */ static inline uint16_t mavlink_msg_attitude_control_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target, float roll, float pitch, float yaw, float thrust, uint8_t roll_manual, uint8_t pitch_manual, uint8_t yaw_manual, uint8_t thrust_manual) { - uint16_t i = 0; + mavlink_attitude_control_t *p = (mavlink_attitude_control_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_ATTITUDE_CONTROL; - i += put_uint8_t_by_index(target, i, msg->payload); // The system to be controlled - i += put_float_by_index(roll, i, msg->payload); // roll - i += put_float_by_index(pitch, i, msg->payload); // pitch - i += put_float_by_index(yaw, i, msg->payload); // yaw - i += put_float_by_index(thrust, i, msg->payload); // thrust - i += put_uint8_t_by_index(roll_manual, i, msg->payload); // roll control enabled auto:0, manual:1 - i += put_uint8_t_by_index(pitch_manual, i, msg->payload); // pitch auto:0, manual:1 - i += put_uint8_t_by_index(yaw_manual, i, msg->payload); // yaw auto:0, manual:1 - i += put_uint8_t_by_index(thrust_manual, i, msg->payload); // thrust auto:0, manual:1 + p->target = target; // uint8_t:The system to be controlled + p->roll = roll; // float:roll + p->pitch = pitch; // float:pitch + p->yaw = yaw; // float:yaw + p->thrust = thrust; // float:thrust + p->roll_manual = roll_manual; // uint8_t:roll control enabled auto:0, manual:1 + p->pitch_manual = pitch_manual; // uint8_t:pitch auto:0, manual:1 + p->yaw_manual = yaw_manual; // uint8_t:yaw auto:0, manual:1 + p->thrust_manual = thrust_manual; // uint8_t:thrust auto:0, manual:1 - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN); } /** @@ -115,13 +115,43 @@ static inline uint16_t mavlink_msg_attitude_control_encode(uint8_t system_id, ui * @param yaw_manual yaw auto:0, manual:1 * @param thrust_manual thrust auto:0, manual:1 */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_attitude_control_send(mavlink_channel_t chan, uint8_t target, float roll, float pitch, float yaw, float thrust, uint8_t roll_manual, uint8_t pitch_manual, uint8_t yaw_manual, uint8_t thrust_manual) { - mavlink_message_t msg; - mavlink_msg_attitude_control_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target, roll, pitch, yaw, thrust, roll_manual, pitch_manual, yaw_manual, thrust_manual); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_attitude_control_t payload; + uint16_t checksum; + mavlink_attitude_control_t *p = &payload; + + p->target = target; // uint8_t:The system to be controlled + p->roll = roll; // float:roll + p->pitch = pitch; // float:pitch + p->yaw = yaw; // float:yaw + p->thrust = thrust; // float:thrust + p->roll_manual = roll_manual; // uint8_t:roll control enabled auto:0, manual:1 + p->pitch_manual = pitch_manual; // uint8_t:pitch auto:0, manual:1 + p->yaw_manual = yaw_manual; // uint8_t:yaw auto:0, manual:1 + p->thrust_manual = thrust_manual; // uint8_t:thrust auto:0, manual:1 + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN; + hdr.msgid = MAVLINK_MSG_ID_ATTITUDE_CONTROL; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -134,7 +164,8 @@ static inline void mavlink_msg_attitude_control_send(mavlink_channel_t chan, uin */ static inline uint8_t mavlink_msg_attitude_control_get_target(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_attitude_control_t *p = (mavlink_attitude_control_t *)&msg->payload[0]; + return (uint8_t)(p->target); } /** @@ -144,12 +175,8 @@ static inline uint8_t mavlink_msg_attitude_control_get_target(const mavlink_mess */ static inline float mavlink_msg_attitude_control_get_roll(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t))[3]; - return (float)r.f; + mavlink_attitude_control_t *p = (mavlink_attitude_control_t *)&msg->payload[0]; + return (float)(p->roll); } /** @@ -159,12 +186,8 @@ static inline float mavlink_msg_attitude_control_get_roll(const mavlink_message_ */ static inline float mavlink_msg_attitude_control_get_pitch(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(float))[3]; - return (float)r.f; + mavlink_attitude_control_t *p = (mavlink_attitude_control_t *)&msg->payload[0]; + return (float)(p->pitch); } /** @@ -174,12 +197,8 @@ static inline float mavlink_msg_attitude_control_get_pitch(const mavlink_message */ static inline float mavlink_msg_attitude_control_get_yaw(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_attitude_control_t *p = (mavlink_attitude_control_t *)&msg->payload[0]; + return (float)(p->yaw); } /** @@ -189,12 +208,8 @@ static inline float mavlink_msg_attitude_control_get_yaw(const mavlink_message_t */ static inline float mavlink_msg_attitude_control_get_thrust(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_attitude_control_t *p = (mavlink_attitude_control_t *)&msg->payload[0]; + return (float)(p->thrust); } /** @@ -204,7 +219,8 @@ static inline float mavlink_msg_attitude_control_get_thrust(const mavlink_messag */ static inline uint8_t mavlink_msg_attitude_control_get_roll_manual(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; + mavlink_attitude_control_t *p = (mavlink_attitude_control_t *)&msg->payload[0]; + return (uint8_t)(p->roll_manual); } /** @@ -214,7 +230,8 @@ static inline uint8_t mavlink_msg_attitude_control_get_roll_manual(const mavlink */ static inline uint8_t mavlink_msg_attitude_control_get_pitch_manual(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t))[0]; + mavlink_attitude_control_t *p = (mavlink_attitude_control_t *)&msg->payload[0]; + return (uint8_t)(p->pitch_manual); } /** @@ -224,7 +241,8 @@ static inline uint8_t mavlink_msg_attitude_control_get_pitch_manual(const mavlin */ static inline uint8_t mavlink_msg_attitude_control_get_yaw_manual(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_attitude_control_t *p = (mavlink_attitude_control_t *)&msg->payload[0]; + return (uint8_t)(p->yaw_manual); } /** @@ -234,7 +252,8 @@ static inline uint8_t mavlink_msg_attitude_control_get_yaw_manual(const mavlink_ */ static inline uint8_t mavlink_msg_attitude_control_get_thrust_manual(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_attitude_control_t *p = (mavlink_attitude_control_t *)&msg->payload[0]; + return (uint8_t)(p->thrust_manual); } /** @@ -245,13 +264,5 @@ static inline uint8_t mavlink_msg_attitude_control_get_thrust_manual(const mavli */ static inline void mavlink_msg_attitude_control_decode(const mavlink_message_t* msg, mavlink_attitude_control_t* attitude_control) { - attitude_control->target = mavlink_msg_attitude_control_get_target(msg); - attitude_control->roll = mavlink_msg_attitude_control_get_roll(msg); - attitude_control->pitch = mavlink_msg_attitude_control_get_pitch(msg); - attitude_control->yaw = mavlink_msg_attitude_control_get_yaw(msg); - attitude_control->thrust = mavlink_msg_attitude_control_get_thrust(msg); - attitude_control->roll_manual = mavlink_msg_attitude_control_get_roll_manual(msg); - attitude_control->pitch_manual = mavlink_msg_attitude_control_get_pitch_manual(msg); - attitude_control->yaw_manual = mavlink_msg_attitude_control_get_yaw_manual(msg); - attitude_control->thrust_manual = mavlink_msg_attitude_control_get_thrust_manual(msg); + memcpy( attitude_control, msg->payload, sizeof(mavlink_attitude_control_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_aux_status.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_aux_status.h index f9dbde8c86ca4350705a5d8dfe9cec05c7240571..2fb3314c931a70bcfd1bf23f620e39bf89658d60 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_aux_status.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_aux_status.h @@ -1,6 +1,8 @@ // MESSAGE AUX_STATUS PACKING #define MAVLINK_MSG_ID_AUX_STATUS 142 +#define MAVLINK_MSG_ID_AUX_STATUS_LEN 12 +#define MAVLINK_MSG_142_LEN 12 typedef struct __mavlink_aux_status_t { @@ -13,8 +15,6 @@ typedef struct __mavlink_aux_status_t } mavlink_aux_status_t; - - /** * @brief Pack a aux_status message * @param system_id ID of this system @@ -31,17 +31,17 @@ typedef struct __mavlink_aux_status_t */ static inline uint16_t mavlink_msg_aux_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint16_t load, uint16_t i2c0_err_count, uint16_t i2c1_err_count, uint16_t spi0_err_count, uint16_t spi1_err_count, uint16_t uart_total_err_count) { - uint16_t i = 0; + mavlink_aux_status_t *p = (mavlink_aux_status_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_AUX_STATUS; - i += put_uint16_t_by_index(load, i, msg->payload); // Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - i += put_uint16_t_by_index(i2c0_err_count, i, msg->payload); // Number of I2C errors since startup - i += put_uint16_t_by_index(i2c1_err_count, i, msg->payload); // Number of I2C errors since startup - i += put_uint16_t_by_index(spi0_err_count, i, msg->payload); // Number of I2C errors since startup - i += put_uint16_t_by_index(spi1_err_count, i, msg->payload); // Number of I2C errors since startup - i += put_uint16_t_by_index(uart_total_err_count, i, msg->payload); // Number of I2C errors since startup + p->load = load; // uint16_t:Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + p->i2c0_err_count = i2c0_err_count; // uint16_t:Number of I2C errors since startup + p->i2c1_err_count = i2c1_err_count; // uint16_t:Number of I2C errors since startup + p->spi0_err_count = spi0_err_count; // uint16_t:Number of I2C errors since startup + p->spi1_err_count = spi1_err_count; // uint16_t:Number of I2C errors since startup + p->uart_total_err_count = uart_total_err_count; // uint16_t:Number of I2C errors since startup - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AUX_STATUS_LEN); } /** @@ -60,17 +60,17 @@ static inline uint16_t mavlink_msg_aux_status_pack(uint8_t system_id, uint8_t co */ static inline uint16_t mavlink_msg_aux_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint16_t load, uint16_t i2c0_err_count, uint16_t i2c1_err_count, uint16_t spi0_err_count, uint16_t spi1_err_count, uint16_t uart_total_err_count) { - uint16_t i = 0; + mavlink_aux_status_t *p = (mavlink_aux_status_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_AUX_STATUS; - i += put_uint16_t_by_index(load, i, msg->payload); // Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - i += put_uint16_t_by_index(i2c0_err_count, i, msg->payload); // Number of I2C errors since startup - i += put_uint16_t_by_index(i2c1_err_count, i, msg->payload); // Number of I2C errors since startup - i += put_uint16_t_by_index(spi0_err_count, i, msg->payload); // Number of I2C errors since startup - i += put_uint16_t_by_index(spi1_err_count, i, msg->payload); // Number of I2C errors since startup - i += put_uint16_t_by_index(uart_total_err_count, i, msg->payload); // Number of I2C errors since startup + p->load = load; // uint16_t:Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + p->i2c0_err_count = i2c0_err_count; // uint16_t:Number of I2C errors since startup + p->i2c1_err_count = i2c1_err_count; // uint16_t:Number of I2C errors since startup + p->spi0_err_count = spi0_err_count; // uint16_t:Number of I2C errors since startup + p->spi1_err_count = spi1_err_count; // uint16_t:Number of I2C errors since startup + p->uart_total_err_count = uart_total_err_count; // uint16_t:Number of I2C errors since startup - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AUX_STATUS_LEN); } /** @@ -97,13 +97,40 @@ static inline uint16_t mavlink_msg_aux_status_encode(uint8_t system_id, uint8_t * @param spi1_err_count Number of I2C errors since startup * @param uart_total_err_count Number of I2C errors since startup */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_aux_status_send(mavlink_channel_t chan, uint16_t load, uint16_t i2c0_err_count, uint16_t i2c1_err_count, uint16_t spi0_err_count, uint16_t spi1_err_count, uint16_t uart_total_err_count) { - mavlink_message_t msg; - mavlink_msg_aux_status_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, load, i2c0_err_count, i2c1_err_count, spi0_err_count, spi1_err_count, uart_total_err_count); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_aux_status_t payload; + uint16_t checksum; + mavlink_aux_status_t *p = &payload; + + p->load = load; // uint16_t:Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + p->i2c0_err_count = i2c0_err_count; // uint16_t:Number of I2C errors since startup + p->i2c1_err_count = i2c1_err_count; // uint16_t:Number of I2C errors since startup + p->spi0_err_count = spi0_err_count; // uint16_t:Number of I2C errors since startup + p->spi1_err_count = spi1_err_count; // uint16_t:Number of I2C errors since startup + p->uart_total_err_count = uart_total_err_count; // uint16_t:Number of I2C errors since startup + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_AUX_STATUS_LEN; + hdr.msgid = MAVLINK_MSG_ID_AUX_STATUS; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -116,10 +143,8 @@ static inline void mavlink_msg_aux_status_send(mavlink_channel_t chan, uint16_t */ static inline uint16_t mavlink_msg_aux_status_get_load(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload)[0]; - r.b[0] = (msg->payload)[1]; - return (uint16_t)r.s; + mavlink_aux_status_t *p = (mavlink_aux_status_t *)&msg->payload[0]; + return (uint16_t)(p->load); } /** @@ -129,10 +154,8 @@ static inline uint16_t mavlink_msg_aux_status_get_load(const mavlink_message_t* */ static inline uint16_t mavlink_msg_aux_status_get_i2c0_err_count(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_aux_status_t *p = (mavlink_aux_status_t *)&msg->payload[0]; + return (uint16_t)(p->i2c0_err_count); } /** @@ -142,10 +165,8 @@ static inline uint16_t mavlink_msg_aux_status_get_i2c0_err_count(const mavlink_m */ static inline uint16_t mavlink_msg_aux_status_get_i2c1_err_count(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_aux_status_t *p = (mavlink_aux_status_t *)&msg->payload[0]; + return (uint16_t)(p->i2c1_err_count); } /** @@ -155,10 +176,8 @@ static inline uint16_t mavlink_msg_aux_status_get_i2c1_err_count(const mavlink_m */ static inline uint16_t mavlink_msg_aux_status_get_spi0_err_count(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_aux_status_t *p = (mavlink_aux_status_t *)&msg->payload[0]; + return (uint16_t)(p->spi0_err_count); } /** @@ -168,10 +187,8 @@ static inline uint16_t mavlink_msg_aux_status_get_spi0_err_count(const mavlink_m */ static inline uint16_t mavlink_msg_aux_status_get_spi1_err_count(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_aux_status_t *p = (mavlink_aux_status_t *)&msg->payload[0]; + return (uint16_t)(p->spi1_err_count); } /** @@ -181,10 +198,8 @@ static inline uint16_t mavlink_msg_aux_status_get_spi1_err_count(const mavlink_m */ static inline uint16_t mavlink_msg_aux_status_get_uart_total_err_count(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_aux_status_t *p = (mavlink_aux_status_t *)&msg->payload[0]; + return (uint16_t)(p->uart_total_err_count); } /** @@ -195,10 +210,5 @@ static inline uint16_t mavlink_msg_aux_status_get_uart_total_err_count(const mav */ static inline void mavlink_msg_aux_status_decode(const mavlink_message_t* msg, mavlink_aux_status_t* aux_status) { - aux_status->load = mavlink_msg_aux_status_get_load(msg); - aux_status->i2c0_err_count = mavlink_msg_aux_status_get_i2c0_err_count(msg); - aux_status->i2c1_err_count = mavlink_msg_aux_status_get_i2c1_err_count(msg); - aux_status->spi0_err_count = mavlink_msg_aux_status_get_spi0_err_count(msg); - aux_status->spi1_err_count = mavlink_msg_aux_status_get_spi1_err_count(msg); - aux_status->uart_total_err_count = mavlink_msg_aux_status_get_uart_total_err_count(msg); + memcpy( aux_status, msg->payload, sizeof(mavlink_aux_status_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_brief_feature.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_brief_feature.h index a61e13bcde2c02a4b79449affb839956a6baf5bf..4e6f6b83bc2ae7e9c0f580ea14f9e677e8b5b53a 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_brief_feature.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_brief_feature.h @@ -1,23 +1,23 @@ // MESSAGE BRIEF_FEATURE PACKING #define MAVLINK_MSG_ID_BRIEF_FEATURE 172 +#define MAVLINK_MSG_ID_BRIEF_FEATURE_LEN 53 +#define MAVLINK_MSG_172_LEN 53 typedef struct __mavlink_brief_feature_t { float x; ///< x position in m float y; ///< y position in m float z; ///< z position in m - uint8_t orientation_assignment; ///< Orientation assignment 0: false, 1:true + float response; ///< Harris operator response at this location uint16_t size; ///< Size in pixels uint16_t orientation; ///< Orientation + uint8_t orientation_assignment; ///< Orientation assignment 0: false, 1:true uint8_t descriptor[32]; ///< Descriptor - float response; ///< Harris operator response at this location } mavlink_brief_feature_t; - #define MAVLINK_MSG_BRIEF_FEATURE_FIELD_DESCRIPTOR_LEN 32 - /** * @brief Pack a brief_feature message * @param system_id ID of this system @@ -36,19 +36,19 @@ typedef struct __mavlink_brief_feature_t */ static inline uint16_t mavlink_msg_brief_feature_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, float x, float y, float z, uint8_t orientation_assignment, uint16_t size, uint16_t orientation, const uint8_t* descriptor, float response) { - uint16_t i = 0; + mavlink_brief_feature_t *p = (mavlink_brief_feature_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_BRIEF_FEATURE; - i += put_float_by_index(x, i, msg->payload); // x position in m - i += put_float_by_index(y, i, msg->payload); // y position in m - i += put_float_by_index(z, i, msg->payload); // z position in m - i += put_uint8_t_by_index(orientation_assignment, i, msg->payload); // Orientation assignment 0: false, 1:true - i += put_uint16_t_by_index(size, i, msg->payload); // Size in pixels - i += put_uint16_t_by_index(orientation, i, msg->payload); // Orientation - i += put_array_by_index((const int8_t*)descriptor, sizeof(uint8_t)*32, i, msg->payload); // Descriptor - i += put_float_by_index(response, i, msg->payload); // Harris operator response at this location + p->x = x; // float:x position in m + p->y = y; // float:y position in m + p->z = z; // float:z position in m + p->orientation_assignment = orientation_assignment; // uint8_t:Orientation assignment 0: false, 1:true + p->size = size; // uint16_t:Size in pixels + p->orientation = orientation; // uint16_t:Orientation + memcpy(p->descriptor, descriptor, sizeof(p->descriptor)); // uint8_t[32]:Descriptor + p->response = response; // float:Harris operator response at this location - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN); } /** @@ -69,19 +69,19 @@ static inline uint16_t mavlink_msg_brief_feature_pack(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_brief_feature_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, float x, float y, float z, uint8_t orientation_assignment, uint16_t size, uint16_t orientation, const uint8_t* descriptor, float response) { - uint16_t i = 0; + mavlink_brief_feature_t *p = (mavlink_brief_feature_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_BRIEF_FEATURE; - i += put_float_by_index(x, i, msg->payload); // x position in m - i += put_float_by_index(y, i, msg->payload); // y position in m - i += put_float_by_index(z, i, msg->payload); // z position in m - i += put_uint8_t_by_index(orientation_assignment, i, msg->payload); // Orientation assignment 0: false, 1:true - i += put_uint16_t_by_index(size, i, msg->payload); // Size in pixels - i += put_uint16_t_by_index(orientation, i, msg->payload); // Orientation - i += put_array_by_index((const int8_t*)descriptor, sizeof(uint8_t)*32, i, msg->payload); // Descriptor - i += put_float_by_index(response, i, msg->payload); // Harris operator response at this location + p->x = x; // float:x position in m + p->y = y; // float:y position in m + p->z = z; // float:z position in m + p->orientation_assignment = orientation_assignment; // uint8_t:Orientation assignment 0: false, 1:true + p->size = size; // uint16_t:Size in pixels + p->orientation = orientation; // uint16_t:Orientation + memcpy(p->descriptor, descriptor, sizeof(p->descriptor)); // uint8_t[32]:Descriptor + p->response = response; // float:Harris operator response at this location - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN); } /** @@ -110,13 +110,42 @@ static inline uint16_t mavlink_msg_brief_feature_encode(uint8_t system_id, uint8 * @param descriptor Descriptor * @param response Harris operator response at this location */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_brief_feature_send(mavlink_channel_t chan, float x, float y, float z, uint8_t orientation_assignment, uint16_t size, uint16_t orientation, const uint8_t* descriptor, float response) { - mavlink_message_t msg; - mavlink_msg_brief_feature_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, x, y, z, orientation_assignment, size, orientation, descriptor, response); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_brief_feature_t payload; + uint16_t checksum; + mavlink_brief_feature_t *p = &payload; + + p->x = x; // float:x position in m + p->y = y; // float:y position in m + p->z = z; // float:z position in m + p->orientation_assignment = orientation_assignment; // uint8_t:Orientation assignment 0: false, 1:true + p->size = size; // uint16_t:Size in pixels + p->orientation = orientation; // uint16_t:Orientation + memcpy(p->descriptor, descriptor, sizeof(p->descriptor)); // uint8_t[32]:Descriptor + p->response = response; // float:Harris operator response at this location + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_BRIEF_FEATURE_LEN; + hdr.msgid = MAVLINK_MSG_ID_BRIEF_FEATURE; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -129,12 +158,8 @@ static inline void mavlink_msg_brief_feature_send(mavlink_channel_t chan, float */ static inline float mavlink_msg_brief_feature_get_x(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 (float)r.f; + mavlink_brief_feature_t *p = (mavlink_brief_feature_t *)&msg->payload[0]; + return (float)(p->x); } /** @@ -144,12 +169,8 @@ static inline float mavlink_msg_brief_feature_get_x(const mavlink_message_t* msg */ static inline float mavlink_msg_brief_feature_get_y(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float))[3]; - return (float)r.f; + mavlink_brief_feature_t *p = (mavlink_brief_feature_t *)&msg->payload[0]; + return (float)(p->y); } /** @@ -159,12 +180,8 @@ static inline float mavlink_msg_brief_feature_get_y(const mavlink_message_t* msg */ static inline float mavlink_msg_brief_feature_get_z(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_brief_feature_t *p = (mavlink_brief_feature_t *)&msg->payload[0]; + return (float)(p->z); } /** @@ -174,7 +191,8 @@ static inline float mavlink_msg_brief_feature_get_z(const mavlink_message_t* msg */ static inline uint8_t mavlink_msg_brief_feature_get_orientation_assignment(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[0]; + mavlink_brief_feature_t *p = (mavlink_brief_feature_t *)&msg->payload[0]; + return (uint8_t)(p->orientation_assignment); } /** @@ -184,10 +202,8 @@ static inline uint8_t mavlink_msg_brief_feature_get_orientation_assignment(const */ static inline uint16_t mavlink_msg_brief_feature_get_size(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t))[0]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t))[1]; - return (uint16_t)r.s; + mavlink_brief_feature_t *p = (mavlink_brief_feature_t *)&msg->payload[0]; + return (uint16_t)(p->size); } /** @@ -197,10 +213,8 @@ static inline uint16_t mavlink_msg_brief_feature_get_size(const mavlink_message_ */ static inline uint16_t mavlink_msg_brief_feature_get_orientation(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_brief_feature_t *p = (mavlink_brief_feature_t *)&msg->payload[0]; + return (uint16_t)(p->orientation); } /** @@ -208,11 +222,12 @@ static inline uint16_t mavlink_msg_brief_feature_get_orientation(const mavlink_m * * @return Descriptor */ -static inline uint16_t mavlink_msg_brief_feature_get_descriptor(const mavlink_message_t* msg, uint8_t* r_data) +static inline uint16_t mavlink_msg_brief_feature_get_descriptor(const mavlink_message_t* msg, uint8_t* descriptor) { + mavlink_brief_feature_t *p = (mavlink_brief_feature_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t), sizeof(uint8_t)*32); - return sizeof(uint8_t)*32; + memcpy(descriptor, p->descriptor, sizeof(p->descriptor)); + return sizeof(p->descriptor); } /** @@ -222,12 +237,8 @@ static inline uint16_t mavlink_msg_brief_feature_get_descriptor(const mavlink_me */ static inline float mavlink_msg_brief_feature_get_response(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)*32)[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)*32)[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)*32)[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)*32)[3]; - return (float)r.f; + mavlink_brief_feature_t *p = (mavlink_brief_feature_t *)&msg->payload[0]; + return (float)(p->response); } /** @@ -238,12 +249,5 @@ static inline float mavlink_msg_brief_feature_get_response(const mavlink_message */ static inline void mavlink_msg_brief_feature_decode(const mavlink_message_t* msg, mavlink_brief_feature_t* brief_feature) { - brief_feature->x = mavlink_msg_brief_feature_get_x(msg); - brief_feature->y = mavlink_msg_brief_feature_get_y(msg); - brief_feature->z = mavlink_msg_brief_feature_get_z(msg); - brief_feature->orientation_assignment = mavlink_msg_brief_feature_get_orientation_assignment(msg); - brief_feature->size = mavlink_msg_brief_feature_get_size(msg); - brief_feature->orientation = mavlink_msg_brief_feature_get_orientation(msg); - mavlink_msg_brief_feature_get_descriptor(msg, brief_feature->descriptor); - brief_feature->response = mavlink_msg_brief_feature_get_response(msg); + memcpy( brief_feature, msg->payload, sizeof(mavlink_brief_feature_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_data_transmission_handshake.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_data_transmission_handshake.h index e9e69f7c9f53319f4c5c4973ebb80e65f3e72750..54d2070a987464f289a4fb0551d7c743588a1dfe 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_data_transmission_handshake.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_data_transmission_handshake.h @@ -1,19 +1,19 @@ // MESSAGE DATA_TRANSMISSION_HANDSHAKE PACKING #define MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE 170 +#define MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN 8 +#define MAVLINK_MSG_170_LEN 8 typedef struct __mavlink_data_transmission_handshake_t { - uint8_t type; ///< type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) uint32_t size; ///< total data size in bytes (set on ACK only) + uint8_t type; ///< type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) uint8_t packets; ///< number of packets beeing sent (set on ACK only) uint8_t payload; ///< payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) uint8_t jpg_quality; ///< JPEG quality out of [1,100] } mavlink_data_transmission_handshake_t; - - /** * @brief Pack a data_transmission_handshake message * @param system_id ID of this system @@ -29,16 +29,16 @@ typedef struct __mavlink_data_transmission_handshake_t */ static inline uint16_t mavlink_msg_data_transmission_handshake_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t type, uint32_t size, uint8_t packets, uint8_t payload, uint8_t jpg_quality) { - uint16_t i = 0; + mavlink_data_transmission_handshake_t *p = (mavlink_data_transmission_handshake_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE; - i += put_uint8_t_by_index(type, i, msg->payload); // type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) - i += put_uint32_t_by_index(size, i, msg->payload); // total data size in bytes (set on ACK only) - i += put_uint8_t_by_index(packets, i, msg->payload); // number of packets beeing sent (set on ACK only) - i += put_uint8_t_by_index(payload, i, msg->payload); // payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) - i += put_uint8_t_by_index(jpg_quality, i, msg->payload); // JPEG quality out of [1,100] + p->type = type; // uint8_t:type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) + p->size = size; // uint32_t:total data size in bytes (set on ACK only) + p->packets = packets; // uint8_t:number of packets beeing sent (set on ACK only) + p->payload = payload; // uint8_t:payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) + p->jpg_quality = jpg_quality; // uint8_t:JPEG quality out of [1,100] - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN); } /** @@ -56,16 +56,16 @@ static inline uint16_t mavlink_msg_data_transmission_handshake_pack(uint8_t syst */ static inline uint16_t mavlink_msg_data_transmission_handshake_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t type, uint32_t size, uint8_t packets, uint8_t payload, uint8_t jpg_quality) { - uint16_t i = 0; + mavlink_data_transmission_handshake_t *p = (mavlink_data_transmission_handshake_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE; - i += put_uint8_t_by_index(type, i, msg->payload); // type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) - i += put_uint32_t_by_index(size, i, msg->payload); // total data size in bytes (set on ACK only) - i += put_uint8_t_by_index(packets, i, msg->payload); // number of packets beeing sent (set on ACK only) - i += put_uint8_t_by_index(payload, i, msg->payload); // payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) - i += put_uint8_t_by_index(jpg_quality, i, msg->payload); // JPEG quality out of [1,100] + p->type = type; // uint8_t:type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) + p->size = size; // uint32_t:total data size in bytes (set on ACK only) + p->packets = packets; // uint8_t:number of packets beeing sent (set on ACK only) + p->payload = payload; // uint8_t:payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) + p->jpg_quality = jpg_quality; // uint8_t:JPEG quality out of [1,100] - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN); } /** @@ -91,13 +91,39 @@ static inline uint16_t mavlink_msg_data_transmission_handshake_encode(uint8_t sy * @param payload payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) * @param jpg_quality JPEG quality out of [1,100] */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_data_transmission_handshake_send(mavlink_channel_t chan, uint8_t type, uint32_t size, uint8_t packets, uint8_t payload, uint8_t jpg_quality) { - mavlink_message_t msg; - mavlink_msg_data_transmission_handshake_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, type, size, packets, payload, jpg_quality); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_data_transmission_handshake_t payload; + uint16_t checksum; + mavlink_data_transmission_handshake_t *p = &payload; + + p->type = type; // uint8_t:type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) + p->size = size; // uint32_t:total data size in bytes (set on ACK only) + p->packets = packets; // uint8_t:number of packets beeing sent (set on ACK only) + p->payload = payload; // uint8_t:payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) + p->jpg_quality = jpg_quality; // uint8_t:JPEG quality out of [1,100] + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN; + hdr.msgid = MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -110,7 +136,8 @@ static inline void mavlink_msg_data_transmission_handshake_send(mavlink_channel_ */ static inline uint8_t mavlink_msg_data_transmission_handshake_get_type(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_data_transmission_handshake_t *p = (mavlink_data_transmission_handshake_t *)&msg->payload[0]; + return (uint8_t)(p->type); } /** @@ -120,12 +147,8 @@ static inline uint8_t mavlink_msg_data_transmission_handshake_get_type(const mav */ static inline uint32_t mavlink_msg_data_transmission_handshake_get_size(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t))[3]; - return (uint32_t)r.i; + mavlink_data_transmission_handshake_t *p = (mavlink_data_transmission_handshake_t *)&msg->payload[0]; + return (uint32_t)(p->size); } /** @@ -135,7 +158,8 @@ static inline uint32_t mavlink_msg_data_transmission_handshake_get_size(const ma */ static inline uint8_t mavlink_msg_data_transmission_handshake_get_packets(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint32_t))[0]; + mavlink_data_transmission_handshake_t *p = (mavlink_data_transmission_handshake_t *)&msg->payload[0]; + return (uint8_t)(p->packets); } /** @@ -145,7 +169,8 @@ static inline uint8_t mavlink_msg_data_transmission_handshake_get_packets(const */ static inline uint8_t mavlink_msg_data_transmission_handshake_get_payload(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint8_t))[0]; + mavlink_data_transmission_handshake_t *p = (mavlink_data_transmission_handshake_t *)&msg->payload[0]; + return (uint8_t)(p->payload); } /** @@ -155,7 +180,8 @@ static inline uint8_t mavlink_msg_data_transmission_handshake_get_payload(const */ static inline uint8_t mavlink_msg_data_transmission_handshake_get_jpg_quality(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_data_transmission_handshake_t *p = (mavlink_data_transmission_handshake_t *)&msg->payload[0]; + return (uint8_t)(p->jpg_quality); } /** @@ -166,9 +192,5 @@ static inline uint8_t mavlink_msg_data_transmission_handshake_get_jpg_quality(co */ static inline void mavlink_msg_data_transmission_handshake_decode(const mavlink_message_t* msg, mavlink_data_transmission_handshake_t* data_transmission_handshake) { - data_transmission_handshake->type = mavlink_msg_data_transmission_handshake_get_type(msg); - data_transmission_handshake->size = mavlink_msg_data_transmission_handshake_get_size(msg); - data_transmission_handshake->packets = mavlink_msg_data_transmission_handshake_get_packets(msg); - data_transmission_handshake->payload = mavlink_msg_data_transmission_handshake_get_payload(msg); - data_transmission_handshake->jpg_quality = mavlink_msg_data_transmission_handshake_get_jpg_quality(msg); + memcpy( data_transmission_handshake, msg->payload, sizeof(mavlink_data_transmission_handshake_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_encapsulated_data.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_encapsulated_data.h index c2c040ae65bcd63a12f434bcfba69f74636c4b43..a5354fddedae8612a5426b27b02cd7767e1d125d 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_encapsulated_data.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_encapsulated_data.h @@ -1,6 +1,8 @@ // MESSAGE ENCAPSULATED_DATA PACKING #define MAVLINK_MSG_ID_ENCAPSULATED_DATA 171 +#define MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN 255 +#define MAVLINK_MSG_171_LEN 255 typedef struct __mavlink_encapsulated_data_t { @@ -8,10 +10,8 @@ typedef struct __mavlink_encapsulated_data_t uint8_t data[253]; ///< image data bytes } mavlink_encapsulated_data_t; - #define MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN 253 - /** * @brief Pack a encapsulated_data message * @param system_id ID of this system @@ -24,13 +24,13 @@ typedef struct __mavlink_encapsulated_data_t */ static inline uint16_t mavlink_msg_encapsulated_data_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint16_t seqnr, const uint8_t* data) { - uint16_t i = 0; + mavlink_encapsulated_data_t *p = (mavlink_encapsulated_data_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_ENCAPSULATED_DATA; - i += put_uint16_t_by_index(seqnr, i, msg->payload); // sequence number (starting with 0 on every transmission) - i += put_array_by_index((const int8_t*)data, sizeof(uint8_t)*253, i, msg->payload); // image data bytes + p->seqnr = seqnr; // uint16_t:sequence number (starting with 0 on every transmission) + memcpy(p->data, data, sizeof(p->data)); // uint8_t[253]:image data bytes - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN); } /** @@ -45,13 +45,13 @@ static inline uint16_t mavlink_msg_encapsulated_data_pack(uint8_t system_id, uin */ static inline uint16_t mavlink_msg_encapsulated_data_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint16_t seqnr, const uint8_t* data) { - uint16_t i = 0; + mavlink_encapsulated_data_t *p = (mavlink_encapsulated_data_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_ENCAPSULATED_DATA; - i += put_uint16_t_by_index(seqnr, i, msg->payload); // sequence number (starting with 0 on every transmission) - i += put_array_by_index((const int8_t*)data, sizeof(uint8_t)*253, i, msg->payload); // image data bytes + p->seqnr = seqnr; // uint16_t:sequence number (starting with 0 on every transmission) + memcpy(p->data, data, sizeof(p->data)); // uint8_t[253]:image data bytes - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN); } /** @@ -74,13 +74,36 @@ static inline uint16_t mavlink_msg_encapsulated_data_encode(uint8_t system_id, u * @param seqnr sequence number (starting with 0 on every transmission) * @param data image data bytes */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_encapsulated_data_send(mavlink_channel_t chan, uint16_t seqnr, const uint8_t* data) { - mavlink_message_t msg; - mavlink_msg_encapsulated_data_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, seqnr, data); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_encapsulated_data_t payload; + uint16_t checksum; + mavlink_encapsulated_data_t *p = &payload; + + p->seqnr = seqnr; // uint16_t:sequence number (starting with 0 on every transmission) + memcpy(p->data, data, sizeof(p->data)); // uint8_t[253]:image data bytes + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN; + hdr.msgid = MAVLINK_MSG_ID_ENCAPSULATED_DATA; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -93,10 +116,8 @@ static inline void mavlink_msg_encapsulated_data_send(mavlink_channel_t chan, ui */ static inline uint16_t mavlink_msg_encapsulated_data_get_seqnr(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload)[0]; - r.b[0] = (msg->payload)[1]; - return (uint16_t)r.s; + mavlink_encapsulated_data_t *p = (mavlink_encapsulated_data_t *)&msg->payload[0]; + return (uint16_t)(p->seqnr); } /** @@ -104,11 +125,12 @@ static inline uint16_t mavlink_msg_encapsulated_data_get_seqnr(const mavlink_mes * * @return image data bytes */ -static inline uint16_t mavlink_msg_encapsulated_data_get_data(const mavlink_message_t* msg, uint8_t* r_data) +static inline uint16_t mavlink_msg_encapsulated_data_get_data(const mavlink_message_t* msg, uint8_t* data) { + mavlink_encapsulated_data_t *p = (mavlink_encapsulated_data_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint16_t), sizeof(uint8_t)*253); - return sizeof(uint8_t)*253; + memcpy(data, p->data, sizeof(p->data)); + return sizeof(p->data); } /** @@ -119,6 +141,5 @@ static inline uint16_t mavlink_msg_encapsulated_data_get_data(const mavlink_mess */ static inline void mavlink_msg_encapsulated_data_decode(const mavlink_message_t* msg, mavlink_encapsulated_data_t* encapsulated_data) { - encapsulated_data->seqnr = mavlink_msg_encapsulated_data_get_seqnr(msg); - mavlink_msg_encapsulated_data_get_data(msg, encapsulated_data->data); + memcpy( encapsulated_data, msg->payload, sizeof(mavlink_encapsulated_data_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_available.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_available.h index b40e23df7b554477b32ea6803b25e051d3552b06..0b4c9c8eff8ffb40a4025a6bef0dda500a0ea6f3 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_available.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_available.h @@ -1,19 +1,16 @@ // MESSAGE IMAGE_AVAILABLE PACKING #define MAVLINK_MSG_ID_IMAGE_AVAILABLE 103 +#define MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN 92 +#define MAVLINK_MSG_103_LEN 92 typedef struct __mavlink_image_available_t { uint64_t cam_id; ///< Camera id - uint8_t cam_no; ///< Camera # (starts with 0) uint64_t timestamp; ///< Timestamp uint64_t valid_until; ///< Until which timestamp this buffer will stay valid uint32_t img_seq; ///< The image sequence number uint32_t img_buf_index; ///< Position of the image in the buffer, starts with 0 - uint16_t width; ///< Image width - uint16_t height; ///< Image height - uint16_t depth; ///< Image depth - uint8_t channels; ///< Image channels uint32_t key; ///< Shared memory area key uint32_t exposure; ///< Exposure time, in microseconds float gain; ///< Camera gain @@ -27,11 +24,14 @@ typedef struct __mavlink_image_available_t float ground_x; ///< Ground truth X float ground_y; ///< Ground truth Y float ground_z; ///< Ground truth Z + uint16_t width; ///< Image width + uint16_t height; ///< Image height + uint16_t depth; ///< Image depth + uint8_t cam_no; ///< Camera # (starts with 0) + uint8_t channels; ///< Image channels } mavlink_image_available_t; - - /** * @brief Pack a image_available message * @param system_id ID of this system @@ -65,34 +65,34 @@ typedef struct __mavlink_image_available_t */ static inline uint16_t mavlink_msg_image_available_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t cam_id, uint8_t cam_no, uint64_t timestamp, uint64_t valid_until, uint32_t img_seq, uint32_t img_buf_index, uint16_t width, uint16_t height, uint16_t depth, uint8_t channels, uint32_t key, uint32_t exposure, float gain, float roll, float pitch, float yaw, float local_z, float lat, float lon, float alt, float ground_x, float ground_y, float ground_z) { - uint16_t i = 0; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_IMAGE_AVAILABLE; - i += put_uint64_t_by_index(cam_id, i, msg->payload); // Camera id - i += put_uint8_t_by_index(cam_no, i, msg->payload); // Camera # (starts with 0) - i += put_uint64_t_by_index(timestamp, i, msg->payload); // Timestamp - i += put_uint64_t_by_index(valid_until, i, msg->payload); // Until which timestamp this buffer will stay valid - i += put_uint32_t_by_index(img_seq, i, msg->payload); // The image sequence number - i += put_uint32_t_by_index(img_buf_index, i, msg->payload); // Position of the image in the buffer, starts with 0 - i += put_uint16_t_by_index(width, i, msg->payload); // Image width - i += put_uint16_t_by_index(height, i, msg->payload); // Image height - i += put_uint16_t_by_index(depth, i, msg->payload); // Image depth - i += put_uint8_t_by_index(channels, i, msg->payload); // Image channels - i += put_uint32_t_by_index(key, i, msg->payload); // Shared memory area key - i += put_uint32_t_by_index(exposure, i, msg->payload); // Exposure time, in microseconds - i += put_float_by_index(gain, i, msg->payload); // Camera gain - i += put_float_by_index(roll, i, msg->payload); // Roll angle in rad - i += put_float_by_index(pitch, i, msg->payload); // Pitch angle in rad - i += put_float_by_index(yaw, i, msg->payload); // Yaw angle in rad - i += put_float_by_index(local_z, i, msg->payload); // Local frame Z coordinate (height over ground) - i += put_float_by_index(lat, i, msg->payload); // GPS X coordinate - i += put_float_by_index(lon, i, msg->payload); // GPS Y coordinate - i += put_float_by_index(alt, i, msg->payload); // Global frame altitude - i += put_float_by_index(ground_x, i, msg->payload); // Ground truth X - i += put_float_by_index(ground_y, i, msg->payload); // Ground truth Y - i += put_float_by_index(ground_z, i, msg->payload); // Ground truth Z - - return mavlink_finalize_message(msg, system_id, component_id, i); + p->cam_id = cam_id; // uint64_t:Camera id + p->cam_no = cam_no; // uint8_t:Camera # (starts with 0) + p->timestamp = timestamp; // uint64_t:Timestamp + p->valid_until = valid_until; // uint64_t:Until which timestamp this buffer will stay valid + p->img_seq = img_seq; // uint32_t:The image sequence number + p->img_buf_index = img_buf_index; // uint32_t:Position of the image in the buffer, starts with 0 + p->width = width; // uint16_t:Image width + p->height = height; // uint16_t:Image height + p->depth = depth; // uint16_t:Image depth + p->channels = channels; // uint8_t:Image channels + p->key = key; // uint32_t:Shared memory area key + p->exposure = exposure; // uint32_t:Exposure time, in microseconds + p->gain = gain; // float:Camera gain + p->roll = roll; // float:Roll angle in rad + p->pitch = pitch; // float:Pitch angle in rad + p->yaw = yaw; // float:Yaw angle in rad + p->local_z = local_z; // float:Local frame Z coordinate (height over ground) + p->lat = lat; // float:GPS X coordinate + p->lon = lon; // float:GPS Y coordinate + p->alt = alt; // float:Global frame altitude + p->ground_x = ground_x; // float:Ground truth X + p->ground_y = ground_y; // float:Ground truth Y + p->ground_z = ground_z; // float:Ground truth Z + + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN); } /** @@ -128,34 +128,34 @@ static inline uint16_t mavlink_msg_image_available_pack(uint8_t system_id, uint8 */ static inline uint16_t mavlink_msg_image_available_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t cam_id, uint8_t cam_no, uint64_t timestamp, uint64_t valid_until, uint32_t img_seq, uint32_t img_buf_index, uint16_t width, uint16_t height, uint16_t depth, uint8_t channels, uint32_t key, uint32_t exposure, float gain, float roll, float pitch, float yaw, float local_z, float lat, float lon, float alt, float ground_x, float ground_y, float ground_z) { - uint16_t i = 0; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_IMAGE_AVAILABLE; - i += put_uint64_t_by_index(cam_id, i, msg->payload); // Camera id - i += put_uint8_t_by_index(cam_no, i, msg->payload); // Camera # (starts with 0) - i += put_uint64_t_by_index(timestamp, i, msg->payload); // Timestamp - i += put_uint64_t_by_index(valid_until, i, msg->payload); // Until which timestamp this buffer will stay valid - i += put_uint32_t_by_index(img_seq, i, msg->payload); // The image sequence number - i += put_uint32_t_by_index(img_buf_index, i, msg->payload); // Position of the image in the buffer, starts with 0 - i += put_uint16_t_by_index(width, i, msg->payload); // Image width - i += put_uint16_t_by_index(height, i, msg->payload); // Image height - i += put_uint16_t_by_index(depth, i, msg->payload); // Image depth - i += put_uint8_t_by_index(channels, i, msg->payload); // Image channels - i += put_uint32_t_by_index(key, i, msg->payload); // Shared memory area key - i += put_uint32_t_by_index(exposure, i, msg->payload); // Exposure time, in microseconds - i += put_float_by_index(gain, i, msg->payload); // Camera gain - i += put_float_by_index(roll, i, msg->payload); // Roll angle in rad - i += put_float_by_index(pitch, i, msg->payload); // Pitch angle in rad - i += put_float_by_index(yaw, i, msg->payload); // Yaw angle in rad - i += put_float_by_index(local_z, i, msg->payload); // Local frame Z coordinate (height over ground) - i += put_float_by_index(lat, i, msg->payload); // GPS X coordinate - i += put_float_by_index(lon, i, msg->payload); // GPS Y coordinate - i += put_float_by_index(alt, i, msg->payload); // Global frame altitude - i += put_float_by_index(ground_x, i, msg->payload); // Ground truth X - i += put_float_by_index(ground_y, i, msg->payload); // Ground truth Y - i += put_float_by_index(ground_z, i, msg->payload); // Ground truth Z - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + p->cam_id = cam_id; // uint64_t:Camera id + p->cam_no = cam_no; // uint8_t:Camera # (starts with 0) + p->timestamp = timestamp; // uint64_t:Timestamp + p->valid_until = valid_until; // uint64_t:Until which timestamp this buffer will stay valid + p->img_seq = img_seq; // uint32_t:The image sequence number + p->img_buf_index = img_buf_index; // uint32_t:Position of the image in the buffer, starts with 0 + p->width = width; // uint16_t:Image width + p->height = height; // uint16_t:Image height + p->depth = depth; // uint16_t:Image depth + p->channels = channels; // uint8_t:Image channels + p->key = key; // uint32_t:Shared memory area key + p->exposure = exposure; // uint32_t:Exposure time, in microseconds + p->gain = gain; // float:Camera gain + p->roll = roll; // float:Roll angle in rad + p->pitch = pitch; // float:Pitch angle in rad + p->yaw = yaw; // float:Yaw angle in rad + p->local_z = local_z; // float:Local frame Z coordinate (height over ground) + p->lat = lat; // float:GPS X coordinate + p->lon = lon; // float:GPS Y coordinate + p->alt = alt; // float:Global frame altitude + p->ground_x = ground_x; // float:Ground truth X + p->ground_y = ground_y; // float:Ground truth Y + p->ground_z = ground_z; // float:Ground truth Z + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN); } /** @@ -199,13 +199,57 @@ static inline uint16_t mavlink_msg_image_available_encode(uint8_t system_id, uin * @param ground_y Ground truth Y * @param ground_z Ground truth Z */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_image_available_send(mavlink_channel_t chan, uint64_t cam_id, uint8_t cam_no, uint64_t timestamp, uint64_t valid_until, uint32_t img_seq, uint32_t img_buf_index, uint16_t width, uint16_t height, uint16_t depth, uint8_t channels, uint32_t key, uint32_t exposure, float gain, float roll, float pitch, float yaw, float local_z, float lat, float lon, float alt, float ground_x, float ground_y, float ground_z) { - mavlink_message_t msg; - mavlink_msg_image_available_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, cam_id, cam_no, timestamp, valid_until, img_seq, img_buf_index, width, height, depth, channels, key, exposure, gain, roll, pitch, yaw, local_z, lat, lon, alt, ground_x, ground_y, ground_z); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_image_available_t payload; + uint16_t checksum; + mavlink_image_available_t *p = &payload; + + p->cam_id = cam_id; // uint64_t:Camera id + p->cam_no = cam_no; // uint8_t:Camera # (starts with 0) + p->timestamp = timestamp; // uint64_t:Timestamp + p->valid_until = valid_until; // uint64_t:Until which timestamp this buffer will stay valid + p->img_seq = img_seq; // uint32_t:The image sequence number + p->img_buf_index = img_buf_index; // uint32_t:Position of the image in the buffer, starts with 0 + p->width = width; // uint16_t:Image width + p->height = height; // uint16_t:Image height + p->depth = depth; // uint16_t:Image depth + p->channels = channels; // uint8_t:Image channels + p->key = key; // uint32_t:Shared memory area key + p->exposure = exposure; // uint32_t:Exposure time, in microseconds + p->gain = gain; // float:Camera gain + p->roll = roll; // float:Roll angle in rad + p->pitch = pitch; // float:Pitch angle in rad + p->yaw = yaw; // float:Yaw angle in rad + p->local_z = local_z; // float:Local frame Z coordinate (height over ground) + p->lat = lat; // float:GPS X coordinate + p->lon = lon; // float:GPS Y coordinate + p->alt = alt; // float:Global frame altitude + p->ground_x = ground_x; // float:Ground truth X + p->ground_y = ground_y; // float:Ground truth Y + p->ground_z = ground_z; // float:Ground truth Z + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN; + hdr.msgid = MAVLINK_MSG_ID_IMAGE_AVAILABLE; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -218,16 +262,8 @@ static inline void mavlink_msg_image_available_send(mavlink_channel_t chan, uint */ static inline uint64_t mavlink_msg_image_available_get_cam_id(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; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (uint64_t)(p->cam_id); } /** @@ -237,7 +273,8 @@ static inline uint64_t mavlink_msg_image_available_get_cam_id(const mavlink_mess */ static inline uint8_t mavlink_msg_image_available_get_cam_no(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint64_t))[0]; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (uint8_t)(p->cam_no); } /** @@ -247,16 +284,8 @@ static inline uint8_t mavlink_msg_image_available_get_cam_no(const mavlink_messa */ static inline uint64_t mavlink_msg_image_available_get_timestamp(const mavlink_message_t* msg) { - generic_64bit r; - r.b[7] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[0]; - r.b[6] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[1]; - r.b[5] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[2]; - r.b[4] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[3]; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[4]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[5]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[6]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[7]; - return (uint64_t)r.ll; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (uint64_t)(p->timestamp); } /** @@ -266,16 +295,8 @@ static inline uint64_t mavlink_msg_image_available_get_timestamp(const mavlink_m */ static inline uint64_t mavlink_msg_image_available_get_valid_until(const mavlink_message_t* msg) { - generic_64bit r; - r.b[7] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t))[0]; - r.b[6] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t))[1]; - r.b[5] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t))[2]; - r.b[4] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t))[3]; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t))[4]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t))[5]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t))[6]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t))[7]; - return (uint64_t)r.ll; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (uint64_t)(p->valid_until); } /** @@ -285,12 +306,8 @@ static inline uint64_t mavlink_msg_image_available_get_valid_until(const mavlink */ static inline uint32_t mavlink_msg_image_available_get_img_seq(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t))[3]; - return (uint32_t)r.i; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (uint32_t)(p->img_seq); } /** @@ -300,12 +317,8 @@ static inline uint32_t mavlink_msg_image_available_get_img_seq(const mavlink_mes */ static inline uint32_t mavlink_msg_image_available_get_img_buf_index(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t))[3]; - return (uint32_t)r.i; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (uint32_t)(p->img_buf_index); } /** @@ -315,10 +328,8 @@ static inline uint32_t mavlink_msg_image_available_get_img_buf_index(const mavli */ static inline uint16_t mavlink_msg_image_available_get_width(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t))[1]; - return (uint16_t)r.s; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (uint16_t)(p->width); } /** @@ -328,10 +339,8 @@ static inline uint16_t mavlink_msg_image_available_get_width(const mavlink_messa */ static inline uint16_t mavlink_msg_image_available_get_height(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (uint16_t)(p->height); } /** @@ -341,10 +350,8 @@ static inline uint16_t mavlink_msg_image_available_get_height(const mavlink_mess */ static inline uint16_t mavlink_msg_image_available_get_depth(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (uint16_t)(p->depth); } /** @@ -354,7 +361,8 @@ static inline uint16_t mavlink_msg_image_available_get_depth(const mavlink_messa */ static inline uint8_t mavlink_msg_image_available_get_channels(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (uint8_t)(p->channels); } /** @@ -364,12 +372,8 @@ static inline uint8_t mavlink_msg_image_available_get_channels(const mavlink_mes */ static inline uint32_t mavlink_msg_image_available_get_key(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t))[3]; - return (uint32_t)r.i; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (uint32_t)(p->key); } /** @@ -379,12 +383,8 @@ static inline uint32_t mavlink_msg_image_available_get_key(const mavlink_message */ static inline uint32_t mavlink_msg_image_available_get_exposure(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t))[3]; - return (uint32_t)r.i; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (uint32_t)(p->exposure); } /** @@ -394,12 +394,8 @@ static inline uint32_t mavlink_msg_image_available_get_exposure(const mavlink_me */ static inline float mavlink_msg_image_available_get_gain(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t))[3]; - return (float)r.f; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (float)(p->gain); } /** @@ -409,12 +405,8 @@ static inline float mavlink_msg_image_available_get_gain(const mavlink_message_t */ static inline float mavlink_msg_image_available_get_roll(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float))[3]; - return (float)r.f; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (float)(p->roll); } /** @@ -424,12 +416,8 @@ static inline float mavlink_msg_image_available_get_roll(const mavlink_message_t */ static inline float mavlink_msg_image_available_get_pitch(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (float)(p->pitch); } /** @@ -439,12 +427,8 @@ static inline float mavlink_msg_image_available_get_pitch(const mavlink_message_ */ static inline float mavlink_msg_image_available_get_yaw(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (float)(p->yaw); } /** @@ -454,12 +438,8 @@ static inline float mavlink_msg_image_available_get_yaw(const mavlink_message_t* */ static inline float mavlink_msg_image_available_get_local_z(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (float)(p->local_z); } /** @@ -469,12 +449,8 @@ static inline float mavlink_msg_image_available_get_local_z(const mavlink_messag */ static inline float mavlink_msg_image_available_get_lat(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (float)(p->lat); } /** @@ -484,12 +460,8 @@ static inline float mavlink_msg_image_available_get_lat(const mavlink_message_t* */ static inline float mavlink_msg_image_available_get_lon(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (float)(p->lon); } /** @@ -499,12 +471,8 @@ static inline float mavlink_msg_image_available_get_lon(const mavlink_message_t* */ static inline float mavlink_msg_image_available_get_alt(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (float)(p->alt); } /** @@ -514,12 +482,8 @@ static inline float mavlink_msg_image_available_get_alt(const mavlink_message_t* */ static inline float mavlink_msg_image_available_get_ground_x(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (float)(p->ground_x); } /** @@ -529,12 +493,8 @@ static inline float mavlink_msg_image_available_get_ground_x(const mavlink_messa */ static inline float mavlink_msg_image_available_get_ground_y(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (float)(p->ground_y); } /** @@ -544,12 +504,8 @@ static inline float mavlink_msg_image_available_get_ground_y(const mavlink_messa */ static inline float mavlink_msg_image_available_get_ground_z(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (float)(p->ground_z); } /** @@ -560,27 +516,5 @@ static inline float mavlink_msg_image_available_get_ground_z(const mavlink_messa */ static inline void mavlink_msg_image_available_decode(const mavlink_message_t* msg, mavlink_image_available_t* image_available) { - image_available->cam_id = mavlink_msg_image_available_get_cam_id(msg); - image_available->cam_no = mavlink_msg_image_available_get_cam_no(msg); - image_available->timestamp = mavlink_msg_image_available_get_timestamp(msg); - image_available->valid_until = mavlink_msg_image_available_get_valid_until(msg); - image_available->img_seq = mavlink_msg_image_available_get_img_seq(msg); - image_available->img_buf_index = mavlink_msg_image_available_get_img_buf_index(msg); - image_available->width = mavlink_msg_image_available_get_width(msg); - image_available->height = mavlink_msg_image_available_get_height(msg); - image_available->depth = mavlink_msg_image_available_get_depth(msg); - image_available->channels = mavlink_msg_image_available_get_channels(msg); - image_available->key = mavlink_msg_image_available_get_key(msg); - image_available->exposure = mavlink_msg_image_available_get_exposure(msg); - image_available->gain = mavlink_msg_image_available_get_gain(msg); - image_available->roll = mavlink_msg_image_available_get_roll(msg); - image_available->pitch = mavlink_msg_image_available_get_pitch(msg); - image_available->yaw = mavlink_msg_image_available_get_yaw(msg); - image_available->local_z = mavlink_msg_image_available_get_local_z(msg); - image_available->lat = mavlink_msg_image_available_get_lat(msg); - image_available->lon = mavlink_msg_image_available_get_lon(msg); - image_available->alt = mavlink_msg_image_available_get_alt(msg); - image_available->ground_x = mavlink_msg_image_available_get_ground_x(msg); - image_available->ground_y = mavlink_msg_image_available_get_ground_y(msg); - image_available->ground_z = mavlink_msg_image_available_get_ground_z(msg); + memcpy( image_available, msg->payload, sizeof(mavlink_image_available_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_trigger_control.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_trigger_control.h index 90aa9dcf4e8659e59fe0be38ece0c3cbecd9ade1..99027fea5bced7caff56823021cec2c109748d97 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_trigger_control.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_trigger_control.h @@ -1,6 +1,8 @@ // MESSAGE IMAGE_TRIGGER_CONTROL PACKING #define MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL 102 +#define MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN 1 +#define MAVLINK_MSG_102_LEN 1 typedef struct __mavlink_image_trigger_control_t { @@ -8,8 +10,6 @@ typedef struct __mavlink_image_trigger_control_t } mavlink_image_trigger_control_t; - - /** * @brief Pack a image_trigger_control message * @param system_id ID of this system @@ -21,12 +21,12 @@ typedef struct __mavlink_image_trigger_control_t */ static inline uint16_t mavlink_msg_image_trigger_control_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t enable) { - uint16_t i = 0; + mavlink_image_trigger_control_t *p = (mavlink_image_trigger_control_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL; - i += put_uint8_t_by_index(enable, i, msg->payload); // 0 to disable, 1 to enable + p->enable = enable; // uint8_t:0 to disable, 1 to enable - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN); } /** @@ -40,12 +40,12 @@ static inline uint16_t mavlink_msg_image_trigger_control_pack(uint8_t system_id, */ static inline uint16_t mavlink_msg_image_trigger_control_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t enable) { - uint16_t i = 0; + mavlink_image_trigger_control_t *p = (mavlink_image_trigger_control_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL; - i += put_uint8_t_by_index(enable, i, msg->payload); // 0 to disable, 1 to enable + p->enable = enable; // uint8_t:0 to disable, 1 to enable - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN); } /** @@ -67,13 +67,35 @@ static inline uint16_t mavlink_msg_image_trigger_control_encode(uint8_t system_i * * @param enable 0 to disable, 1 to enable */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_image_trigger_control_send(mavlink_channel_t chan, uint8_t enable) { - mavlink_message_t msg; - mavlink_msg_image_trigger_control_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, enable); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_image_trigger_control_t payload; + uint16_t checksum; + mavlink_image_trigger_control_t *p = &payload; + + p->enable = enable; // uint8_t:0 to disable, 1 to enable + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN; + hdr.msgid = MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -86,7 +108,8 @@ static inline void mavlink_msg_image_trigger_control_send(mavlink_channel_t chan */ static inline uint8_t mavlink_msg_image_trigger_control_get_enable(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_image_trigger_control_t *p = (mavlink_image_trigger_control_t *)&msg->payload[0]; + return (uint8_t)(p->enable); } /** @@ -97,5 +120,5 @@ static inline uint8_t mavlink_msg_image_trigger_control_get_enable(const mavlink */ static inline void mavlink_msg_image_trigger_control_decode(const mavlink_message_t* msg, mavlink_image_trigger_control_t* image_trigger_control) { - image_trigger_control->enable = mavlink_msg_image_trigger_control_get_enable(msg); + memcpy( image_trigger_control, msg->payload, sizeof(mavlink_image_trigger_control_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_triggered.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_triggered.h index 000003f3d73ba3239590a8701acc935877ea0dc2..250bee3a1b0c6b9d8265cddd0dcd3ae14326bd2f 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_triggered.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_triggered.h @@ -1,6 +1,8 @@ // MESSAGE IMAGE_TRIGGERED PACKING #define MAVLINK_MSG_ID_IMAGE_TRIGGERED 101 +#define MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN 52 +#define MAVLINK_MSG_101_LEN 52 typedef struct __mavlink_image_triggered_t { @@ -19,8 +21,6 @@ typedef struct __mavlink_image_triggered_t } mavlink_image_triggered_t; - - /** * @brief Pack a image_triggered message * @param system_id ID of this system @@ -43,23 +43,23 @@ typedef struct __mavlink_image_triggered_t */ static inline uint16_t mavlink_msg_image_triggered_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t timestamp, uint32_t seq, float roll, float pitch, float yaw, float local_z, float lat, float lon, float alt, float ground_x, float ground_y, float ground_z) { - uint16_t i = 0; + mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGERED; - i += put_uint64_t_by_index(timestamp, i, msg->payload); // Timestamp - i += put_uint32_t_by_index(seq, i, msg->payload); // IMU seq - i += put_float_by_index(roll, i, msg->payload); // Roll angle in rad - i += put_float_by_index(pitch, i, msg->payload); // Pitch angle in rad - i += put_float_by_index(yaw, i, msg->payload); // Yaw angle in rad - i += put_float_by_index(local_z, i, msg->payload); // Local frame Z coordinate (height over ground) - i += put_float_by_index(lat, i, msg->payload); // GPS X coordinate - i += put_float_by_index(lon, i, msg->payload); // GPS Y coordinate - i += put_float_by_index(alt, i, msg->payload); // Global frame altitude - i += put_float_by_index(ground_x, i, msg->payload); // Ground truth X - i += put_float_by_index(ground_y, i, msg->payload); // Ground truth Y - i += put_float_by_index(ground_z, i, msg->payload); // Ground truth Z + p->timestamp = timestamp; // uint64_t:Timestamp + p->seq = seq; // uint32_t:IMU seq + p->roll = roll; // float:Roll angle in rad + p->pitch = pitch; // float:Pitch angle in rad + p->yaw = yaw; // float:Yaw angle in rad + p->local_z = local_z; // float:Local frame Z coordinate (height over ground) + p->lat = lat; // float:GPS X coordinate + p->lon = lon; // float:GPS Y coordinate + p->alt = alt; // float:Global frame altitude + p->ground_x = ground_x; // float:Ground truth X + p->ground_y = ground_y; // float:Ground truth Y + p->ground_z = ground_z; // float:Ground truth Z - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN); } /** @@ -84,23 +84,23 @@ static inline uint16_t mavlink_msg_image_triggered_pack(uint8_t system_id, uint8 */ static inline uint16_t mavlink_msg_image_triggered_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t timestamp, uint32_t seq, float roll, float pitch, float yaw, float local_z, float lat, float lon, float alt, float ground_x, float ground_y, float ground_z) { - uint16_t i = 0; + mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGERED; - i += put_uint64_t_by_index(timestamp, i, msg->payload); // Timestamp - i += put_uint32_t_by_index(seq, i, msg->payload); // IMU seq - i += put_float_by_index(roll, i, msg->payload); // Roll angle in rad - i += put_float_by_index(pitch, i, msg->payload); // Pitch angle in rad - i += put_float_by_index(yaw, i, msg->payload); // Yaw angle in rad - i += put_float_by_index(local_z, i, msg->payload); // Local frame Z coordinate (height over ground) - i += put_float_by_index(lat, i, msg->payload); // GPS X coordinate - i += put_float_by_index(lon, i, msg->payload); // GPS Y coordinate - i += put_float_by_index(alt, i, msg->payload); // Global frame altitude - i += put_float_by_index(ground_x, i, msg->payload); // Ground truth X - i += put_float_by_index(ground_y, i, msg->payload); // Ground truth Y - i += put_float_by_index(ground_z, i, msg->payload); // Ground truth Z + p->timestamp = timestamp; // uint64_t:Timestamp + p->seq = seq; // uint32_t:IMU seq + p->roll = roll; // float:Roll angle in rad + p->pitch = pitch; // float:Pitch angle in rad + p->yaw = yaw; // float:Yaw angle in rad + p->local_z = local_z; // float:Local frame Z coordinate (height over ground) + p->lat = lat; // float:GPS X coordinate + p->lon = lon; // float:GPS Y coordinate + p->alt = alt; // float:Global frame altitude + p->ground_x = ground_x; // float:Ground truth X + p->ground_y = ground_y; // float:Ground truth Y + p->ground_z = ground_z; // float:Ground truth Z - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN); } /** @@ -133,13 +133,46 @@ static inline uint16_t mavlink_msg_image_triggered_encode(uint8_t system_id, uin * @param ground_y Ground truth Y * @param ground_z Ground truth Z */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_image_triggered_send(mavlink_channel_t chan, uint64_t timestamp, uint32_t seq, float roll, float pitch, float yaw, float local_z, float lat, float lon, float alt, float ground_x, float ground_y, float ground_z) { - mavlink_message_t msg; - mavlink_msg_image_triggered_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, timestamp, seq, roll, pitch, yaw, local_z, lat, lon, alt, ground_x, ground_y, ground_z); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_image_triggered_t payload; + uint16_t checksum; + mavlink_image_triggered_t *p = &payload; + + p->timestamp = timestamp; // uint64_t:Timestamp + p->seq = seq; // uint32_t:IMU seq + p->roll = roll; // float:Roll angle in rad + p->pitch = pitch; // float:Pitch angle in rad + p->yaw = yaw; // float:Yaw angle in rad + p->local_z = local_z; // float:Local frame Z coordinate (height over ground) + p->lat = lat; // float:GPS X coordinate + p->lon = lon; // float:GPS Y coordinate + p->alt = alt; // float:Global frame altitude + p->ground_x = ground_x; // float:Ground truth X + p->ground_y = ground_y; // float:Ground truth Y + p->ground_z = ground_z; // float:Ground truth Z + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN; + hdr.msgid = MAVLINK_MSG_ID_IMAGE_TRIGGERED; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -152,16 +185,8 @@ static inline void mavlink_msg_image_triggered_send(mavlink_channel_t chan, uint */ static inline uint64_t mavlink_msg_image_triggered_get_timestamp(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; + mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg->payload[0]; + return (uint64_t)(p->timestamp); } /** @@ -171,12 +196,8 @@ static inline uint64_t mavlink_msg_image_triggered_get_timestamp(const mavlink_m */ static inline uint32_t mavlink_msg_image_triggered_get_seq(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 (uint32_t)r.i; + mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg->payload[0]; + return (uint32_t)(p->seq); } /** @@ -186,12 +207,8 @@ static inline uint32_t mavlink_msg_image_triggered_get_seq(const mavlink_message */ static inline float mavlink_msg_image_triggered_get_roll(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t))[3]; - return (float)r.f; + mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg->payload[0]; + return (float)(p->roll); } /** @@ -201,12 +218,8 @@ static inline float mavlink_msg_image_triggered_get_roll(const mavlink_message_t */ static inline float mavlink_msg_image_triggered_get_pitch(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float))[3]; - return (float)r.f; + mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg->payload[0]; + return (float)(p->pitch); } /** @@ -216,12 +229,8 @@ static inline float mavlink_msg_image_triggered_get_pitch(const mavlink_message_ */ static inline float mavlink_msg_image_triggered_get_yaw(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg->payload[0]; + return (float)(p->yaw); } /** @@ -231,12 +240,8 @@ static inline float mavlink_msg_image_triggered_get_yaw(const mavlink_message_t* */ static inline float mavlink_msg_image_triggered_get_local_z(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg->payload[0]; + return (float)(p->local_z); } /** @@ -246,12 +251,8 @@ static inline float mavlink_msg_image_triggered_get_local_z(const mavlink_messag */ static inline float mavlink_msg_image_triggered_get_lat(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg->payload[0]; + return (float)(p->lat); } /** @@ -261,12 +262,8 @@ static inline float mavlink_msg_image_triggered_get_lat(const mavlink_message_t* */ static inline float mavlink_msg_image_triggered_get_lon(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg->payload[0]; + return (float)(p->lon); } /** @@ -276,12 +273,8 @@ static inline float mavlink_msg_image_triggered_get_lon(const mavlink_message_t* */ static inline float mavlink_msg_image_triggered_get_alt(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg->payload[0]; + return (float)(p->alt); } /** @@ -291,12 +284,8 @@ static inline float mavlink_msg_image_triggered_get_alt(const mavlink_message_t* */ static inline float mavlink_msg_image_triggered_get_ground_x(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg->payload[0]; + return (float)(p->ground_x); } /** @@ -306,12 +295,8 @@ static inline float mavlink_msg_image_triggered_get_ground_x(const mavlink_messa */ static inline float mavlink_msg_image_triggered_get_ground_y(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg->payload[0]; + return (float)(p->ground_y); } /** @@ -321,12 +306,8 @@ static inline float mavlink_msg_image_triggered_get_ground_y(const mavlink_messa */ static inline float mavlink_msg_image_triggered_get_ground_z(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg->payload[0]; + return (float)(p->ground_z); } /** @@ -337,16 +318,5 @@ static inline float mavlink_msg_image_triggered_get_ground_z(const mavlink_messa */ static inline void mavlink_msg_image_triggered_decode(const mavlink_message_t* msg, mavlink_image_triggered_t* image_triggered) { - image_triggered->timestamp = mavlink_msg_image_triggered_get_timestamp(msg); - image_triggered->seq = mavlink_msg_image_triggered_get_seq(msg); - image_triggered->roll = mavlink_msg_image_triggered_get_roll(msg); - image_triggered->pitch = mavlink_msg_image_triggered_get_pitch(msg); - image_triggered->yaw = mavlink_msg_image_triggered_get_yaw(msg); - image_triggered->local_z = mavlink_msg_image_triggered_get_local_z(msg); - image_triggered->lat = mavlink_msg_image_triggered_get_lat(msg); - image_triggered->lon = mavlink_msg_image_triggered_get_lon(msg); - image_triggered->alt = mavlink_msg_image_triggered_get_alt(msg); - image_triggered->ground_x = mavlink_msg_image_triggered_get_ground_x(msg); - image_triggered->ground_y = mavlink_msg_image_triggered_get_ground_y(msg); - image_triggered->ground_z = mavlink_msg_image_triggered_get_ground_z(msg); + memcpy( image_triggered, msg->payload, sizeof(mavlink_image_triggered_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_marker.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_marker.h index fb275534ae15fd00cd590beec1002677fc68f050..34ebf635172a102d27739c4ed7cc77863c094f99 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_marker.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_marker.h @@ -1,21 +1,21 @@ // MESSAGE MARKER PACKING #define MAVLINK_MSG_ID_MARKER 130 +#define MAVLINK_MSG_ID_MARKER_LEN 26 +#define MAVLINK_MSG_130_LEN 26 typedef struct __mavlink_marker_t { - uint16_t id; ///< ID float x; ///< x position float y; ///< y position float z; ///< z position float roll; ///< roll orientation float pitch; ///< pitch orientation float yaw; ///< yaw orientation + uint16_t id; ///< ID } mavlink_marker_t; - - /** * @brief Pack a marker message * @param system_id ID of this system @@ -33,18 +33,18 @@ typedef struct __mavlink_marker_t */ static inline uint16_t mavlink_msg_marker_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint16_t id, float x, float y, float z, float roll, float pitch, float yaw) { - uint16_t i = 0; + mavlink_marker_t *p = (mavlink_marker_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_MARKER; - i += put_uint16_t_by_index(id, i, msg->payload); // ID - i += put_float_by_index(x, i, msg->payload); // x position - i += put_float_by_index(y, i, msg->payload); // y position - i += put_float_by_index(z, i, msg->payload); // z position - i += put_float_by_index(roll, i, msg->payload); // roll orientation - i += put_float_by_index(pitch, i, msg->payload); // pitch orientation - i += put_float_by_index(yaw, i, msg->payload); // yaw orientation + p->id = id; // uint16_t:ID + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->roll = roll; // float:roll orientation + p->pitch = pitch; // float:pitch orientation + p->yaw = yaw; // float:yaw orientation - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MARKER_LEN); } /** @@ -64,18 +64,18 @@ static inline uint16_t mavlink_msg_marker_pack(uint8_t system_id, uint8_t compon */ static inline uint16_t mavlink_msg_marker_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint16_t id, float x, float y, float z, float roll, float pitch, float yaw) { - uint16_t i = 0; + mavlink_marker_t *p = (mavlink_marker_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_MARKER; - i += put_uint16_t_by_index(id, i, msg->payload); // ID - i += put_float_by_index(x, i, msg->payload); // x position - i += put_float_by_index(y, i, msg->payload); // y position - i += put_float_by_index(z, i, msg->payload); // z position - i += put_float_by_index(roll, i, msg->payload); // roll orientation - i += put_float_by_index(pitch, i, msg->payload); // pitch orientation - i += put_float_by_index(yaw, i, msg->payload); // yaw orientation + p->id = id; // uint16_t:ID + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->roll = roll; // float:roll orientation + p->pitch = pitch; // float:pitch orientation + p->yaw = yaw; // float:yaw orientation - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MARKER_LEN); } /** @@ -103,13 +103,41 @@ static inline uint16_t mavlink_msg_marker_encode(uint8_t system_id, uint8_t comp * @param pitch pitch orientation * @param yaw yaw orientation */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_marker_send(mavlink_channel_t chan, uint16_t id, float x, float y, float z, float roll, float pitch, float yaw) { - mavlink_message_t msg; - mavlink_msg_marker_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, id, x, y, z, roll, pitch, yaw); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_marker_t payload; + uint16_t checksum; + mavlink_marker_t *p = &payload; + + p->id = id; // uint16_t:ID + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->roll = roll; // float:roll orientation + p->pitch = pitch; // float:pitch orientation + p->yaw = yaw; // float:yaw orientation + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_MARKER_LEN; + hdr.msgid = MAVLINK_MSG_ID_MARKER; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -122,10 +150,8 @@ static inline void mavlink_msg_marker_send(mavlink_channel_t chan, uint16_t id, */ static inline uint16_t mavlink_msg_marker_get_id(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload)[0]; - r.b[0] = (msg->payload)[1]; - return (uint16_t)r.s; + mavlink_marker_t *p = (mavlink_marker_t *)&msg->payload[0]; + return (uint16_t)(p->id); } /** @@ -135,12 +161,8 @@ static inline uint16_t mavlink_msg_marker_get_id(const mavlink_message_t* msg) */ static inline float mavlink_msg_marker_get_x(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint16_t))[0]; - r.b[2] = (msg->payload+sizeof(uint16_t))[1]; - r.b[1] = (msg->payload+sizeof(uint16_t))[2]; - r.b[0] = (msg->payload+sizeof(uint16_t))[3]; - return (float)r.f; + mavlink_marker_t *p = (mavlink_marker_t *)&msg->payload[0]; + return (float)(p->x); } /** @@ -150,12 +172,8 @@ static inline float mavlink_msg_marker_get_x(const mavlink_message_t* msg) */ static inline float mavlink_msg_marker_get_y(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint16_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint16_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(float))[3]; - return (float)r.f; + mavlink_marker_t *p = (mavlink_marker_t *)&msg->payload[0]; + return (float)(p->y); } /** @@ -165,12 +183,8 @@ static inline float mavlink_msg_marker_get_y(const mavlink_message_t* msg) */ static inline float mavlink_msg_marker_get_z(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_marker_t *p = (mavlink_marker_t *)&msg->payload[0]; + return (float)(p->z); } /** @@ -180,12 +194,8 @@ static inline float mavlink_msg_marker_get_z(const mavlink_message_t* msg) */ static inline float mavlink_msg_marker_get_roll(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_marker_t *p = (mavlink_marker_t *)&msg->payload[0]; + return (float)(p->roll); } /** @@ -195,12 +205,8 @@ static inline float mavlink_msg_marker_get_roll(const mavlink_message_t* msg) */ static inline float mavlink_msg_marker_get_pitch(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_marker_t *p = (mavlink_marker_t *)&msg->payload[0]; + return (float)(p->pitch); } /** @@ -210,12 +216,8 @@ static inline float mavlink_msg_marker_get_pitch(const mavlink_message_t* msg) */ static inline float mavlink_msg_marker_get_yaw(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_marker_t *p = (mavlink_marker_t *)&msg->payload[0]; + return (float)(p->yaw); } /** @@ -226,11 +228,5 @@ static inline float mavlink_msg_marker_get_yaw(const mavlink_message_t* msg) */ static inline void mavlink_msg_marker_decode(const mavlink_message_t* msg, mavlink_marker_t* marker) { - marker->id = mavlink_msg_marker_get_id(msg); - marker->x = mavlink_msg_marker_get_x(msg); - marker->y = mavlink_msg_marker_get_y(msg); - marker->z = mavlink_msg_marker_get_z(msg); - marker->roll = mavlink_msg_marker_get_roll(msg); - marker->pitch = mavlink_msg_marker_get_pitch(msg); - marker->yaw = mavlink_msg_marker_get_yaw(msg); + memcpy( marker, msg->payload, sizeof(mavlink_marker_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_pattern_detected.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_pattern_detected.h index 1a2b46596b72bc27a50d9dae2812abf412663230..d5d803bd0cfe66c3491a845fb947ac664e6e3d3f 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_pattern_detected.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_pattern_detected.h @@ -1,19 +1,19 @@ // MESSAGE PATTERN_DETECTED PACKING #define MAVLINK_MSG_ID_PATTERN_DETECTED 160 +#define MAVLINK_MSG_ID_PATTERN_DETECTED_LEN 106 +#define MAVLINK_MSG_160_LEN 106 typedef struct __mavlink_pattern_detected_t { - uint8_t type; ///< 0: Pattern, 1: Letter float confidence; ///< Confidence of detection - int8_t file[100]; ///< Pattern file name + uint8_t type; ///< 0: Pattern, 1: Letter + char file[100]; ///< Pattern file name uint8_t detected; ///< Accepted as true detection, 0 no, 1 yes } mavlink_pattern_detected_t; - #define MAVLINK_MSG_PATTERN_DETECTED_FIELD_FILE_LEN 100 - /** * @brief Pack a pattern_detected message * @param system_id ID of this system @@ -26,17 +26,17 @@ typedef struct __mavlink_pattern_detected_t * @param detected Accepted as true detection, 0 no, 1 yes * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_pattern_detected_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t type, float confidence, const int8_t* file, uint8_t detected) +static inline uint16_t mavlink_msg_pattern_detected_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t type, float confidence, const char* file, uint8_t detected) { - uint16_t i = 0; + mavlink_pattern_detected_t *p = (mavlink_pattern_detected_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_PATTERN_DETECTED; - i += put_uint8_t_by_index(type, i, msg->payload); // 0: Pattern, 1: Letter - i += put_float_by_index(confidence, i, msg->payload); // Confidence of detection - i += put_array_by_index(file, 100, i, msg->payload); // Pattern file name - i += put_uint8_t_by_index(detected, i, msg->payload); // Accepted as true detection, 0 no, 1 yes + p->type = type; // uint8_t:0: Pattern, 1: Letter + p->confidence = confidence; // float:Confidence of detection + memcpy(p->file, file, sizeof(p->file)); // char[100]:Pattern file name + p->detected = detected; // uint8_t:Accepted as true detection, 0 no, 1 yes - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN); } /** @@ -51,17 +51,17 @@ static inline uint16_t mavlink_msg_pattern_detected_pack(uint8_t system_id, uint * @param detected Accepted as true detection, 0 no, 1 yes * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_pattern_detected_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t type, float confidence, const int8_t* file, uint8_t detected) +static inline uint16_t mavlink_msg_pattern_detected_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t type, float confidence, const char* file, uint8_t detected) { - uint16_t i = 0; + mavlink_pattern_detected_t *p = (mavlink_pattern_detected_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_PATTERN_DETECTED; - i += put_uint8_t_by_index(type, i, msg->payload); // 0: Pattern, 1: Letter - i += put_float_by_index(confidence, i, msg->payload); // Confidence of detection - i += put_array_by_index(file, 100, i, msg->payload); // Pattern file name - i += put_uint8_t_by_index(detected, i, msg->payload); // Accepted as true detection, 0 no, 1 yes + p->type = type; // uint8_t:0: Pattern, 1: Letter + p->confidence = confidence; // float:Confidence of detection + memcpy(p->file, file, sizeof(p->file)); // char[100]:Pattern file name + p->detected = detected; // uint8_t:Accepted as true detection, 0 no, 1 yes - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN); } /** @@ -86,13 +86,38 @@ static inline uint16_t mavlink_msg_pattern_detected_encode(uint8_t system_id, ui * @param file Pattern file name * @param detected Accepted as true detection, 0 no, 1 yes */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_pattern_detected_send(mavlink_channel_t chan, uint8_t type, float confidence, const int8_t* file, uint8_t detected) + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS +static inline void mavlink_msg_pattern_detected_send(mavlink_channel_t chan, uint8_t type, float confidence, const char* file, uint8_t detected) { - mavlink_message_t msg; - mavlink_msg_pattern_detected_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, type, confidence, file, detected); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_pattern_detected_t payload; + uint16_t checksum; + mavlink_pattern_detected_t *p = &payload; + + p->type = type; // uint8_t:0: Pattern, 1: Letter + p->confidence = confidence; // float:Confidence of detection + memcpy(p->file, file, sizeof(p->file)); // char[100]:Pattern file name + p->detected = detected; // uint8_t:Accepted as true detection, 0 no, 1 yes + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_PATTERN_DETECTED_LEN; + hdr.msgid = MAVLINK_MSG_ID_PATTERN_DETECTED; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -105,7 +130,8 @@ static inline void mavlink_msg_pattern_detected_send(mavlink_channel_t chan, uin */ static inline uint8_t mavlink_msg_pattern_detected_get_type(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_pattern_detected_t *p = (mavlink_pattern_detected_t *)&msg->payload[0]; + return (uint8_t)(p->type); } /** @@ -115,12 +141,8 @@ static inline uint8_t mavlink_msg_pattern_detected_get_type(const mavlink_messag */ static inline float mavlink_msg_pattern_detected_get_confidence(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t))[3]; - return (float)r.f; + mavlink_pattern_detected_t *p = (mavlink_pattern_detected_t *)&msg->payload[0]; + return (float)(p->confidence); } /** @@ -128,11 +150,12 @@ static inline float mavlink_msg_pattern_detected_get_confidence(const mavlink_me * * @return Pattern file name */ -static inline uint16_t mavlink_msg_pattern_detected_get_file(const mavlink_message_t* msg, int8_t* r_data) +static inline uint16_t mavlink_msg_pattern_detected_get_file(const mavlink_message_t* msg, char* file) { + mavlink_pattern_detected_t *p = (mavlink_pattern_detected_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint8_t)+sizeof(float), 100); - return 100; + memcpy(file, p->file, sizeof(p->file)); + return sizeof(p->file); } /** @@ -142,7 +165,8 @@ static inline uint16_t mavlink_msg_pattern_detected_get_file(const mavlink_messa */ static inline uint8_t mavlink_msg_pattern_detected_get_detected(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(float)+100)[0]; + mavlink_pattern_detected_t *p = (mavlink_pattern_detected_t *)&msg->payload[0]; + return (uint8_t)(p->detected); } /** @@ -153,8 +177,5 @@ static inline uint8_t mavlink_msg_pattern_detected_get_detected(const mavlink_me */ static inline void mavlink_msg_pattern_detected_decode(const mavlink_message_t* msg, mavlink_pattern_detected_t* pattern_detected) { - pattern_detected->type = mavlink_msg_pattern_detected_get_type(msg); - pattern_detected->confidence = mavlink_msg_pattern_detected_get_confidence(msg); - mavlink_msg_pattern_detected_get_file(msg, pattern_detected->file); - pattern_detected->detected = mavlink_msg_pattern_detected_get_detected(msg); + memcpy( pattern_detected, msg->payload, sizeof(mavlink_pattern_detected_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_point_of_interest.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_point_of_interest.h index 7e48c0fd913da62aa650594591bf7bda2faa7509..148d8f784e7e636b7390b65a41e21e1ff0bb7daf 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_point_of_interest.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_point_of_interest.h @@ -1,22 +1,22 @@ // MESSAGE POINT_OF_INTEREST PACKING #define MAVLINK_MSG_ID_POINT_OF_INTEREST 161 +#define MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN 43 +#define MAVLINK_MSG_161_LEN 43 typedef struct __mavlink_point_of_interest_t { - uint8_t type; ///< 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - uint8_t color; ///< 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - uint8_t coordinate_system; ///< 0: global, 1:local - uint16_t timeout; ///< 0: no timeout, >1: timeout in seconds float x; ///< X Position float y; ///< Y Position float z; ///< Z Position - int8_t name[25]; ///< POI name + uint16_t timeout; ///< 0: no timeout, >1: timeout in seconds + uint8_t type; ///< 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug + uint8_t color; ///< 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta + uint8_t coordinate_system; ///< 0: global, 1:local + char name[26]; ///< POI name } mavlink_point_of_interest_t; - -#define MAVLINK_MSG_POINT_OF_INTEREST_FIELD_NAME_LEN 25 - +#define MAVLINK_MSG_POINT_OF_INTEREST_FIELD_NAME_LEN 26 /** * @brief Pack a point_of_interest message @@ -34,21 +34,21 @@ typedef struct __mavlink_point_of_interest_t * @param name POI name * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_point_of_interest_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float x, float y, float z, const int8_t* name) +static inline uint16_t mavlink_msg_point_of_interest_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float x, float y, float z, const char* name) { - uint16_t i = 0; + mavlink_point_of_interest_t *p = (mavlink_point_of_interest_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST; - i += put_uint8_t_by_index(type, i, msg->payload); // 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - i += put_uint8_t_by_index(color, i, msg->payload); // 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - i += put_uint8_t_by_index(coordinate_system, i, msg->payload); // 0: global, 1:local - i += put_uint16_t_by_index(timeout, i, msg->payload); // 0: no timeout, >1: timeout in seconds - i += put_float_by_index(x, i, msg->payload); // X Position - i += put_float_by_index(y, i, msg->payload); // Y Position - i += put_float_by_index(z, i, msg->payload); // Z Position - i += put_array_by_index(name, 25, i, msg->payload); // POI name + p->type = type; // uint8_t:0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug + p->color = color; // uint8_t:0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta + p->coordinate_system = coordinate_system; // uint8_t:0: global, 1:local + p->timeout = timeout; // uint16_t:0: no timeout, >1: timeout in seconds + p->x = x; // float:X Position + p->y = y; // float:Y Position + p->z = z; // float:Z Position + memcpy(p->name, name, sizeof(p->name)); // char[26]:POI name - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN); } /** @@ -67,21 +67,21 @@ static inline uint16_t mavlink_msg_point_of_interest_pack(uint8_t system_id, uin * @param name POI name * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_point_of_interest_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float x, float y, float z, const int8_t* name) +static inline uint16_t mavlink_msg_point_of_interest_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float x, float y, float z, const char* name) { - uint16_t i = 0; + mavlink_point_of_interest_t *p = (mavlink_point_of_interest_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST; - i += put_uint8_t_by_index(type, i, msg->payload); // 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - i += put_uint8_t_by_index(color, i, msg->payload); // 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - i += put_uint8_t_by_index(coordinate_system, i, msg->payload); // 0: global, 1:local - i += put_uint16_t_by_index(timeout, i, msg->payload); // 0: no timeout, >1: timeout in seconds - i += put_float_by_index(x, i, msg->payload); // X Position - i += put_float_by_index(y, i, msg->payload); // Y Position - i += put_float_by_index(z, i, msg->payload); // Z Position - i += put_array_by_index(name, 25, i, msg->payload); // POI name + p->type = type; // uint8_t:0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug + p->color = color; // uint8_t:0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta + p->coordinate_system = coordinate_system; // uint8_t:0: global, 1:local + p->timeout = timeout; // uint16_t:0: no timeout, >1: timeout in seconds + p->x = x; // float:X Position + p->y = y; // float:Y Position + p->z = z; // float:Z Position + memcpy(p->name, name, sizeof(p->name)); // char[26]:POI name - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN); } /** @@ -110,13 +110,42 @@ static inline uint16_t mavlink_msg_point_of_interest_encode(uint8_t system_id, u * @param z Z Position * @param name POI name */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_point_of_interest_send(mavlink_channel_t chan, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float x, float y, float z, const int8_t* name) + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS +static inline void mavlink_msg_point_of_interest_send(mavlink_channel_t chan, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float x, float y, float z, const char* name) { - mavlink_message_t msg; - mavlink_msg_point_of_interest_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, type, color, coordinate_system, timeout, x, y, z, name); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_point_of_interest_t payload; + uint16_t checksum; + mavlink_point_of_interest_t *p = &payload; + + p->type = type; // uint8_t:0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug + p->color = color; // uint8_t:0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta + p->coordinate_system = coordinate_system; // uint8_t:0: global, 1:local + p->timeout = timeout; // uint16_t:0: no timeout, >1: timeout in seconds + p->x = x; // float:X Position + p->y = y; // float:Y Position + p->z = z; // float:Z Position + memcpy(p->name, name, sizeof(p->name)); // char[26]:POI name + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN; + hdr.msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -129,7 +158,8 @@ static inline void mavlink_msg_point_of_interest_send(mavlink_channel_t chan, ui */ static inline uint8_t mavlink_msg_point_of_interest_get_type(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_point_of_interest_t *p = (mavlink_point_of_interest_t *)&msg->payload[0]; + return (uint8_t)(p->type); } /** @@ -139,7 +169,8 @@ static inline uint8_t mavlink_msg_point_of_interest_get_type(const mavlink_messa */ static inline uint8_t mavlink_msg_point_of_interest_get_color(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_point_of_interest_t *p = (mavlink_point_of_interest_t *)&msg->payload[0]; + return (uint8_t)(p->color); } /** @@ -149,7 +180,8 @@ static inline uint8_t mavlink_msg_point_of_interest_get_color(const mavlink_mess */ static inline uint8_t mavlink_msg_point_of_interest_get_coordinate_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_point_of_interest_t *p = (mavlink_point_of_interest_t *)&msg->payload[0]; + return (uint8_t)(p->coordinate_system); } /** @@ -159,10 +191,8 @@ static inline uint8_t mavlink_msg_point_of_interest_get_coordinate_system(const */ static inline uint16_t mavlink_msg_point_of_interest_get_timeout(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[1]; - return (uint16_t)r.s; + mavlink_point_of_interest_t *p = (mavlink_point_of_interest_t *)&msg->payload[0]; + return (uint16_t)(p->timeout); } /** @@ -172,12 +202,8 @@ static inline uint16_t mavlink_msg_point_of_interest_get_timeout(const mavlink_m */ static inline float mavlink_msg_point_of_interest_get_x(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[3]; - return (float)r.f; + mavlink_point_of_interest_t *p = (mavlink_point_of_interest_t *)&msg->payload[0]; + return (float)(p->x); } /** @@ -187,12 +213,8 @@ static inline float mavlink_msg_point_of_interest_get_x(const mavlink_message_t* */ static inline float mavlink_msg_point_of_interest_get_y(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float))[3]; - return (float)r.f; + mavlink_point_of_interest_t *p = (mavlink_point_of_interest_t *)&msg->payload[0]; + return (float)(p->y); } /** @@ -202,12 +224,8 @@ static inline float mavlink_msg_point_of_interest_get_y(const mavlink_message_t* */ static inline float mavlink_msg_point_of_interest_get_z(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_point_of_interest_t *p = (mavlink_point_of_interest_t *)&msg->payload[0]; + return (float)(p->z); } /** @@ -215,11 +233,12 @@ static inline float mavlink_msg_point_of_interest_get_z(const mavlink_message_t* * * @return POI name */ -static inline uint16_t mavlink_msg_point_of_interest_get_name(const mavlink_message_t* msg, int8_t* r_data) +static inline uint16_t mavlink_msg_point_of_interest_get_name(const mavlink_message_t* msg, char* name) { + mavlink_point_of_interest_t *p = (mavlink_point_of_interest_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float), 25); - return 25; + memcpy(name, p->name, sizeof(p->name)); + return sizeof(p->name); } /** @@ -230,12 +249,5 @@ static inline uint16_t mavlink_msg_point_of_interest_get_name(const mavlink_mess */ static inline void mavlink_msg_point_of_interest_decode(const mavlink_message_t* msg, mavlink_point_of_interest_t* point_of_interest) { - point_of_interest->type = mavlink_msg_point_of_interest_get_type(msg); - point_of_interest->color = mavlink_msg_point_of_interest_get_color(msg); - point_of_interest->coordinate_system = mavlink_msg_point_of_interest_get_coordinate_system(msg); - point_of_interest->timeout = mavlink_msg_point_of_interest_get_timeout(msg); - point_of_interest->x = mavlink_msg_point_of_interest_get_x(msg); - point_of_interest->y = mavlink_msg_point_of_interest_get_y(msg); - point_of_interest->z = mavlink_msg_point_of_interest_get_z(msg); - mavlink_msg_point_of_interest_get_name(msg, point_of_interest->name); + memcpy( point_of_interest, msg->payload, sizeof(mavlink_point_of_interest_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_point_of_interest_connection.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_point_of_interest_connection.h index fb3051746356ba92895d6a5f5c9d3635f8325bf3..7accdbc9c33f2fbd6b00215aea3a2a1f77cead57 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_point_of_interest_connection.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_point_of_interest_connection.h @@ -1,25 +1,25 @@ // MESSAGE POINT_OF_INTEREST_CONNECTION PACKING #define MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION 162 +#define MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN 55 +#define MAVLINK_MSG_162_LEN 55 typedef struct __mavlink_point_of_interest_connection_t { - uint8_t type; ///< 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - uint8_t color; ///< 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - uint8_t coordinate_system; ///< 0: global, 1:local - uint16_t timeout; ///< 0: no timeout, >1: timeout in seconds float xp1; ///< X1 Position float yp1; ///< Y1 Position float zp1; ///< Z1 Position float xp2; ///< X2 Position float yp2; ///< Y2 Position float zp2; ///< Z2 Position - int8_t name[25]; ///< POI connection name + uint16_t timeout; ///< 0: no timeout, >1: timeout in seconds + uint8_t type; ///< 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug + uint8_t color; ///< 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta + uint8_t coordinate_system; ///< 0: global, 1:local + char name[26]; ///< POI connection name } mavlink_point_of_interest_connection_t; - -#define MAVLINK_MSG_POINT_OF_INTEREST_CONNECTION_FIELD_NAME_LEN 25 - +#define MAVLINK_MSG_POINT_OF_INTEREST_CONNECTION_FIELD_NAME_LEN 26 /** * @brief Pack a point_of_interest_connection message @@ -40,24 +40,24 @@ typedef struct __mavlink_point_of_interest_connection_t * @param name POI connection name * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_point_of_interest_connection_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float xp1, float yp1, float zp1, float xp2, float yp2, float zp2, const int8_t* name) +static inline uint16_t mavlink_msg_point_of_interest_connection_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float xp1, float yp1, float zp1, float xp2, float yp2, float zp2, const char* name) { - uint16_t i = 0; + mavlink_point_of_interest_connection_t *p = (mavlink_point_of_interest_connection_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION; - i += put_uint8_t_by_index(type, i, msg->payload); // 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - i += put_uint8_t_by_index(color, i, msg->payload); // 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - i += put_uint8_t_by_index(coordinate_system, i, msg->payload); // 0: global, 1:local - i += put_uint16_t_by_index(timeout, i, msg->payload); // 0: no timeout, >1: timeout in seconds - i += put_float_by_index(xp1, i, msg->payload); // X1 Position - i += put_float_by_index(yp1, i, msg->payload); // Y1 Position - i += put_float_by_index(zp1, i, msg->payload); // Z1 Position - i += put_float_by_index(xp2, i, msg->payload); // X2 Position - i += put_float_by_index(yp2, i, msg->payload); // Y2 Position - i += put_float_by_index(zp2, i, msg->payload); // Z2 Position - i += put_array_by_index(name, 25, i, msg->payload); // POI connection name + p->type = type; // uint8_t:0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug + p->color = color; // uint8_t:0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta + p->coordinate_system = coordinate_system; // uint8_t:0: global, 1:local + p->timeout = timeout; // uint16_t:0: no timeout, >1: timeout in seconds + p->xp1 = xp1; // float:X1 Position + p->yp1 = yp1; // float:Y1 Position + p->zp1 = zp1; // float:Z1 Position + p->xp2 = xp2; // float:X2 Position + p->yp2 = yp2; // float:Y2 Position + p->zp2 = zp2; // float:Z2 Position + memcpy(p->name, name, sizeof(p->name)); // char[26]:POI connection name - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN); } /** @@ -79,24 +79,24 @@ static inline uint16_t mavlink_msg_point_of_interest_connection_pack(uint8_t sys * @param name POI connection name * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_point_of_interest_connection_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float xp1, float yp1, float zp1, float xp2, float yp2, float zp2, const int8_t* name) +static inline uint16_t mavlink_msg_point_of_interest_connection_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float xp1, float yp1, float zp1, float xp2, float yp2, float zp2, const char* name) { - uint16_t i = 0; + mavlink_point_of_interest_connection_t *p = (mavlink_point_of_interest_connection_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION; - i += put_uint8_t_by_index(type, i, msg->payload); // 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - i += put_uint8_t_by_index(color, i, msg->payload); // 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - i += put_uint8_t_by_index(coordinate_system, i, msg->payload); // 0: global, 1:local - i += put_uint16_t_by_index(timeout, i, msg->payload); // 0: no timeout, >1: timeout in seconds - i += put_float_by_index(xp1, i, msg->payload); // X1 Position - i += put_float_by_index(yp1, i, msg->payload); // Y1 Position - i += put_float_by_index(zp1, i, msg->payload); // Z1 Position - i += put_float_by_index(xp2, i, msg->payload); // X2 Position - i += put_float_by_index(yp2, i, msg->payload); // Y2 Position - i += put_float_by_index(zp2, i, msg->payload); // Z2 Position - i += put_array_by_index(name, 25, i, msg->payload); // POI connection name + p->type = type; // uint8_t:0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug + p->color = color; // uint8_t:0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta + p->coordinate_system = coordinate_system; // uint8_t:0: global, 1:local + p->timeout = timeout; // uint16_t:0: no timeout, >1: timeout in seconds + p->xp1 = xp1; // float:X1 Position + p->yp1 = yp1; // float:Y1 Position + p->zp1 = zp1; // float:Z1 Position + p->xp2 = xp2; // float:X2 Position + p->yp2 = yp2; // float:Y2 Position + p->zp2 = zp2; // float:Z2 Position + memcpy(p->name, name, sizeof(p->name)); // char[26]:POI connection name - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN); } /** @@ -128,13 +128,45 @@ static inline uint16_t mavlink_msg_point_of_interest_connection_encode(uint8_t s * @param zp2 Z2 Position * @param name POI connection name */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_point_of_interest_connection_send(mavlink_channel_t chan, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float xp1, float yp1, float zp1, float xp2, float yp2, float zp2, const int8_t* name) + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS +static inline void mavlink_msg_point_of_interest_connection_send(mavlink_channel_t chan, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float xp1, float yp1, float zp1, float xp2, float yp2, float zp2, const char* name) { - mavlink_message_t msg; - mavlink_msg_point_of_interest_connection_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, type, color, coordinate_system, timeout, xp1, yp1, zp1, xp2, yp2, zp2, name); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_point_of_interest_connection_t payload; + uint16_t checksum; + mavlink_point_of_interest_connection_t *p = &payload; + + p->type = type; // uint8_t:0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug + p->color = color; // uint8_t:0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta + p->coordinate_system = coordinate_system; // uint8_t:0: global, 1:local + p->timeout = timeout; // uint16_t:0: no timeout, >1: timeout in seconds + p->xp1 = xp1; // float:X1 Position + p->yp1 = yp1; // float:Y1 Position + p->zp1 = zp1; // float:Z1 Position + p->xp2 = xp2; // float:X2 Position + p->yp2 = yp2; // float:Y2 Position + p->zp2 = zp2; // float:Z2 Position + memcpy(p->name, name, sizeof(p->name)); // char[26]:POI connection name + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN; + hdr.msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -147,7 +179,8 @@ static inline void mavlink_msg_point_of_interest_connection_send(mavlink_channel */ static inline uint8_t mavlink_msg_point_of_interest_connection_get_type(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_point_of_interest_connection_t *p = (mavlink_point_of_interest_connection_t *)&msg->payload[0]; + return (uint8_t)(p->type); } /** @@ -157,7 +190,8 @@ static inline uint8_t mavlink_msg_point_of_interest_connection_get_type(const ma */ static inline uint8_t mavlink_msg_point_of_interest_connection_get_color(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_point_of_interest_connection_t *p = (mavlink_point_of_interest_connection_t *)&msg->payload[0]; + return (uint8_t)(p->color); } /** @@ -167,7 +201,8 @@ static inline uint8_t mavlink_msg_point_of_interest_connection_get_color(const m */ static inline uint8_t mavlink_msg_point_of_interest_connection_get_coordinate_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_point_of_interest_connection_t *p = (mavlink_point_of_interest_connection_t *)&msg->payload[0]; + return (uint8_t)(p->coordinate_system); } /** @@ -177,10 +212,8 @@ static inline uint8_t mavlink_msg_point_of_interest_connection_get_coordinate_sy */ static inline uint16_t mavlink_msg_point_of_interest_connection_get_timeout(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[1]; - return (uint16_t)r.s; + mavlink_point_of_interest_connection_t *p = (mavlink_point_of_interest_connection_t *)&msg->payload[0]; + return (uint16_t)(p->timeout); } /** @@ -190,12 +223,8 @@ static inline uint16_t mavlink_msg_point_of_interest_connection_get_timeout(cons */ static inline float mavlink_msg_point_of_interest_connection_get_xp1(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[3]; - return (float)r.f; + mavlink_point_of_interest_connection_t *p = (mavlink_point_of_interest_connection_t *)&msg->payload[0]; + return (float)(p->xp1); } /** @@ -205,12 +234,8 @@ static inline float mavlink_msg_point_of_interest_connection_get_xp1(const mavli */ static inline float mavlink_msg_point_of_interest_connection_get_yp1(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float))[3]; - return (float)r.f; + mavlink_point_of_interest_connection_t *p = (mavlink_point_of_interest_connection_t *)&msg->payload[0]; + return (float)(p->yp1); } /** @@ -220,12 +245,8 @@ static inline float mavlink_msg_point_of_interest_connection_get_yp1(const mavli */ static inline float mavlink_msg_point_of_interest_connection_get_zp1(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_point_of_interest_connection_t *p = (mavlink_point_of_interest_connection_t *)&msg->payload[0]; + return (float)(p->zp1); } /** @@ -235,12 +256,8 @@ static inline float mavlink_msg_point_of_interest_connection_get_zp1(const mavli */ static inline float mavlink_msg_point_of_interest_connection_get_xp2(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_point_of_interest_connection_t *p = (mavlink_point_of_interest_connection_t *)&msg->payload[0]; + return (float)(p->xp2); } /** @@ -250,12 +267,8 @@ static inline float mavlink_msg_point_of_interest_connection_get_xp2(const mavli */ static inline float mavlink_msg_point_of_interest_connection_get_yp2(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_point_of_interest_connection_t *p = (mavlink_point_of_interest_connection_t *)&msg->payload[0]; + return (float)(p->yp2); } /** @@ -265,12 +278,8 @@ static inline float mavlink_msg_point_of_interest_connection_get_yp2(const mavli */ static inline float mavlink_msg_point_of_interest_connection_get_zp2(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_point_of_interest_connection_t *p = (mavlink_point_of_interest_connection_t *)&msg->payload[0]; + return (float)(p->zp2); } /** @@ -278,11 +287,12 @@ static inline float mavlink_msg_point_of_interest_connection_get_zp2(const mavli * * @return POI connection name */ -static inline uint16_t mavlink_msg_point_of_interest_connection_get_name(const mavlink_message_t* msg, int8_t* r_data) +static inline uint16_t mavlink_msg_point_of_interest_connection_get_name(const mavlink_message_t* msg, char* name) { + mavlink_point_of_interest_connection_t *p = (mavlink_point_of_interest_connection_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float), 25); - return 25; + memcpy(name, p->name, sizeof(p->name)); + return sizeof(p->name); } /** @@ -293,15 +303,5 @@ static inline uint16_t mavlink_msg_point_of_interest_connection_get_name(const m */ static inline void mavlink_msg_point_of_interest_connection_decode(const mavlink_message_t* msg, mavlink_point_of_interest_connection_t* point_of_interest_connection) { - point_of_interest_connection->type = mavlink_msg_point_of_interest_connection_get_type(msg); - point_of_interest_connection->color = mavlink_msg_point_of_interest_connection_get_color(msg); - point_of_interest_connection->coordinate_system = mavlink_msg_point_of_interest_connection_get_coordinate_system(msg); - point_of_interest_connection->timeout = mavlink_msg_point_of_interest_connection_get_timeout(msg); - point_of_interest_connection->xp1 = mavlink_msg_point_of_interest_connection_get_xp1(msg); - point_of_interest_connection->yp1 = mavlink_msg_point_of_interest_connection_get_yp1(msg); - point_of_interest_connection->zp1 = mavlink_msg_point_of_interest_connection_get_zp1(msg); - point_of_interest_connection->xp2 = mavlink_msg_point_of_interest_connection_get_xp2(msg); - point_of_interest_connection->yp2 = mavlink_msg_point_of_interest_connection_get_yp2(msg); - point_of_interest_connection->zp2 = mavlink_msg_point_of_interest_connection_get_zp2(msg); - mavlink_msg_point_of_interest_connection_get_name(msg, point_of_interest_connection->name); + memcpy( point_of_interest_connection, msg->payload, sizeof(mavlink_point_of_interest_connection_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_offset_set.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_offset_set.h index 30f076bba896df52d847ad3b19b6b65ae4dafc7c..b5446931f4dce3cda1d518540bb9f46c0e569dfb 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_offset_set.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_offset_set.h @@ -1,20 +1,20 @@ // MESSAGE POSITION_CONTROL_OFFSET_SET PACKING #define MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET 154 +#define MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET_LEN 18 +#define MAVLINK_MSG_154_LEN 18 typedef struct __mavlink_position_control_offset_set_t { - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID float x; ///< x position offset float y; ///< y position offset float z; ///< z position offset float yaw; ///< yaw orientation offset in radians, 0 = NORTH + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID } mavlink_position_control_offset_set_t; - - /** * @brief Pack a position_control_offset_set message * @param system_id ID of this system @@ -31,17 +31,17 @@ typedef struct __mavlink_position_control_offset_set_t */ static inline uint16_t mavlink_msg_position_control_offset_set_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, float x, float y, float z, float yaw) { - uint16_t i = 0; + mavlink_position_control_offset_set_t *p = (mavlink_position_control_offset_set_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET; - 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(x, i, msg->payload); // x position offset - i += put_float_by_index(y, i, msg->payload); // y position offset - i += put_float_by_index(z, i, msg->payload); // z position offset - i += put_float_by_index(yaw, i, msg->payload); // yaw orientation offset in radians, 0 = NORTH + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->x = x; // float:x position offset + p->y = y; // float:y position offset + p->z = z; // float:z position offset + p->yaw = yaw; // float:yaw orientation offset in radians, 0 = NORTH - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET_LEN); } /** @@ -60,17 +60,17 @@ static inline uint16_t mavlink_msg_position_control_offset_set_pack(uint8_t syst */ static inline uint16_t mavlink_msg_position_control_offset_set_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 x, float y, float z, float yaw) { - uint16_t i = 0; + mavlink_position_control_offset_set_t *p = (mavlink_position_control_offset_set_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET; - 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(x, i, msg->payload); // x position offset - i += put_float_by_index(y, i, msg->payload); // y position offset - i += put_float_by_index(z, i, msg->payload); // z position offset - i += put_float_by_index(yaw, i, msg->payload); // yaw orientation offset in radians, 0 = NORTH + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->x = x; // float:x position offset + p->y = y; // float:y position offset + p->z = z; // float:z position offset + p->yaw = yaw; // float:yaw orientation offset in radians, 0 = NORTH - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET_LEN); } /** @@ -97,13 +97,40 @@ static inline uint16_t mavlink_msg_position_control_offset_set_encode(uint8_t sy * @param z z position offset * @param yaw yaw orientation offset in radians, 0 = NORTH */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_position_control_offset_set_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float x, float y, float z, float yaw) { - mavlink_message_t msg; - mavlink_msg_position_control_offset_set_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, x, y, z, yaw); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_position_control_offset_set_t payload; + uint16_t checksum; + mavlink_position_control_offset_set_t *p = &payload; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->x = x; // float:x position offset + p->y = y; // float:y position offset + p->z = z; // float:z position offset + p->yaw = yaw; // float:yaw orientation offset in radians, 0 = NORTH + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET_LEN; + hdr.msgid = MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -116,7 +143,8 @@ static inline void mavlink_msg_position_control_offset_set_send(mavlink_channel_ */ static inline uint8_t mavlink_msg_position_control_offset_set_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_position_control_offset_set_t *p = (mavlink_position_control_offset_set_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); } /** @@ -126,7 +154,8 @@ static inline uint8_t mavlink_msg_position_control_offset_set_get_target_system( */ static inline uint8_t mavlink_msg_position_control_offset_set_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_position_control_offset_set_t *p = (mavlink_position_control_offset_set_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); } /** @@ -136,12 +165,8 @@ static inline uint8_t mavlink_msg_position_control_offset_set_get_target_compone */ static inline float mavlink_msg_position_control_offset_set_get_x(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; + mavlink_position_control_offset_set_t *p = (mavlink_position_control_offset_set_t *)&msg->payload[0]; + return (float)(p->x); } /** @@ -151,12 +176,8 @@ static inline float mavlink_msg_position_control_offset_set_get_x(const mavlink_ */ static inline float mavlink_msg_position_control_offset_set_get_y(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; + mavlink_position_control_offset_set_t *p = (mavlink_position_control_offset_set_t *)&msg->payload[0]; + return (float)(p->y); } /** @@ -166,12 +187,8 @@ static inline float mavlink_msg_position_control_offset_set_get_y(const mavlink_ */ static inline float mavlink_msg_position_control_offset_set_get_z(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; + mavlink_position_control_offset_set_t *p = (mavlink_position_control_offset_set_t *)&msg->payload[0]; + return (float)(p->z); } /** @@ -181,12 +198,8 @@ static inline float mavlink_msg_position_control_offset_set_get_z(const mavlink_ */ static inline float mavlink_msg_position_control_offset_set_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)+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; + mavlink_position_control_offset_set_t *p = (mavlink_position_control_offset_set_t *)&msg->payload[0]; + return (float)(p->yaw); } /** @@ -197,10 +210,5 @@ static inline float mavlink_msg_position_control_offset_set_get_yaw(const mavlin */ static inline void mavlink_msg_position_control_offset_set_decode(const mavlink_message_t* msg, mavlink_position_control_offset_set_t* position_control_offset_set) { - position_control_offset_set->target_system = mavlink_msg_position_control_offset_set_get_target_system(msg); - position_control_offset_set->target_component = mavlink_msg_position_control_offset_set_get_target_component(msg); - position_control_offset_set->x = mavlink_msg_position_control_offset_set_get_x(msg); - position_control_offset_set->y = mavlink_msg_position_control_offset_set_get_y(msg); - position_control_offset_set->z = mavlink_msg_position_control_offset_set_get_z(msg); - position_control_offset_set->yaw = mavlink_msg_position_control_offset_set_get_yaw(msg); + memcpy( position_control_offset_set, msg->payload, sizeof(mavlink_position_control_offset_set_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_setpoint.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_setpoint.h index 59bcb131ce5e0e1c8e08a32145f99147144ee7ba..dd3b92ca8cb78540540608cd20870cb884ba604b 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_setpoint.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_setpoint.h @@ -1,19 +1,19 @@ // MESSAGE POSITION_CONTROL_SETPOINT PACKING #define MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT 121 +#define MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN 18 +#define MAVLINK_MSG_121_LEN 18 typedef struct __mavlink_position_control_setpoint_t { - uint16_t id; ///< ID of waypoint, 0 for plain position float x; ///< x position float y; ///< y position float z; ///< z position float yaw; ///< yaw orientation in radians, 0 = NORTH + uint16_t id; ///< ID of waypoint, 0 for plain position } mavlink_position_control_setpoint_t; - - /** * @brief Pack a position_control_setpoint message * @param system_id ID of this system @@ -29,16 +29,16 @@ typedef struct __mavlink_position_control_setpoint_t */ static inline uint16_t mavlink_msg_position_control_setpoint_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint16_t id, float x, float y, float z, float yaw) { - uint16_t i = 0; + mavlink_position_control_setpoint_t *p = (mavlink_position_control_setpoint_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT; - i += put_uint16_t_by_index(id, i, msg->payload); // ID of waypoint, 0 for plain position - i += put_float_by_index(x, i, msg->payload); // x position - i += put_float_by_index(y, i, msg->payload); // y position - i += put_float_by_index(z, i, msg->payload); // z position - i += put_float_by_index(yaw, i, msg->payload); // yaw orientation in radians, 0 = NORTH + p->id = id; // uint16_t:ID of waypoint, 0 for plain position + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float:yaw orientation in radians, 0 = NORTH - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN); } /** @@ -56,16 +56,16 @@ static inline uint16_t mavlink_msg_position_control_setpoint_pack(uint8_t system */ static inline uint16_t mavlink_msg_position_control_setpoint_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint16_t id, float x, float y, float z, float yaw) { - uint16_t i = 0; + mavlink_position_control_setpoint_t *p = (mavlink_position_control_setpoint_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT; - i += put_uint16_t_by_index(id, i, msg->payload); // ID of waypoint, 0 for plain position - i += put_float_by_index(x, i, msg->payload); // x position - i += put_float_by_index(y, i, msg->payload); // y position - i += put_float_by_index(z, i, msg->payload); // z position - i += put_float_by_index(yaw, i, msg->payload); // yaw orientation in radians, 0 = NORTH + p->id = id; // uint16_t:ID of waypoint, 0 for plain position + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float:yaw orientation in radians, 0 = NORTH - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN); } /** @@ -91,13 +91,39 @@ static inline uint16_t mavlink_msg_position_control_setpoint_encode(uint8_t syst * @param z z position * @param yaw yaw orientation in radians, 0 = NORTH */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_position_control_setpoint_send(mavlink_channel_t chan, uint16_t id, float x, float y, float z, float yaw) { - mavlink_message_t msg; - mavlink_msg_position_control_setpoint_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, id, x, y, z, yaw); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_position_control_setpoint_t payload; + uint16_t checksum; + mavlink_position_control_setpoint_t *p = &payload; + + p->id = id; // uint16_t:ID of waypoint, 0 for plain position + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float:yaw orientation in radians, 0 = NORTH + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN; + hdr.msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -110,10 +136,8 @@ static inline void mavlink_msg_position_control_setpoint_send(mavlink_channel_t */ static inline uint16_t mavlink_msg_position_control_setpoint_get_id(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload)[0]; - r.b[0] = (msg->payload)[1]; - return (uint16_t)r.s; + mavlink_position_control_setpoint_t *p = (mavlink_position_control_setpoint_t *)&msg->payload[0]; + return (uint16_t)(p->id); } /** @@ -123,12 +147,8 @@ static inline uint16_t mavlink_msg_position_control_setpoint_get_id(const mavlin */ static inline float mavlink_msg_position_control_setpoint_get_x(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint16_t))[0]; - r.b[2] = (msg->payload+sizeof(uint16_t))[1]; - r.b[1] = (msg->payload+sizeof(uint16_t))[2]; - r.b[0] = (msg->payload+sizeof(uint16_t))[3]; - return (float)r.f; + mavlink_position_control_setpoint_t *p = (mavlink_position_control_setpoint_t *)&msg->payload[0]; + return (float)(p->x); } /** @@ -138,12 +158,8 @@ static inline float mavlink_msg_position_control_setpoint_get_x(const mavlink_me */ static inline float mavlink_msg_position_control_setpoint_get_y(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint16_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint16_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(float))[3]; - return (float)r.f; + mavlink_position_control_setpoint_t *p = (mavlink_position_control_setpoint_t *)&msg->payload[0]; + return (float)(p->y); } /** @@ -153,12 +169,8 @@ static inline float mavlink_msg_position_control_setpoint_get_y(const mavlink_me */ static inline float mavlink_msg_position_control_setpoint_get_z(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_position_control_setpoint_t *p = (mavlink_position_control_setpoint_t *)&msg->payload[0]; + return (float)(p->z); } /** @@ -168,12 +180,8 @@ static inline float mavlink_msg_position_control_setpoint_get_z(const mavlink_me */ static inline float mavlink_msg_position_control_setpoint_get_yaw(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_position_control_setpoint_t *p = (mavlink_position_control_setpoint_t *)&msg->payload[0]; + return (float)(p->yaw); } /** @@ -184,9 +192,5 @@ static inline float mavlink_msg_position_control_setpoint_get_yaw(const mavlink_ */ static inline void mavlink_msg_position_control_setpoint_decode(const mavlink_message_t* msg, mavlink_position_control_setpoint_t* position_control_setpoint) { - position_control_setpoint->id = mavlink_msg_position_control_setpoint_get_id(msg); - position_control_setpoint->x = mavlink_msg_position_control_setpoint_get_x(msg); - position_control_setpoint->y = mavlink_msg_position_control_setpoint_get_y(msg); - position_control_setpoint->z = mavlink_msg_position_control_setpoint_get_z(msg); - position_control_setpoint->yaw = mavlink_msg_position_control_setpoint_get_yaw(msg); + memcpy( position_control_setpoint, msg->payload, sizeof(mavlink_position_control_setpoint_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_setpoint_set.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_setpoint_set.h index 75150e66b9aab096b8cac43183d2996a6f8a919f..f14a31c467889ecc03b85d71f9478532c363b8bd 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_setpoint_set.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_setpoint_set.h @@ -1,21 +1,21 @@ // MESSAGE POSITION_CONTROL_SETPOINT_SET PACKING #define MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET 120 +#define MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET_LEN 20 +#define MAVLINK_MSG_120_LEN 20 typedef struct __mavlink_position_control_setpoint_set_t { - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - uint16_t id; ///< ID of waypoint, 0 for plain position float x; ///< x position float y; ///< y position float z; ///< z position float yaw; ///< yaw orientation in radians, 0 = NORTH + uint16_t id; ///< ID of waypoint, 0 for plain position + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID } mavlink_position_control_setpoint_set_t; - - /** * @brief Pack a position_control_setpoint_set message * @param system_id ID of this system @@ -33,18 +33,18 @@ typedef struct __mavlink_position_control_setpoint_set_t */ static inline uint16_t mavlink_msg_position_control_setpoint_set_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint16_t id, float x, float y, float z, float yaw) { - uint16_t i = 0; + mavlink_position_control_setpoint_set_t *p = (mavlink_position_control_setpoint_set_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET; - 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(id, i, msg->payload); // ID of waypoint, 0 for plain position - i += put_float_by_index(x, i, msg->payload); // x position - i += put_float_by_index(y, i, msg->payload); // y position - i += put_float_by_index(z, i, msg->payload); // z position - i += put_float_by_index(yaw, i, msg->payload); // yaw orientation in radians, 0 = NORTH + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->id = id; // uint16_t:ID of waypoint, 0 for plain position + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float:yaw orientation in radians, 0 = NORTH - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET_LEN); } /** @@ -64,18 +64,18 @@ static inline uint16_t mavlink_msg_position_control_setpoint_set_pack(uint8_t sy */ static inline uint16_t mavlink_msg_position_control_setpoint_set_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 id, float x, float y, float z, float yaw) { - uint16_t i = 0; + mavlink_position_control_setpoint_set_t *p = (mavlink_position_control_setpoint_set_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET; - 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(id, i, msg->payload); // ID of waypoint, 0 for plain position - i += put_float_by_index(x, i, msg->payload); // x position - i += put_float_by_index(y, i, msg->payload); // y position - i += put_float_by_index(z, i, msg->payload); // z position - i += put_float_by_index(yaw, i, msg->payload); // yaw orientation in radians, 0 = NORTH + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->id = id; // uint16_t:ID of waypoint, 0 for plain position + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float:yaw orientation in radians, 0 = NORTH - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET_LEN); } /** @@ -103,13 +103,41 @@ static inline uint16_t mavlink_msg_position_control_setpoint_set_encode(uint8_t * @param z z position * @param yaw yaw orientation in radians, 0 = NORTH */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_position_control_setpoint_set_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t id, float x, float y, float z, float yaw) { - mavlink_message_t msg; - mavlink_msg_position_control_setpoint_set_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, id, x, y, z, yaw); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_position_control_setpoint_set_t payload; + uint16_t checksum; + mavlink_position_control_setpoint_set_t *p = &payload; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->id = id; // uint16_t:ID of waypoint, 0 for plain position + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float:yaw orientation in radians, 0 = NORTH + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET_LEN; + hdr.msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -122,7 +150,8 @@ static inline void mavlink_msg_position_control_setpoint_set_send(mavlink_channe */ static inline uint8_t mavlink_msg_position_control_setpoint_set_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_position_control_setpoint_set_t *p = (mavlink_position_control_setpoint_set_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); } /** @@ -132,7 +161,8 @@ static inline uint8_t mavlink_msg_position_control_setpoint_set_get_target_syste */ static inline uint8_t mavlink_msg_position_control_setpoint_set_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_position_control_setpoint_set_t *p = (mavlink_position_control_setpoint_set_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); } /** @@ -142,10 +172,8 @@ static inline uint8_t mavlink_msg_position_control_setpoint_set_get_target_compo */ static inline uint16_t mavlink_msg_position_control_setpoint_set_get_id(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; + mavlink_position_control_setpoint_set_t *p = (mavlink_position_control_setpoint_set_t *)&msg->payload[0]; + return (uint16_t)(p->id); } /** @@ -155,12 +183,8 @@ static inline uint16_t mavlink_msg_position_control_setpoint_set_get_id(const ma */ static inline float mavlink_msg_position_control_setpoint_set_get_x(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[3]; - return (float)r.f; + mavlink_position_control_setpoint_set_t *p = (mavlink_position_control_setpoint_set_t *)&msg->payload[0]; + return (float)(p->x); } /** @@ -170,12 +194,8 @@ static inline float mavlink_msg_position_control_setpoint_set_get_x(const mavlin */ static inline float mavlink_msg_position_control_setpoint_set_get_y(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float))[3]; - return (float)r.f; + mavlink_position_control_setpoint_set_t *p = (mavlink_position_control_setpoint_set_t *)&msg->payload[0]; + return (float)(p->y); } /** @@ -185,12 +205,8 @@ static inline float mavlink_msg_position_control_setpoint_set_get_y(const mavlin */ static inline float mavlink_msg_position_control_setpoint_set_get_z(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_position_control_setpoint_set_t *p = (mavlink_position_control_setpoint_set_t *)&msg->payload[0]; + return (float)(p->z); } /** @@ -200,12 +216,8 @@ static inline float mavlink_msg_position_control_setpoint_set_get_z(const mavlin */ static inline float mavlink_msg_position_control_setpoint_set_get_yaw(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_position_control_setpoint_set_t *p = (mavlink_position_control_setpoint_set_t *)&msg->payload[0]; + return (float)(p->yaw); } /** @@ -216,11 +228,5 @@ static inline float mavlink_msg_position_control_setpoint_set_get_yaw(const mavl */ static inline void mavlink_msg_position_control_setpoint_set_decode(const mavlink_message_t* msg, mavlink_position_control_setpoint_set_t* position_control_setpoint_set) { - position_control_setpoint_set->target_system = mavlink_msg_position_control_setpoint_set_get_target_system(msg); - position_control_setpoint_set->target_component = mavlink_msg_position_control_setpoint_set_get_target_component(msg); - position_control_setpoint_set->id = mavlink_msg_position_control_setpoint_set_get_id(msg); - position_control_setpoint_set->x = mavlink_msg_position_control_setpoint_set_get_x(msg); - position_control_setpoint_set->y = mavlink_msg_position_control_setpoint_set_get_y(msg); - position_control_setpoint_set->z = mavlink_msg_position_control_setpoint_set_get_z(msg); - position_control_setpoint_set->yaw = mavlink_msg_position_control_setpoint_set_get_yaw(msg); + memcpy( position_control_setpoint_set, msg->payload, sizeof(mavlink_position_control_setpoint_set_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_raw_aux.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_raw_aux.h index 61adee29708d3814ad5d24496170ffae4876991b..7bafeb4c99b9c77b1046d5873bac3815e936afaa 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_raw_aux.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_raw_aux.h @@ -1,21 +1,21 @@ // MESSAGE RAW_AUX PACKING #define MAVLINK_MSG_ID_RAW_AUX 141 +#define MAVLINK_MSG_ID_RAW_AUX_LEN 16 +#define MAVLINK_MSG_141_LEN 16 typedef struct __mavlink_raw_aux_t { + int32_t baro; ///< Barometric pressure (hecto Pascal) uint16_t adc1; ///< ADC1 (J405 ADC3, LPC2148 AD0.6) uint16_t adc2; ///< ADC2 (J405 ADC5, LPC2148 AD0.2) uint16_t adc3; ///< ADC3 (J405 ADC6, LPC2148 AD0.1) uint16_t adc4; ///< ADC4 (J405 ADC7, LPC2148 AD1.3) uint16_t vbat; ///< Battery voltage int16_t temp; ///< Temperature (degrees celcius) - int32_t baro; ///< Barometric pressure (hecto Pascal) } mavlink_raw_aux_t; - - /** * @brief Pack a raw_aux message * @param system_id ID of this system @@ -33,18 +33,18 @@ typedef struct __mavlink_raw_aux_t */ static inline uint16_t mavlink_msg_raw_aux_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint16_t adc1, uint16_t adc2, uint16_t adc3, uint16_t adc4, uint16_t vbat, int16_t temp, int32_t baro) { - uint16_t i = 0; + mavlink_raw_aux_t *p = (mavlink_raw_aux_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_RAW_AUX; - i += put_uint16_t_by_index(adc1, i, msg->payload); // ADC1 (J405 ADC3, LPC2148 AD0.6) - i += put_uint16_t_by_index(adc2, i, msg->payload); // ADC2 (J405 ADC5, LPC2148 AD0.2) - i += put_uint16_t_by_index(adc3, i, msg->payload); // ADC3 (J405 ADC6, LPC2148 AD0.1) - i += put_uint16_t_by_index(adc4, i, msg->payload); // ADC4 (J405 ADC7, LPC2148 AD1.3) - i += put_uint16_t_by_index(vbat, i, msg->payload); // Battery voltage - i += put_int16_t_by_index(temp, i, msg->payload); // Temperature (degrees celcius) - i += put_int32_t_by_index(baro, i, msg->payload); // Barometric pressure (hecto Pascal) + p->adc1 = adc1; // uint16_t:ADC1 (J405 ADC3, LPC2148 AD0.6) + p->adc2 = adc2; // uint16_t:ADC2 (J405 ADC5, LPC2148 AD0.2) + p->adc3 = adc3; // uint16_t:ADC3 (J405 ADC6, LPC2148 AD0.1) + p->adc4 = adc4; // uint16_t:ADC4 (J405 ADC7, LPC2148 AD1.3) + p->vbat = vbat; // uint16_t:Battery voltage + p->temp = temp; // int16_t:Temperature (degrees celcius) + p->baro = baro; // int32_t:Barometric pressure (hecto Pascal) - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RAW_AUX_LEN); } /** @@ -64,18 +64,18 @@ static inline uint16_t mavlink_msg_raw_aux_pack(uint8_t system_id, uint8_t compo */ static inline uint16_t mavlink_msg_raw_aux_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint16_t adc1, uint16_t adc2, uint16_t adc3, uint16_t adc4, uint16_t vbat, int16_t temp, int32_t baro) { - uint16_t i = 0; + mavlink_raw_aux_t *p = (mavlink_raw_aux_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_RAW_AUX; - i += put_uint16_t_by_index(adc1, i, msg->payload); // ADC1 (J405 ADC3, LPC2148 AD0.6) - i += put_uint16_t_by_index(adc2, i, msg->payload); // ADC2 (J405 ADC5, LPC2148 AD0.2) - i += put_uint16_t_by_index(adc3, i, msg->payload); // ADC3 (J405 ADC6, LPC2148 AD0.1) - i += put_uint16_t_by_index(adc4, i, msg->payload); // ADC4 (J405 ADC7, LPC2148 AD1.3) - i += put_uint16_t_by_index(vbat, i, msg->payload); // Battery voltage - i += put_int16_t_by_index(temp, i, msg->payload); // Temperature (degrees celcius) - i += put_int32_t_by_index(baro, i, msg->payload); // Barometric pressure (hecto Pascal) + p->adc1 = adc1; // uint16_t:ADC1 (J405 ADC3, LPC2148 AD0.6) + p->adc2 = adc2; // uint16_t:ADC2 (J405 ADC5, LPC2148 AD0.2) + p->adc3 = adc3; // uint16_t:ADC3 (J405 ADC6, LPC2148 AD0.1) + p->adc4 = adc4; // uint16_t:ADC4 (J405 ADC7, LPC2148 AD1.3) + p->vbat = vbat; // uint16_t:Battery voltage + p->temp = temp; // int16_t:Temperature (degrees celcius) + p->baro = baro; // int32_t:Barometric pressure (hecto Pascal) - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RAW_AUX_LEN); } /** @@ -103,13 +103,41 @@ static inline uint16_t mavlink_msg_raw_aux_encode(uint8_t system_id, uint8_t com * @param temp Temperature (degrees celcius) * @param baro Barometric pressure (hecto Pascal) */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_raw_aux_send(mavlink_channel_t chan, uint16_t adc1, uint16_t adc2, uint16_t adc3, uint16_t adc4, uint16_t vbat, int16_t temp, int32_t baro) { - mavlink_message_t msg; - mavlink_msg_raw_aux_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, adc1, adc2, adc3, adc4, vbat, temp, baro); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_raw_aux_t payload; + uint16_t checksum; + mavlink_raw_aux_t *p = &payload; + + p->adc1 = adc1; // uint16_t:ADC1 (J405 ADC3, LPC2148 AD0.6) + p->adc2 = adc2; // uint16_t:ADC2 (J405 ADC5, LPC2148 AD0.2) + p->adc3 = adc3; // uint16_t:ADC3 (J405 ADC6, LPC2148 AD0.1) + p->adc4 = adc4; // uint16_t:ADC4 (J405 ADC7, LPC2148 AD1.3) + p->vbat = vbat; // uint16_t:Battery voltage + p->temp = temp; // int16_t:Temperature (degrees celcius) + p->baro = baro; // int32_t:Barometric pressure (hecto Pascal) + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_RAW_AUX_LEN; + hdr.msgid = MAVLINK_MSG_ID_RAW_AUX; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -122,10 +150,8 @@ static inline void mavlink_msg_raw_aux_send(mavlink_channel_t chan, uint16_t adc */ static inline uint16_t mavlink_msg_raw_aux_get_adc1(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload)[0]; - r.b[0] = (msg->payload)[1]; - return (uint16_t)r.s; + mavlink_raw_aux_t *p = (mavlink_raw_aux_t *)&msg->payload[0]; + return (uint16_t)(p->adc1); } /** @@ -135,10 +161,8 @@ static inline uint16_t mavlink_msg_raw_aux_get_adc1(const mavlink_message_t* msg */ static inline uint16_t mavlink_msg_raw_aux_get_adc2(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_raw_aux_t *p = (mavlink_raw_aux_t *)&msg->payload[0]; + return (uint16_t)(p->adc2); } /** @@ -148,10 +172,8 @@ static inline uint16_t mavlink_msg_raw_aux_get_adc2(const mavlink_message_t* msg */ static inline uint16_t mavlink_msg_raw_aux_get_adc3(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_raw_aux_t *p = (mavlink_raw_aux_t *)&msg->payload[0]; + return (uint16_t)(p->adc3); } /** @@ -161,10 +183,8 @@ static inline uint16_t mavlink_msg_raw_aux_get_adc3(const mavlink_message_t* msg */ static inline uint16_t mavlink_msg_raw_aux_get_adc4(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_raw_aux_t *p = (mavlink_raw_aux_t *)&msg->payload[0]; + return (uint16_t)(p->adc4); } /** @@ -174,10 +194,8 @@ static inline uint16_t mavlink_msg_raw_aux_get_adc4(const mavlink_message_t* msg */ static inline uint16_t mavlink_msg_raw_aux_get_vbat(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_raw_aux_t *p = (mavlink_raw_aux_t *)&msg->payload[0]; + return (uint16_t)(p->vbat); } /** @@ -187,10 +205,8 @@ static inline uint16_t mavlink_msg_raw_aux_get_vbat(const mavlink_message_t* msg */ static inline int16_t mavlink_msg_raw_aux_get_temp(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (int16_t)r.s; + mavlink_raw_aux_t *p = (mavlink_raw_aux_t *)&msg->payload[0]; + return (int16_t)(p->temp); } /** @@ -200,12 +216,8 @@ static inline int16_t mavlink_msg_raw_aux_get_temp(const mavlink_message_t* msg) */ static inline int32_t mavlink_msg_raw_aux_get_baro(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(int16_t))[0]; - r.b[2] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(int16_t))[1]; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(int16_t))[2]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(int16_t))[3]; - return (int32_t)r.i; + mavlink_raw_aux_t *p = (mavlink_raw_aux_t *)&msg->payload[0]; + return (int32_t)(p->baro); } /** @@ -216,11 +228,5 @@ static inline int32_t mavlink_msg_raw_aux_get_baro(const mavlink_message_t* msg) */ static inline void mavlink_msg_raw_aux_decode(const mavlink_message_t* msg, mavlink_raw_aux_t* raw_aux) { - raw_aux->adc1 = mavlink_msg_raw_aux_get_adc1(msg); - raw_aux->adc2 = mavlink_msg_raw_aux_get_adc2(msg); - raw_aux->adc3 = mavlink_msg_raw_aux_get_adc3(msg); - raw_aux->adc4 = mavlink_msg_raw_aux_get_adc4(msg); - raw_aux->vbat = mavlink_msg_raw_aux_get_vbat(msg); - raw_aux->temp = mavlink_msg_raw_aux_get_temp(msg); - raw_aux->baro = mavlink_msg_raw_aux_get_baro(msg); + memcpy( raw_aux, msg->payload, sizeof(mavlink_raw_aux_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_set_cam_shutter.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_set_cam_shutter.h index 6a34b7e4bd3c97d9990a32b71f5ce42a1736301b..5b14662365bd32633880816326a7533f34a57f51 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_set_cam_shutter.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_set_cam_shutter.h @@ -1,20 +1,20 @@ // MESSAGE SET_CAM_SHUTTER PACKING #define MAVLINK_MSG_ID_SET_CAM_SHUTTER 100 +#define MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN 11 +#define MAVLINK_MSG_100_LEN 11 typedef struct __mavlink_set_cam_shutter_t { + float gain; ///< Camera gain + uint16_t interval; ///< Shutter interval, in microseconds + uint16_t exposure; ///< Exposure time, in microseconds uint8_t cam_no; ///< Camera id uint8_t cam_mode; ///< Camera mode: 0 = auto, 1 = manual uint8_t trigger_pin; ///< Trigger pin, 0-3 for PtGrey FireFly - uint16_t interval; ///< Shutter interval, in microseconds - uint16_t exposure; ///< Exposure time, in microseconds - float gain; ///< Camera gain } mavlink_set_cam_shutter_t; - - /** * @brief Pack a set_cam_shutter message * @param system_id ID of this system @@ -31,17 +31,17 @@ typedef struct __mavlink_set_cam_shutter_t */ static inline uint16_t mavlink_msg_set_cam_shutter_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t cam_no, uint8_t cam_mode, uint8_t trigger_pin, uint16_t interval, uint16_t exposure, float gain) { - uint16_t i = 0; + mavlink_set_cam_shutter_t *p = (mavlink_set_cam_shutter_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SET_CAM_SHUTTER; - i += put_uint8_t_by_index(cam_no, i, msg->payload); // Camera id - i += put_uint8_t_by_index(cam_mode, i, msg->payload); // Camera mode: 0 = auto, 1 = manual - i += put_uint8_t_by_index(trigger_pin, i, msg->payload); // Trigger pin, 0-3 for PtGrey FireFly - i += put_uint16_t_by_index(interval, i, msg->payload); // Shutter interval, in microseconds - i += put_uint16_t_by_index(exposure, i, msg->payload); // Exposure time, in microseconds - i += put_float_by_index(gain, i, msg->payload); // Camera gain + p->cam_no = cam_no; // uint8_t:Camera id + p->cam_mode = cam_mode; // uint8_t:Camera mode: 0 = auto, 1 = manual + p->trigger_pin = trigger_pin; // uint8_t:Trigger pin, 0-3 for PtGrey FireFly + p->interval = interval; // uint16_t:Shutter interval, in microseconds + p->exposure = exposure; // uint16_t:Exposure time, in microseconds + p->gain = gain; // float:Camera gain - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN); } /** @@ -60,17 +60,17 @@ static inline uint16_t mavlink_msg_set_cam_shutter_pack(uint8_t system_id, uint8 */ static inline uint16_t mavlink_msg_set_cam_shutter_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t cam_no, uint8_t cam_mode, uint8_t trigger_pin, uint16_t interval, uint16_t exposure, float gain) { - uint16_t i = 0; + mavlink_set_cam_shutter_t *p = (mavlink_set_cam_shutter_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SET_CAM_SHUTTER; - i += put_uint8_t_by_index(cam_no, i, msg->payload); // Camera id - i += put_uint8_t_by_index(cam_mode, i, msg->payload); // Camera mode: 0 = auto, 1 = manual - i += put_uint8_t_by_index(trigger_pin, i, msg->payload); // Trigger pin, 0-3 for PtGrey FireFly - i += put_uint16_t_by_index(interval, i, msg->payload); // Shutter interval, in microseconds - i += put_uint16_t_by_index(exposure, i, msg->payload); // Exposure time, in microseconds - i += put_float_by_index(gain, i, msg->payload); // Camera gain + p->cam_no = cam_no; // uint8_t:Camera id + p->cam_mode = cam_mode; // uint8_t:Camera mode: 0 = auto, 1 = manual + p->trigger_pin = trigger_pin; // uint8_t:Trigger pin, 0-3 for PtGrey FireFly + p->interval = interval; // uint16_t:Shutter interval, in microseconds + p->exposure = exposure; // uint16_t:Exposure time, in microseconds + p->gain = gain; // float:Camera gain - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN); } /** @@ -97,13 +97,40 @@ static inline uint16_t mavlink_msg_set_cam_shutter_encode(uint8_t system_id, uin * @param exposure Exposure time, in microseconds * @param gain Camera gain */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_set_cam_shutter_send(mavlink_channel_t chan, uint8_t cam_no, uint8_t cam_mode, uint8_t trigger_pin, uint16_t interval, uint16_t exposure, float gain) { - mavlink_message_t msg; - mavlink_msg_set_cam_shutter_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, cam_no, cam_mode, trigger_pin, interval, exposure, gain); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_set_cam_shutter_t payload; + uint16_t checksum; + mavlink_set_cam_shutter_t *p = &payload; + + p->cam_no = cam_no; // uint8_t:Camera id + p->cam_mode = cam_mode; // uint8_t:Camera mode: 0 = auto, 1 = manual + p->trigger_pin = trigger_pin; // uint8_t:Trigger pin, 0-3 for PtGrey FireFly + p->interval = interval; // uint16_t:Shutter interval, in microseconds + p->exposure = exposure; // uint16_t:Exposure time, in microseconds + p->gain = gain; // float:Camera gain + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN; + hdr.msgid = MAVLINK_MSG_ID_SET_CAM_SHUTTER; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -116,7 +143,8 @@ static inline void mavlink_msg_set_cam_shutter_send(mavlink_channel_t chan, uint */ static inline uint8_t mavlink_msg_set_cam_shutter_get_cam_no(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_set_cam_shutter_t *p = (mavlink_set_cam_shutter_t *)&msg->payload[0]; + return (uint8_t)(p->cam_no); } /** @@ -126,7 +154,8 @@ static inline uint8_t mavlink_msg_set_cam_shutter_get_cam_no(const mavlink_messa */ static inline uint8_t mavlink_msg_set_cam_shutter_get_cam_mode(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_set_cam_shutter_t *p = (mavlink_set_cam_shutter_t *)&msg->payload[0]; + return (uint8_t)(p->cam_mode); } /** @@ -136,7 +165,8 @@ static inline uint8_t mavlink_msg_set_cam_shutter_get_cam_mode(const mavlink_mes */ static inline uint8_t mavlink_msg_set_cam_shutter_get_trigger_pin(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_set_cam_shutter_t *p = (mavlink_set_cam_shutter_t *)&msg->payload[0]; + return (uint8_t)(p->trigger_pin); } /** @@ -146,10 +176,8 @@ static inline uint8_t mavlink_msg_set_cam_shutter_get_trigger_pin(const mavlink_ */ static inline uint16_t mavlink_msg_set_cam_shutter_get_interval(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[1]; - return (uint16_t)r.s; + mavlink_set_cam_shutter_t *p = (mavlink_set_cam_shutter_t *)&msg->payload[0]; + return (uint16_t)(p->interval); } /** @@ -159,10 +187,8 @@ static inline uint16_t mavlink_msg_set_cam_shutter_get_interval(const mavlink_me */ static inline uint16_t mavlink_msg_set_cam_shutter_get_exposure(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_set_cam_shutter_t *p = (mavlink_set_cam_shutter_t *)&msg->payload[0]; + return (uint16_t)(p->exposure); } /** @@ -172,12 +198,8 @@ static inline uint16_t mavlink_msg_set_cam_shutter_get_exposure(const mavlink_me */ static inline float mavlink_msg_set_cam_shutter_get_gain(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t))[3]; - return (float)r.f; + mavlink_set_cam_shutter_t *p = (mavlink_set_cam_shutter_t *)&msg->payload[0]; + return (float)(p->gain); } /** @@ -188,10 +210,5 @@ static inline float mavlink_msg_set_cam_shutter_get_gain(const mavlink_message_t */ static inline void mavlink_msg_set_cam_shutter_decode(const mavlink_message_t* msg, mavlink_set_cam_shutter_t* set_cam_shutter) { - set_cam_shutter->cam_no = mavlink_msg_set_cam_shutter_get_cam_no(msg); - set_cam_shutter->cam_mode = mavlink_msg_set_cam_shutter_get_cam_mode(msg); - set_cam_shutter->trigger_pin = mavlink_msg_set_cam_shutter_get_trigger_pin(msg); - set_cam_shutter->interval = mavlink_msg_set_cam_shutter_get_interval(msg); - set_cam_shutter->exposure = mavlink_msg_set_cam_shutter_get_exposure(msg); - set_cam_shutter->gain = mavlink_msg_set_cam_shutter_get_gain(msg); + memcpy( set_cam_shutter, msg->payload, sizeof(mavlink_set_cam_shutter_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_vicon_position_estimate.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_vicon_position_estimate.h index 6e349eafc55eaaca92c6ec4a3a5e86dfdce8bb91..862ea8cbef57172cdebd8464d79474fd2ae50d51 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_vicon_position_estimate.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_vicon_position_estimate.h @@ -1,6 +1,8 @@ // MESSAGE VICON_POSITION_ESTIMATE PACKING #define MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE 112 +#define MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN 32 +#define MAVLINK_MSG_112_LEN 32 typedef struct __mavlink_vicon_position_estimate_t { @@ -14,8 +16,6 @@ typedef struct __mavlink_vicon_position_estimate_t } mavlink_vicon_position_estimate_t; - - /** * @brief Pack a vicon_position_estimate message * @param system_id ID of this system @@ -33,18 +33,18 @@ typedef struct __mavlink_vicon_position_estimate_t */ static inline uint16_t mavlink_msg_vicon_position_estimate_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) { - uint16_t i = 0; + mavlink_vicon_position_estimate_t *p = (mavlink_vicon_position_estimate_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE; - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (milliseconds) - i += put_float_by_index(x, i, msg->payload); // Global X position - i += put_float_by_index(y, i, msg->payload); // Global Y position - i += put_float_by_index(z, i, msg->payload); // Global Z position - i += put_float_by_index(roll, i, msg->payload); // Roll angle in rad - i += put_float_by_index(pitch, i, msg->payload); // Pitch angle in rad - i += put_float_by_index(yaw, i, msg->payload); // Yaw angle in rad + p->usec = usec; // uint64_t:Timestamp (milliseconds) + p->x = x; // float:Global X position + p->y = y; // float:Global Y position + p->z = z; // float:Global Z position + p->roll = roll; // float:Roll angle in rad + p->pitch = pitch; // float:Pitch angle in rad + p->yaw = yaw; // float:Yaw angle in rad - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN); } /** @@ -64,18 +64,18 @@ static inline uint16_t mavlink_msg_vicon_position_estimate_pack(uint8_t system_i */ static inline uint16_t mavlink_msg_vicon_position_estimate_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) { - uint16_t i = 0; + mavlink_vicon_position_estimate_t *p = (mavlink_vicon_position_estimate_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE; - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (milliseconds) - i += put_float_by_index(x, i, msg->payload); // Global X position - i += put_float_by_index(y, i, msg->payload); // Global Y position - i += put_float_by_index(z, i, msg->payload); // Global Z position - i += put_float_by_index(roll, i, msg->payload); // Roll angle in rad - i += put_float_by_index(pitch, i, msg->payload); // Pitch angle in rad - i += put_float_by_index(yaw, i, msg->payload); // Yaw angle in rad + p->usec = usec; // uint64_t:Timestamp (milliseconds) + p->x = x; // float:Global X position + p->y = y; // float:Global Y position + p->z = z; // float:Global Z position + p->roll = roll; // float:Roll angle in rad + p->pitch = pitch; // float:Pitch angle in rad + p->yaw = yaw; // float:Yaw angle in rad - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN); } /** @@ -103,13 +103,41 @@ static inline uint16_t mavlink_msg_vicon_position_estimate_encode(uint8_t system * @param pitch Pitch angle in rad * @param yaw Yaw angle in rad */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_vicon_position_estimate_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) { - mavlink_message_t msg; - mavlink_msg_vicon_position_estimate_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, usec, x, y, z, roll, pitch, yaw); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_vicon_position_estimate_t payload; + uint16_t checksum; + mavlink_vicon_position_estimate_t *p = &payload; + + p->usec = usec; // uint64_t:Timestamp (milliseconds) + p->x = x; // float:Global X position + p->y = y; // float:Global Y position + p->z = z; // float:Global Z position + p->roll = roll; // float:Roll angle in rad + p->pitch = pitch; // float:Pitch angle in rad + p->yaw = yaw; // float:Yaw angle in rad + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN; + hdr.msgid = MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -122,16 +150,8 @@ static inline void mavlink_msg_vicon_position_estimate_send(mavlink_channel_t ch */ static inline uint64_t mavlink_msg_vicon_position_estimate_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; + mavlink_vicon_position_estimate_t *p = (mavlink_vicon_position_estimate_t *)&msg->payload[0]; + return (uint64_t)(p->usec); } /** @@ -141,12 +161,8 @@ static inline uint64_t mavlink_msg_vicon_position_estimate_get_usec(const mavlin */ static inline float mavlink_msg_vicon_position_estimate_get_x(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; + mavlink_vicon_position_estimate_t *p = (mavlink_vicon_position_estimate_t *)&msg->payload[0]; + return (float)(p->x); } /** @@ -156,12 +172,8 @@ static inline float mavlink_msg_vicon_position_estimate_get_x(const mavlink_mess */ static inline float mavlink_msg_vicon_position_estimate_get_y(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; + mavlink_vicon_position_estimate_t *p = (mavlink_vicon_position_estimate_t *)&msg->payload[0]; + return (float)(p->y); } /** @@ -171,12 +183,8 @@ static inline float mavlink_msg_vicon_position_estimate_get_y(const mavlink_mess */ static inline float mavlink_msg_vicon_position_estimate_get_z(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; + mavlink_vicon_position_estimate_t *p = (mavlink_vicon_position_estimate_t *)&msg->payload[0]; + return (float)(p->z); } /** @@ -186,12 +194,8 @@ static inline float mavlink_msg_vicon_position_estimate_get_z(const mavlink_mess */ static inline float mavlink_msg_vicon_position_estimate_get_roll(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; + mavlink_vicon_position_estimate_t *p = (mavlink_vicon_position_estimate_t *)&msg->payload[0]; + return (float)(p->roll); } /** @@ -201,12 +205,8 @@ static inline float mavlink_msg_vicon_position_estimate_get_roll(const mavlink_m */ static inline float mavlink_msg_vicon_position_estimate_get_pitch(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; + mavlink_vicon_position_estimate_t *p = (mavlink_vicon_position_estimate_t *)&msg->payload[0]; + return (float)(p->pitch); } /** @@ -216,12 +216,8 @@ static inline float mavlink_msg_vicon_position_estimate_get_pitch(const mavlink_ */ static inline float mavlink_msg_vicon_position_estimate_get_yaw(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; + mavlink_vicon_position_estimate_t *p = (mavlink_vicon_position_estimate_t *)&msg->payload[0]; + return (float)(p->yaw); } /** @@ -232,11 +228,5 @@ static inline float mavlink_msg_vicon_position_estimate_get_yaw(const mavlink_me */ static inline void mavlink_msg_vicon_position_estimate_decode(const mavlink_message_t* msg, mavlink_vicon_position_estimate_t* vicon_position_estimate) { - vicon_position_estimate->usec = mavlink_msg_vicon_position_estimate_get_usec(msg); - vicon_position_estimate->x = mavlink_msg_vicon_position_estimate_get_x(msg); - vicon_position_estimate->y = mavlink_msg_vicon_position_estimate_get_y(msg); - vicon_position_estimate->z = mavlink_msg_vicon_position_estimate_get_z(msg); - vicon_position_estimate->roll = mavlink_msg_vicon_position_estimate_get_roll(msg); - vicon_position_estimate->pitch = mavlink_msg_vicon_position_estimate_get_pitch(msg); - vicon_position_estimate->yaw = mavlink_msg_vicon_position_estimate_get_yaw(msg); + memcpy( vicon_position_estimate, msg->payload, sizeof(mavlink_vicon_position_estimate_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_position_estimate.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_position_estimate.h index 30728191d4a45423c65544c3dd16e409bf7809cc..cfd0b15f7cad035f2d4044702d0c067727064184 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_position_estimate.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_position_estimate.h @@ -1,6 +1,8 @@ // MESSAGE VISION_POSITION_ESTIMATE PACKING #define MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE 111 +#define MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN 32 +#define MAVLINK_MSG_111_LEN 32 typedef struct __mavlink_vision_position_estimate_t { @@ -14,8 +16,6 @@ typedef struct __mavlink_vision_position_estimate_t } mavlink_vision_position_estimate_t; - - /** * @brief Pack a vision_position_estimate message * @param system_id ID of this system @@ -33,18 +33,18 @@ typedef struct __mavlink_vision_position_estimate_t */ static inline uint16_t mavlink_msg_vision_position_estimate_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) { - uint16_t i = 0; + mavlink_vision_position_estimate_t *p = (mavlink_vision_position_estimate_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE; - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (milliseconds) - i += put_float_by_index(x, i, msg->payload); // Global X position - i += put_float_by_index(y, i, msg->payload); // Global Y position - i += put_float_by_index(z, i, msg->payload); // Global Z position - i += put_float_by_index(roll, i, msg->payload); // Roll angle in rad - i += put_float_by_index(pitch, i, msg->payload); // Pitch angle in rad - i += put_float_by_index(yaw, i, msg->payload); // Yaw angle in rad + p->usec = usec; // uint64_t:Timestamp (milliseconds) + p->x = x; // float:Global X position + p->y = y; // float:Global Y position + p->z = z; // float:Global Z position + p->roll = roll; // float:Roll angle in rad + p->pitch = pitch; // float:Pitch angle in rad + p->yaw = yaw; // float:Yaw angle in rad - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN); } /** @@ -64,18 +64,18 @@ static inline uint16_t mavlink_msg_vision_position_estimate_pack(uint8_t system_ */ static inline uint16_t mavlink_msg_vision_position_estimate_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) { - uint16_t i = 0; + mavlink_vision_position_estimate_t *p = (mavlink_vision_position_estimate_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE; - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (milliseconds) - i += put_float_by_index(x, i, msg->payload); // Global X position - i += put_float_by_index(y, i, msg->payload); // Global Y position - i += put_float_by_index(z, i, msg->payload); // Global Z position - i += put_float_by_index(roll, i, msg->payload); // Roll angle in rad - i += put_float_by_index(pitch, i, msg->payload); // Pitch angle in rad - i += put_float_by_index(yaw, i, msg->payload); // Yaw angle in rad + p->usec = usec; // uint64_t:Timestamp (milliseconds) + p->x = x; // float:Global X position + p->y = y; // float:Global Y position + p->z = z; // float:Global Z position + p->roll = roll; // float:Roll angle in rad + p->pitch = pitch; // float:Pitch angle in rad + p->yaw = yaw; // float:Yaw angle in rad - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN); } /** @@ -103,13 +103,41 @@ static inline uint16_t mavlink_msg_vision_position_estimate_encode(uint8_t syste * @param pitch Pitch angle in rad * @param yaw Yaw angle in rad */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_vision_position_estimate_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) { - mavlink_message_t msg; - mavlink_msg_vision_position_estimate_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, usec, x, y, z, roll, pitch, yaw); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_vision_position_estimate_t payload; + uint16_t checksum; + mavlink_vision_position_estimate_t *p = &payload; + + p->usec = usec; // uint64_t:Timestamp (milliseconds) + p->x = x; // float:Global X position + p->y = y; // float:Global Y position + p->z = z; // float:Global Z position + p->roll = roll; // float:Roll angle in rad + p->pitch = pitch; // float:Pitch angle in rad + p->yaw = yaw; // float:Yaw angle in rad + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN; + hdr.msgid = MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -122,16 +150,8 @@ static inline void mavlink_msg_vision_position_estimate_send(mavlink_channel_t c */ static inline uint64_t mavlink_msg_vision_position_estimate_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; + mavlink_vision_position_estimate_t *p = (mavlink_vision_position_estimate_t *)&msg->payload[0]; + return (uint64_t)(p->usec); } /** @@ -141,12 +161,8 @@ static inline uint64_t mavlink_msg_vision_position_estimate_get_usec(const mavli */ static inline float mavlink_msg_vision_position_estimate_get_x(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; + mavlink_vision_position_estimate_t *p = (mavlink_vision_position_estimate_t *)&msg->payload[0]; + return (float)(p->x); } /** @@ -156,12 +172,8 @@ static inline float mavlink_msg_vision_position_estimate_get_x(const mavlink_mes */ static inline float mavlink_msg_vision_position_estimate_get_y(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; + mavlink_vision_position_estimate_t *p = (mavlink_vision_position_estimate_t *)&msg->payload[0]; + return (float)(p->y); } /** @@ -171,12 +183,8 @@ static inline float mavlink_msg_vision_position_estimate_get_y(const mavlink_mes */ static inline float mavlink_msg_vision_position_estimate_get_z(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; + mavlink_vision_position_estimate_t *p = (mavlink_vision_position_estimate_t *)&msg->payload[0]; + return (float)(p->z); } /** @@ -186,12 +194,8 @@ static inline float mavlink_msg_vision_position_estimate_get_z(const mavlink_mes */ static inline float mavlink_msg_vision_position_estimate_get_roll(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; + mavlink_vision_position_estimate_t *p = (mavlink_vision_position_estimate_t *)&msg->payload[0]; + return (float)(p->roll); } /** @@ -201,12 +205,8 @@ static inline float mavlink_msg_vision_position_estimate_get_roll(const mavlink_ */ static inline float mavlink_msg_vision_position_estimate_get_pitch(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; + mavlink_vision_position_estimate_t *p = (mavlink_vision_position_estimate_t *)&msg->payload[0]; + return (float)(p->pitch); } /** @@ -216,12 +216,8 @@ static inline float mavlink_msg_vision_position_estimate_get_pitch(const mavlink */ static inline float mavlink_msg_vision_position_estimate_get_yaw(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; + mavlink_vision_position_estimate_t *p = (mavlink_vision_position_estimate_t *)&msg->payload[0]; + return (float)(p->yaw); } /** @@ -232,11 +228,5 @@ static inline float mavlink_msg_vision_position_estimate_get_yaw(const mavlink_m */ static inline void mavlink_msg_vision_position_estimate_decode(const mavlink_message_t* msg, mavlink_vision_position_estimate_t* vision_position_estimate) { - vision_position_estimate->usec = mavlink_msg_vision_position_estimate_get_usec(msg); - vision_position_estimate->x = mavlink_msg_vision_position_estimate_get_x(msg); - vision_position_estimate->y = mavlink_msg_vision_position_estimate_get_y(msg); - vision_position_estimate->z = mavlink_msg_vision_position_estimate_get_z(msg); - vision_position_estimate->roll = mavlink_msg_vision_position_estimate_get_roll(msg); - vision_position_estimate->pitch = mavlink_msg_vision_position_estimate_get_pitch(msg); - vision_position_estimate->yaw = mavlink_msg_vision_position_estimate_get_yaw(msg); + memcpy( vision_position_estimate, msg->payload, sizeof(mavlink_vision_position_estimate_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_speed_estimate.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_speed_estimate.h index 66224c28e9e4487e3f8eb245985aefd824a3f914..e3d0755de3d699b39c5c392bcb63b3cbebdf8239 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_speed_estimate.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_speed_estimate.h @@ -1,6 +1,8 @@ // MESSAGE VISION_SPEED_ESTIMATE PACKING #define MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE 113 +#define MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN 20 +#define MAVLINK_MSG_113_LEN 20 typedef struct __mavlink_vision_speed_estimate_t { @@ -11,8 +13,6 @@ typedef struct __mavlink_vision_speed_estimate_t } mavlink_vision_speed_estimate_t; - - /** * @brief Pack a vision_speed_estimate message * @param system_id ID of this system @@ -27,15 +27,15 @@ typedef struct __mavlink_vision_speed_estimate_t */ static inline uint16_t mavlink_msg_vision_speed_estimate_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t usec, float x, float y, float z) { - uint16_t i = 0; + mavlink_vision_speed_estimate_t *p = (mavlink_vision_speed_estimate_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE; - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (milliseconds) - i += put_float_by_index(x, i, msg->payload); // Global X speed - i += put_float_by_index(y, i, msg->payload); // Global Y speed - i += put_float_by_index(z, i, msg->payload); // Global Z speed + p->usec = usec; // uint64_t:Timestamp (milliseconds) + p->x = x; // float:Global X speed + p->y = y; // float:Global Y speed + p->z = z; // float:Global Z speed - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN); } /** @@ -52,15 +52,15 @@ static inline uint16_t mavlink_msg_vision_speed_estimate_pack(uint8_t system_id, */ static inline uint16_t mavlink_msg_vision_speed_estimate_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t usec, float x, float y, float z) { - uint16_t i = 0; + mavlink_vision_speed_estimate_t *p = (mavlink_vision_speed_estimate_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE; - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (milliseconds) - i += put_float_by_index(x, i, msg->payload); // Global X speed - i += put_float_by_index(y, i, msg->payload); // Global Y speed - i += put_float_by_index(z, i, msg->payload); // Global Z speed + p->usec = usec; // uint64_t:Timestamp (milliseconds) + p->x = x; // float:Global X speed + p->y = y; // float:Global Y speed + p->z = z; // float:Global Z speed - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN); } /** @@ -85,13 +85,38 @@ static inline uint16_t mavlink_msg_vision_speed_estimate_encode(uint8_t system_i * @param y Global Y speed * @param z Global Z speed */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_vision_speed_estimate_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z) { - mavlink_message_t msg; - mavlink_msg_vision_speed_estimate_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, usec, x, y, z); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_vision_speed_estimate_t payload; + uint16_t checksum; + mavlink_vision_speed_estimate_t *p = &payload; + + p->usec = usec; // uint64_t:Timestamp (milliseconds) + p->x = x; // float:Global X speed + p->y = y; // float:Global Y speed + p->z = z; // float:Global Z speed + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN; + hdr.msgid = MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -104,16 +129,8 @@ static inline void mavlink_msg_vision_speed_estimate_send(mavlink_channel_t chan */ static inline uint64_t mavlink_msg_vision_speed_estimate_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; + mavlink_vision_speed_estimate_t *p = (mavlink_vision_speed_estimate_t *)&msg->payload[0]; + return (uint64_t)(p->usec); } /** @@ -123,12 +140,8 @@ static inline uint64_t mavlink_msg_vision_speed_estimate_get_usec(const mavlink_ */ static inline float mavlink_msg_vision_speed_estimate_get_x(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; + mavlink_vision_speed_estimate_t *p = (mavlink_vision_speed_estimate_t *)&msg->payload[0]; + return (float)(p->x); } /** @@ -138,12 +151,8 @@ static inline float mavlink_msg_vision_speed_estimate_get_x(const mavlink_messag */ static inline float mavlink_msg_vision_speed_estimate_get_y(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; + mavlink_vision_speed_estimate_t *p = (mavlink_vision_speed_estimate_t *)&msg->payload[0]; + return (float)(p->y); } /** @@ -153,12 +162,8 @@ static inline float mavlink_msg_vision_speed_estimate_get_y(const mavlink_messag */ static inline float mavlink_msg_vision_speed_estimate_get_z(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; + mavlink_vision_speed_estimate_t *p = (mavlink_vision_speed_estimate_t *)&msg->payload[0]; + return (float)(p->z); } /** @@ -169,8 +174,5 @@ static inline float mavlink_msg_vision_speed_estimate_get_z(const mavlink_messag */ static inline void mavlink_msg_vision_speed_estimate_decode(const mavlink_message_t* msg, mavlink_vision_speed_estimate_t* vision_speed_estimate) { - vision_speed_estimate->usec = mavlink_msg_vision_speed_estimate_get_usec(msg); - vision_speed_estimate->x = mavlink_msg_vision_speed_estimate_get_x(msg); - vision_speed_estimate->y = mavlink_msg_vision_speed_estimate_get_y(msg); - vision_speed_estimate->z = mavlink_msg_vision_speed_estimate_get_z(msg); + memcpy( vision_speed_estimate, msg->payload, sizeof(mavlink_vision_speed_estimate_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_command.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_command.h index 25ea0f5f1de38eca19fb1da9b67d7ed4d3ba4596..dee5bcf0aaf99d366d81d9a21eac37a53919daf2 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_command.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_command.h @@ -1,18 +1,18 @@ // MESSAGE WATCHDOG_COMMAND PACKING #define MAVLINK_MSG_ID_WATCHDOG_COMMAND 153 +#define MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN 6 +#define MAVLINK_MSG_153_LEN 6 typedef struct __mavlink_watchdog_command_t { - uint8_t target_system_id; ///< Target system ID uint16_t watchdog_id; ///< Watchdog ID uint16_t process_id; ///< Process ID + uint8_t target_system_id; ///< Target system ID uint8_t command_id; ///< Command ID } mavlink_watchdog_command_t; - - /** * @brief Pack a watchdog_command message * @param system_id ID of this system @@ -27,15 +27,15 @@ typedef struct __mavlink_watchdog_command_t */ static inline uint16_t mavlink_msg_watchdog_command_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system_id, uint16_t watchdog_id, uint16_t process_id, uint8_t command_id) { - uint16_t i = 0; + mavlink_watchdog_command_t *p = (mavlink_watchdog_command_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WATCHDOG_COMMAND; - i += put_uint8_t_by_index(target_system_id, i, msg->payload); // Target system ID - i += put_uint16_t_by_index(watchdog_id, i, msg->payload); // Watchdog ID - i += put_uint16_t_by_index(process_id, i, msg->payload); // Process ID - i += put_uint8_t_by_index(command_id, i, msg->payload); // Command ID + p->target_system_id = target_system_id; // uint8_t:Target system ID + p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID + p->process_id = process_id; // uint16_t:Process ID + p->command_id = command_id; // uint8_t:Command ID - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN); } /** @@ -52,15 +52,15 @@ static inline uint16_t mavlink_msg_watchdog_command_pack(uint8_t system_id, uint */ static inline uint16_t mavlink_msg_watchdog_command_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system_id, uint16_t watchdog_id, uint16_t process_id, uint8_t command_id) { - uint16_t i = 0; + mavlink_watchdog_command_t *p = (mavlink_watchdog_command_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WATCHDOG_COMMAND; - i += put_uint8_t_by_index(target_system_id, i, msg->payload); // Target system ID - i += put_uint16_t_by_index(watchdog_id, i, msg->payload); // Watchdog ID - i += put_uint16_t_by_index(process_id, i, msg->payload); // Process ID - i += put_uint8_t_by_index(command_id, i, msg->payload); // Command ID + p->target_system_id = target_system_id; // uint8_t:Target system ID + p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID + p->process_id = process_id; // uint16_t:Process ID + p->command_id = command_id; // uint8_t:Command ID - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN); } /** @@ -85,13 +85,38 @@ static inline uint16_t mavlink_msg_watchdog_command_encode(uint8_t system_id, ui * @param process_id Process ID * @param command_id Command ID */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_watchdog_command_send(mavlink_channel_t chan, uint8_t target_system_id, uint16_t watchdog_id, uint16_t process_id, uint8_t command_id) { - mavlink_message_t msg; - mavlink_msg_watchdog_command_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system_id, watchdog_id, process_id, command_id); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_watchdog_command_t payload; + uint16_t checksum; + mavlink_watchdog_command_t *p = &payload; + + p->target_system_id = target_system_id; // uint8_t:Target system ID + p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID + p->process_id = process_id; // uint16_t:Process ID + p->command_id = command_id; // uint8_t:Command ID + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN; + hdr.msgid = MAVLINK_MSG_ID_WATCHDOG_COMMAND; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -104,7 +129,8 @@ static inline void mavlink_msg_watchdog_command_send(mavlink_channel_t chan, uin */ static inline uint8_t mavlink_msg_watchdog_command_get_target_system_id(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_watchdog_command_t *p = (mavlink_watchdog_command_t *)&msg->payload[0]; + return (uint8_t)(p->target_system_id); } /** @@ -114,10 +140,8 @@ static inline uint8_t mavlink_msg_watchdog_command_get_target_system_id(const ma */ static inline uint16_t mavlink_msg_watchdog_command_get_watchdog_id(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t))[1]; - return (uint16_t)r.s; + mavlink_watchdog_command_t *p = (mavlink_watchdog_command_t *)&msg->payload[0]; + return (uint16_t)(p->watchdog_id); } /** @@ -127,10 +151,8 @@ static inline uint16_t mavlink_msg_watchdog_command_get_watchdog_id(const mavlin */ static inline uint16_t mavlink_msg_watchdog_command_get_process_id(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_watchdog_command_t *p = (mavlink_watchdog_command_t *)&msg->payload[0]; + return (uint16_t)(p->process_id); } /** @@ -140,7 +162,8 @@ static inline uint16_t mavlink_msg_watchdog_command_get_process_id(const mavlink */ static inline uint8_t mavlink_msg_watchdog_command_get_command_id(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; + mavlink_watchdog_command_t *p = (mavlink_watchdog_command_t *)&msg->payload[0]; + return (uint8_t)(p->command_id); } /** @@ -151,8 +174,5 @@ static inline uint8_t mavlink_msg_watchdog_command_get_command_id(const mavlink_ */ static inline void mavlink_msg_watchdog_command_decode(const mavlink_message_t* msg, mavlink_watchdog_command_t* watchdog_command) { - watchdog_command->target_system_id = mavlink_msg_watchdog_command_get_target_system_id(msg); - watchdog_command->watchdog_id = mavlink_msg_watchdog_command_get_watchdog_id(msg); - watchdog_command->process_id = mavlink_msg_watchdog_command_get_process_id(msg); - watchdog_command->command_id = mavlink_msg_watchdog_command_get_command_id(msg); + memcpy( watchdog_command, msg->payload, sizeof(mavlink_watchdog_command_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_heartbeat.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_heartbeat.h index db693bc554069e8fa22624b64e8c5bb4e35aef2d..e31a060eadb009b3fd7db5838e20cdf140e47821 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_heartbeat.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_heartbeat.h @@ -1,6 +1,8 @@ // MESSAGE WATCHDOG_HEARTBEAT PACKING #define MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT 150 +#define MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN 4 +#define MAVLINK_MSG_150_LEN 4 typedef struct __mavlink_watchdog_heartbeat_t { @@ -9,8 +11,6 @@ typedef struct __mavlink_watchdog_heartbeat_t } mavlink_watchdog_heartbeat_t; - - /** * @brief Pack a watchdog_heartbeat message * @param system_id ID of this system @@ -23,13 +23,13 @@ typedef struct __mavlink_watchdog_heartbeat_t */ static inline uint16_t mavlink_msg_watchdog_heartbeat_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint16_t watchdog_id, uint16_t process_count) { - uint16_t i = 0; + mavlink_watchdog_heartbeat_t *p = (mavlink_watchdog_heartbeat_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT; - i += put_uint16_t_by_index(watchdog_id, i, msg->payload); // Watchdog ID - i += put_uint16_t_by_index(process_count, i, msg->payload); // Number of processes + p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID + p->process_count = process_count; // uint16_t:Number of processes - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN); } /** @@ -44,13 +44,13 @@ static inline uint16_t mavlink_msg_watchdog_heartbeat_pack(uint8_t system_id, ui */ static inline uint16_t mavlink_msg_watchdog_heartbeat_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint16_t watchdog_id, uint16_t process_count) { - uint16_t i = 0; + mavlink_watchdog_heartbeat_t *p = (mavlink_watchdog_heartbeat_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT; - i += put_uint16_t_by_index(watchdog_id, i, msg->payload); // Watchdog ID - i += put_uint16_t_by_index(process_count, i, msg->payload); // Number of processes + p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID + p->process_count = process_count; // uint16_t:Number of processes - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN); } /** @@ -73,13 +73,36 @@ static inline uint16_t mavlink_msg_watchdog_heartbeat_encode(uint8_t system_id, * @param watchdog_id Watchdog ID * @param process_count Number of processes */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_watchdog_heartbeat_send(mavlink_channel_t chan, uint16_t watchdog_id, uint16_t process_count) { - mavlink_message_t msg; - mavlink_msg_watchdog_heartbeat_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, watchdog_id, process_count); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_watchdog_heartbeat_t payload; + uint16_t checksum; + mavlink_watchdog_heartbeat_t *p = &payload; + + p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID + p->process_count = process_count; // uint16_t:Number of processes + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN; + hdr.msgid = MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -92,10 +115,8 @@ static inline void mavlink_msg_watchdog_heartbeat_send(mavlink_channel_t chan, u */ static inline uint16_t mavlink_msg_watchdog_heartbeat_get_watchdog_id(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload)[0]; - r.b[0] = (msg->payload)[1]; - return (uint16_t)r.s; + mavlink_watchdog_heartbeat_t *p = (mavlink_watchdog_heartbeat_t *)&msg->payload[0]; + return (uint16_t)(p->watchdog_id); } /** @@ -105,10 +126,8 @@ static inline uint16_t mavlink_msg_watchdog_heartbeat_get_watchdog_id(const mavl */ static inline uint16_t mavlink_msg_watchdog_heartbeat_get_process_count(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_watchdog_heartbeat_t *p = (mavlink_watchdog_heartbeat_t *)&msg->payload[0]; + return (uint16_t)(p->process_count); } /** @@ -119,6 +138,5 @@ static inline uint16_t mavlink_msg_watchdog_heartbeat_get_process_count(const ma */ static inline void mavlink_msg_watchdog_heartbeat_decode(const mavlink_message_t* msg, mavlink_watchdog_heartbeat_t* watchdog_heartbeat) { - watchdog_heartbeat->watchdog_id = mavlink_msg_watchdog_heartbeat_get_watchdog_id(msg); - watchdog_heartbeat->process_count = mavlink_msg_watchdog_heartbeat_get_process_count(msg); + memcpy( watchdog_heartbeat, msg->payload, sizeof(mavlink_watchdog_heartbeat_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_info.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_info.h index a312a14679fb7589f6eaa81c19df0922bf3d9444..e9d7703f9e56e76dd5b5cc25d133609933540149 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_info.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_info.h @@ -1,21 +1,21 @@ // MESSAGE WATCHDOG_PROCESS_INFO PACKING #define MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO 151 +#define MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN 255 +#define MAVLINK_MSG_151_LEN 255 typedef struct __mavlink_watchdog_process_info_t { + int32_t timeout; ///< Timeout (seconds) uint16_t watchdog_id; ///< Watchdog ID uint16_t process_id; ///< Process ID - int8_t name[100]; ///< Process name - int8_t arguments[147]; ///< Process arguments - int32_t timeout; ///< Timeout (seconds) + char name[100]; ///< Process name + char arguments[147]; ///< Process arguments } mavlink_watchdog_process_info_t; - #define MAVLINK_MSG_WATCHDOG_PROCESS_INFO_FIELD_NAME_LEN 100 #define MAVLINK_MSG_WATCHDOG_PROCESS_INFO_FIELD_ARGUMENTS_LEN 147 - /** * @brief Pack a watchdog_process_info message * @param system_id ID of this system @@ -29,18 +29,18 @@ typedef struct __mavlink_watchdog_process_info_t * @param timeout Timeout (seconds) * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_watchdog_process_info_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint16_t watchdog_id, uint16_t process_id, const int8_t* name, const int8_t* arguments, int32_t timeout) +static inline uint16_t mavlink_msg_watchdog_process_info_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint16_t watchdog_id, uint16_t process_id, const char* name, const char* arguments, int32_t timeout) { - uint16_t i = 0; + mavlink_watchdog_process_info_t *p = (mavlink_watchdog_process_info_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO; - i += put_uint16_t_by_index(watchdog_id, i, msg->payload); // Watchdog ID - i += put_uint16_t_by_index(process_id, i, msg->payload); // Process ID - i += put_array_by_index(name, 100, i, msg->payload); // Process name - i += put_array_by_index(arguments, 147, i, msg->payload); // Process arguments - i += put_int32_t_by_index(timeout, i, msg->payload); // Timeout (seconds) + p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID + p->process_id = process_id; // uint16_t:Process ID + memcpy(p->name, name, sizeof(p->name)); // char[100]:Process name + memcpy(p->arguments, arguments, sizeof(p->arguments)); // char[147]:Process arguments + p->timeout = timeout; // int32_t:Timeout (seconds) - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN); } /** @@ -56,18 +56,18 @@ static inline uint16_t mavlink_msg_watchdog_process_info_pack(uint8_t system_id, * @param timeout Timeout (seconds) * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_watchdog_process_info_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint16_t watchdog_id, uint16_t process_id, const int8_t* name, const int8_t* arguments, int32_t timeout) +static inline uint16_t mavlink_msg_watchdog_process_info_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint16_t watchdog_id, uint16_t process_id, const char* name, const char* arguments, int32_t timeout) { - uint16_t i = 0; + mavlink_watchdog_process_info_t *p = (mavlink_watchdog_process_info_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO; - i += put_uint16_t_by_index(watchdog_id, i, msg->payload); // Watchdog ID - i += put_uint16_t_by_index(process_id, i, msg->payload); // Process ID - i += put_array_by_index(name, 100, i, msg->payload); // Process name - i += put_array_by_index(arguments, 147, i, msg->payload); // Process arguments - i += put_int32_t_by_index(timeout, i, msg->payload); // Timeout (seconds) + p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID + p->process_id = process_id; // uint16_t:Process ID + memcpy(p->name, name, sizeof(p->name)); // char[100]:Process name + memcpy(p->arguments, arguments, sizeof(p->arguments)); // char[147]:Process arguments + p->timeout = timeout; // int32_t:Timeout (seconds) - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN); } /** @@ -93,13 +93,39 @@ static inline uint16_t mavlink_msg_watchdog_process_info_encode(uint8_t system_i * @param arguments Process arguments * @param timeout Timeout (seconds) */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_watchdog_process_info_send(mavlink_channel_t chan, uint16_t watchdog_id, uint16_t process_id, const int8_t* name, const int8_t* arguments, int32_t timeout) + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS +static inline void mavlink_msg_watchdog_process_info_send(mavlink_channel_t chan, uint16_t watchdog_id, uint16_t process_id, const char* name, const char* arguments, int32_t timeout) { - mavlink_message_t msg; - mavlink_msg_watchdog_process_info_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, watchdog_id, process_id, name, arguments, timeout); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_watchdog_process_info_t payload; + uint16_t checksum; + mavlink_watchdog_process_info_t *p = &payload; + + p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID + p->process_id = process_id; // uint16_t:Process ID + memcpy(p->name, name, sizeof(p->name)); // char[100]:Process name + memcpy(p->arguments, arguments, sizeof(p->arguments)); // char[147]:Process arguments + p->timeout = timeout; // int32_t:Timeout (seconds) + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN; + hdr.msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -112,10 +138,8 @@ static inline void mavlink_msg_watchdog_process_info_send(mavlink_channel_t chan */ static inline uint16_t mavlink_msg_watchdog_process_info_get_watchdog_id(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload)[0]; - r.b[0] = (msg->payload)[1]; - return (uint16_t)r.s; + mavlink_watchdog_process_info_t *p = (mavlink_watchdog_process_info_t *)&msg->payload[0]; + return (uint16_t)(p->watchdog_id); } /** @@ -125,10 +149,8 @@ static inline uint16_t mavlink_msg_watchdog_process_info_get_watchdog_id(const m */ static inline uint16_t mavlink_msg_watchdog_process_info_get_process_id(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_watchdog_process_info_t *p = (mavlink_watchdog_process_info_t *)&msg->payload[0]; + return (uint16_t)(p->process_id); } /** @@ -136,11 +158,12 @@ static inline uint16_t mavlink_msg_watchdog_process_info_get_process_id(const ma * * @return Process name */ -static inline uint16_t mavlink_msg_watchdog_process_info_get_name(const mavlink_message_t* msg, int8_t* r_data) +static inline uint16_t mavlink_msg_watchdog_process_info_get_name(const mavlink_message_t* msg, char* name) { + mavlink_watchdog_process_info_t *p = (mavlink_watchdog_process_info_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint16_t)+sizeof(uint16_t), 100); - return 100; + memcpy(name, p->name, sizeof(p->name)); + return sizeof(p->name); } /** @@ -148,11 +171,12 @@ static inline uint16_t mavlink_msg_watchdog_process_info_get_name(const mavlink_ * * @return Process arguments */ -static inline uint16_t mavlink_msg_watchdog_process_info_get_arguments(const mavlink_message_t* msg, int8_t* r_data) +static inline uint16_t mavlink_msg_watchdog_process_info_get_arguments(const mavlink_message_t* msg, char* arguments) { + mavlink_watchdog_process_info_t *p = (mavlink_watchdog_process_info_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+100, 147); - return 147; + memcpy(arguments, p->arguments, sizeof(p->arguments)); + return sizeof(p->arguments); } /** @@ -162,12 +186,8 @@ static inline uint16_t mavlink_msg_watchdog_process_info_get_arguments(const mav */ static inline int32_t mavlink_msg_watchdog_process_info_get_timeout(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+100+147)[0]; - r.b[2] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+100+147)[1]; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+100+147)[2]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+100+147)[3]; - return (int32_t)r.i; + mavlink_watchdog_process_info_t *p = (mavlink_watchdog_process_info_t *)&msg->payload[0]; + return (int32_t)(p->timeout); } /** @@ -178,9 +198,5 @@ static inline int32_t mavlink_msg_watchdog_process_info_get_timeout(const mavlin */ static inline void mavlink_msg_watchdog_process_info_decode(const mavlink_message_t* msg, mavlink_watchdog_process_info_t* watchdog_process_info) { - watchdog_process_info->watchdog_id = mavlink_msg_watchdog_process_info_get_watchdog_id(msg); - watchdog_process_info->process_id = mavlink_msg_watchdog_process_info_get_process_id(msg); - mavlink_msg_watchdog_process_info_get_name(msg, watchdog_process_info->name); - mavlink_msg_watchdog_process_info_get_arguments(msg, watchdog_process_info->arguments); - watchdog_process_info->timeout = mavlink_msg_watchdog_process_info_get_timeout(msg); + memcpy( watchdog_process_info, msg->payload, sizeof(mavlink_watchdog_process_info_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_status.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_status.h index 9afa6ca14af95e685c5aa606eb47c6b11c708ace..f6f3dd26d77402bd6819047873106f4d39654703 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_status.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_status.h @@ -1,20 +1,20 @@ // MESSAGE WATCHDOG_PROCESS_STATUS PACKING #define MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS 152 +#define MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN 12 +#define MAVLINK_MSG_152_LEN 12 typedef struct __mavlink_watchdog_process_status_t { + int32_t pid; ///< PID uint16_t watchdog_id; ///< Watchdog ID uint16_t process_id; ///< Process ID + uint16_t crashes; ///< Number of crashes uint8_t state; ///< Is running / finished / suspended / crashed uint8_t muted; ///< Is muted - int32_t pid; ///< PID - uint16_t crashes; ///< Number of crashes } mavlink_watchdog_process_status_t; - - /** * @brief Pack a watchdog_process_status message * @param system_id ID of this system @@ -31,17 +31,17 @@ typedef struct __mavlink_watchdog_process_status_t */ static inline uint16_t mavlink_msg_watchdog_process_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint16_t watchdog_id, uint16_t process_id, uint8_t state, uint8_t muted, int32_t pid, uint16_t crashes) { - uint16_t i = 0; + mavlink_watchdog_process_status_t *p = (mavlink_watchdog_process_status_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS; - i += put_uint16_t_by_index(watchdog_id, i, msg->payload); // Watchdog ID - i += put_uint16_t_by_index(process_id, i, msg->payload); // Process ID - i += put_uint8_t_by_index(state, i, msg->payload); // Is running / finished / suspended / crashed - i += put_uint8_t_by_index(muted, i, msg->payload); // Is muted - i += put_int32_t_by_index(pid, i, msg->payload); // PID - i += put_uint16_t_by_index(crashes, i, msg->payload); // Number of crashes + p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID + p->process_id = process_id; // uint16_t:Process ID + p->state = state; // uint8_t:Is running / finished / suspended / crashed + p->muted = muted; // uint8_t:Is muted + p->pid = pid; // int32_t:PID + p->crashes = crashes; // uint16_t:Number of crashes - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN); } /** @@ -60,17 +60,17 @@ static inline uint16_t mavlink_msg_watchdog_process_status_pack(uint8_t system_i */ static inline uint16_t mavlink_msg_watchdog_process_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint16_t watchdog_id, uint16_t process_id, uint8_t state, uint8_t muted, int32_t pid, uint16_t crashes) { - uint16_t i = 0; + mavlink_watchdog_process_status_t *p = (mavlink_watchdog_process_status_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS; - i += put_uint16_t_by_index(watchdog_id, i, msg->payload); // Watchdog ID - i += put_uint16_t_by_index(process_id, i, msg->payload); // Process ID - i += put_uint8_t_by_index(state, i, msg->payload); // Is running / finished / suspended / crashed - i += put_uint8_t_by_index(muted, i, msg->payload); // Is muted - i += put_int32_t_by_index(pid, i, msg->payload); // PID - i += put_uint16_t_by_index(crashes, i, msg->payload); // Number of crashes + p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID + p->process_id = process_id; // uint16_t:Process ID + p->state = state; // uint8_t:Is running / finished / suspended / crashed + p->muted = muted; // uint8_t:Is muted + p->pid = pid; // int32_t:PID + p->crashes = crashes; // uint16_t:Number of crashes - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN); } /** @@ -97,13 +97,40 @@ static inline uint16_t mavlink_msg_watchdog_process_status_encode(uint8_t system * @param pid PID * @param crashes Number of crashes */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_watchdog_process_status_send(mavlink_channel_t chan, uint16_t watchdog_id, uint16_t process_id, uint8_t state, uint8_t muted, int32_t pid, uint16_t crashes) { - mavlink_message_t msg; - mavlink_msg_watchdog_process_status_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, watchdog_id, process_id, state, muted, pid, crashes); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_watchdog_process_status_t payload; + uint16_t checksum; + mavlink_watchdog_process_status_t *p = &payload; + + p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID + p->process_id = process_id; // uint16_t:Process ID + p->state = state; // uint8_t:Is running / finished / suspended / crashed + p->muted = muted; // uint8_t:Is muted + p->pid = pid; // int32_t:PID + p->crashes = crashes; // uint16_t:Number of crashes + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN; + hdr.msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -116,10 +143,8 @@ static inline void mavlink_msg_watchdog_process_status_send(mavlink_channel_t ch */ static inline uint16_t mavlink_msg_watchdog_process_status_get_watchdog_id(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload)[0]; - r.b[0] = (msg->payload)[1]; - return (uint16_t)r.s; + mavlink_watchdog_process_status_t *p = (mavlink_watchdog_process_status_t *)&msg->payload[0]; + return (uint16_t)(p->watchdog_id); } /** @@ -129,10 +154,8 @@ static inline uint16_t mavlink_msg_watchdog_process_status_get_watchdog_id(const */ static inline uint16_t mavlink_msg_watchdog_process_status_get_process_id(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_watchdog_process_status_t *p = (mavlink_watchdog_process_status_t *)&msg->payload[0]; + return (uint16_t)(p->process_id); } /** @@ -142,7 +165,8 @@ static inline uint16_t mavlink_msg_watchdog_process_status_get_process_id(const */ static inline uint8_t mavlink_msg_watchdog_process_status_get_state(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint16_t)+sizeof(uint16_t))[0]; + mavlink_watchdog_process_status_t *p = (mavlink_watchdog_process_status_t *)&msg->payload[0]; + return (uint8_t)(p->state); } /** @@ -152,7 +176,8 @@ static inline uint8_t mavlink_msg_watchdog_process_status_get_state(const mavlin */ static inline uint8_t mavlink_msg_watchdog_process_status_get_muted(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t))[0]; + mavlink_watchdog_process_status_t *p = (mavlink_watchdog_process_status_t *)&msg->payload[0]; + return (uint8_t)(p->muted); } /** @@ -162,12 +187,8 @@ static inline uint8_t mavlink_msg_watchdog_process_status_get_muted(const mavlin */ static inline int32_t mavlink_msg_watchdog_process_status_get_pid(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t))[3]; - return (int32_t)r.i; + mavlink_watchdog_process_status_t *p = (mavlink_watchdog_process_status_t *)&msg->payload[0]; + return (int32_t)(p->pid); } /** @@ -177,10 +198,8 @@ static inline int32_t mavlink_msg_watchdog_process_status_get_pid(const mavlink_ */ static inline uint16_t mavlink_msg_watchdog_process_status_get_crashes(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(int32_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(int32_t))[1]; - return (uint16_t)r.s; + mavlink_watchdog_process_status_t *p = (mavlink_watchdog_process_status_t *)&msg->payload[0]; + return (uint16_t)(p->crashes); } /** @@ -191,10 +210,5 @@ static inline uint16_t mavlink_msg_watchdog_process_status_get_crashes(const mav */ static inline void mavlink_msg_watchdog_process_status_decode(const mavlink_message_t* msg, mavlink_watchdog_process_status_t* watchdog_process_status) { - watchdog_process_status->watchdog_id = mavlink_msg_watchdog_process_status_get_watchdog_id(msg); - watchdog_process_status->process_id = mavlink_msg_watchdog_process_status_get_process_id(msg); - watchdog_process_status->state = mavlink_msg_watchdog_process_status_get_state(msg); - watchdog_process_status->muted = mavlink_msg_watchdog_process_status_get_muted(msg); - watchdog_process_status->pid = mavlink_msg_watchdog_process_status_get_pid(msg); - watchdog_process_status->crashes = mavlink_msg_watchdog_process_status_get_crashes(msg); + memcpy( watchdog_process_status, msg->payload, sizeof(mavlink_watchdog_process_status_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/pixhawk.h b/thirdParty/mavlink/include/pixhawk/pixhawk.h index 30474d09cb8c1268d12d14fe373d79f0f06551a4..a0bd474b19a7c78ebfcfbad22273a04a4a82b6c7 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 Tuesday, August 9 2011, 16:49 UTC */ #ifndef PIXHAWK_H #define PIXHAWK_H @@ -30,7 +30,7 @@ extern "C" { // ENUM DEFINITIONS -/** @brief Content Types for data transmission handshake */ +/** @brief Content Types for data transmission handshake */ enum DATA_TYPES { DATA_TYPE_JPEG_IMAGE=1, @@ -68,10 +68,10 @@ enum DATA_TYPES #include "./mavlink_msg_brief_feature.h" -// MESSAGE LENGTHS +// MESSAGE CRC KEYS -#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 } +#undef MAVLINK_MESSAGE_KEYS +#define MAVLINK_MESSAGE_KEYS { 71, 249, 232, 226, 76, 126, 117, 186, 0, 144, 0, 249, 133, 0, 0, 0, 0, 0, 0, 0, 33, 34, 163, 45, 0, 223, 28, 99, 28, 21, 243, 240, 204, 21, 111, 43, 192, 234, 22, 197, 192, 192, 166, 34, 233, 34, 166, 158, 142, 60, 10, 75, 249, 247, 234, 157, 125, 0, 0, 0, 85, 211, 62, 75, 185, 18, 42, 38, 0, 127, 200, 0, 0, 53, 251, 20, 22, 0, 0, 0, 0, 0, 0, 0, 0, 127, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 8, 238, 165, 0, 0, 0, 0, 0, 0, 0, 218, 218, 235, 0, 0, 0, 0, 0, 0, 225, 114, 0, 0, 0, 0, 0, 0, 0, 0, 221, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 171, 122, 0, 0, 0, 0, 0, 0, 0, 92, 99, 4, 169, 10, 0, 0, 0, 0, 0, 52, 163, 16, 0, 0, 0, 0, 0, 0, 0, 200, 135, 217, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 43, 141, 211, 144 } #ifdef __cplusplus } diff --git a/thirdParty/mavlink/include/protocol.h b/thirdParty/mavlink/include/protocol.h index ed1f9546b499e2a1c3e10738102f70687e7cd3ae..a45993e692e3777208cf75e8a61b4c4a97c558f1 100644 --- a/thirdParty/mavlink/include/protocol.h +++ b/thirdParty/mavlink/include/protocol.h @@ -2,10 +2,9 @@ #define _MAVLINK_PROTOCOL_H_ #include "string.h" -#include "checksum.h" - #include "mavlink_types.h" +#include "checksum.h" /** * @brief Initialize the communication stack @@ -34,7 +33,12 @@ static void mavlink_parse_state_initialize(mavlink_status_t* initStatus) static inline mavlink_status_t* mavlink_get_channel_status(uint8_t chan) { - static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS]; +#if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32) + static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB_HIGH]; +#else + static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB]; +#endif + return &m_mavlink_status[chan]; } @@ -68,8 +72,9 @@ static inline uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t msg->compid = component_id; // One sequence number per component msg->seq = mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq; - mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq = mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq+1; - checksum = crc_calculate((uint8_t*)((void*)msg), length + MAVLINK_CORE_HEADER_LEN); + mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq = mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq+1; +// checksum = crc_calculate((uint8_t*)((void*)msg), length + MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_msg(msg, length + MAVLINK_CORE_HEADER_LEN); msg->ck_a = (uint8_t)(checksum & 0xFF); ///< High byte msg->ck_b = (uint8_t)(checksum >> 8); ///< Low byte @@ -98,7 +103,8 @@ static inline uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uin // One sequence number per component msg->seq = mavlink_get_channel_status(chan)->current_tx_seq; mavlink_get_channel_status(chan)->current_tx_seq = mavlink_get_channel_status(chan)->current_tx_seq+1; - checksum = crc_calculate((uint8_t*)((void*)msg), length + MAVLINK_CORE_HEADER_LEN); +// checksum = crc_calculate((uint8_t*)((void*)msg), length + MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_msg(msg, length + MAVLINK_CORE_HEADER_LEN); msg->ck_a = (uint8_t)(checksum & 0xFF); ///< High byte msg->ck_b = (uint8_t)(checksum >> 8); ///< Low byte @@ -111,11 +117,13 @@ static inline uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uin static inline uint16_t mavlink_msg_to_send_buffer(uint8_t* buffer, const mavlink_message_t* msg) { *(buffer+0) = MAVLINK_STX; ///< Start transmit - memcpy((buffer+1), msg, msg->len + MAVLINK_CORE_HEADER_LEN); ///< Core header plus payload - *(buffer + msg->len + MAVLINK_CORE_HEADER_LEN + 1) = msg->ck_a; +// memcpy((buffer+1), msg, msg->len + MAVLINK_CORE_HEADER_LEN); ///< Core header plus payload + memcpy((buffer+1), &msg->len, MAVLINK_CORE_HEADER_LEN); ///< Core header + memcpy((buffer+1+MAVLINK_CORE_HEADER_LEN), &msg->payload[0], msg->len); ///< payload + *(buffer + msg->len + MAVLINK_CORE_HEADER_LEN + 1) = msg->ck_a; *(buffer + msg->len + MAVLINK_CORE_HEADER_LEN + 2) = msg->ck_b; return msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES; - return 0; +// return 0; } /** @@ -197,8 +205,13 @@ static inline void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c) */ static inline uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status) { - static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NUM_BUFFERS]; - +#if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32) + static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB_HIGH]; +#elif defined(NB_MAVLINK_COMM) + static mavlink_message_t m_mavlink_message[NB_MAVLINK_COMM]; +#else + static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB]; +#endif // Initializes only once, values keep unchanged after first initialization mavlink_parse_state_initialize(mavlink_get_channel_status(chan)); @@ -258,6 +271,11 @@ static inline uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_messag case MAVLINK_PARSE_STATE_GOT_COMPID: rxmsg->msgid = c; mavlink_update_checksum(rxmsg, c); +#ifdef MAVLINK_CHECK_LENGTH + if (rxmsg->len != mavlink_msg_lengths[c] ) + status->parse_state = MAVLINK_PARSE_STATE_IDLE; // abort, not going to understand it anyway + else ; +#endif if (rxmsg->len == 0) { status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; @@ -304,16 +322,19 @@ static inline uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_messag status->parse_state = MAVLINK_PARSE_STATE_IDLE; if (c == MAVLINK_STX) { - status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; - mavlink_start_checksum(rxmsg); + status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; + mavlink_start_checksum(rxmsg); } - } + } else { - // Successfully got message - status->msg_received = 1; - status->parse_state = MAVLINK_PARSE_STATE_IDLE; - memcpy(r_message, rxmsg, sizeof(mavlink_message_t)); + // Successfully got message + status->msg_received = 1; + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + if ( r_message != NULL ) + { + memcpy(r_message, rxmsg, sizeof(mavlink_message_t)); + } } break; } @@ -338,506 +359,25 @@ static inline uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_messag r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count; r_mavlink_status->packet_rx_drop_count = status->parse_error; status->parse_error = 0; - return status->msg_received; -} - - -/** - * This is a convenience function which handles the complete MAVLink parsing. - * the function will parse one byte at a time and return the complete packet once - * it could be successfully decoded. Checksum and other failures will be silently - * ignored. - * - * @param chan ID of the current channel. This allows to parse different channels with this function. - * a channel is not a physical message channel like a serial port, but a logic partition of - * the communication streams in this case. COMM_NB is the limit for the number of channels - * on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows - * @param c The char to barse - * - * @param returnMsg NULL if no message could be decoded, the message data else - * @return 0 if no message could be decoded, 1 else - * - * A typical use scenario of this function call is: - * - * @code - * #include // For fixed-width uint8_t type - * - * mavlink_message_t msg; - * int chan = 0; - * - * - * while(serial.bytesAvailable > 0) - * { - * uint8_t byte = serial.getNextByte(); - * if (mavlink_parse_char(chan, byte, &msg)) - * { - * printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid); - * } - * } - * - * - * @endcode - */ - -#define MAVLINK_PACKET_START_CANDIDATES 50 -/* -static inline uint8_t mavlink_parse_char_new(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status) -{ - static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS]; - static uint8_t m_msgbuf[MAVLINK_COMM_NUM_BUFFERS][MAVLINK_MAX_PACKET_LEN * 2]; - static uint8_t m_msgbuf_index[MAVLINK_COMM_NUM_BUFFERS]; - static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NUM_BUFFERS]; - static uint8_t m_packet_start[MAVLINK_COMM_NUM_BUFFERS][MAVLINK_PACKET_START_CANDIDATES]; - static uint8_t m_packet_start_index_read[MAVLINK_COMM_NUM_BUFFERS]; - static uint8_t m_packet_start_index_write[MAVLINK_COMM_NUM_BUFFERS]; - - // Set a packet start candidate index if sign is start sign - if (c == MAVLINK_STX) - { - m_packet_start[chan][++(m_packet_start_index_write[chan]) % MAVLINK_PACKET_START_CANDIDATES] = m_msgbuf_index[chan]; - } - - // Parse normally, if a CRC mismatch occurs retry with the next packet index -} -// static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS]; -// static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NUM_BUFFERS]; -//// Initializes only once, values keep unchanged after first initialization -// mavlink_parse_state_initialize(&m_mavlink_status[chan]); -// -//mavlink_message_t* rxmsg = &m_mavlink_message[chan]; ///< The currently decoded message -//mavlink_status_t* status = &m_mavlink_status[chan]; ///< The current decode status -//int bufferIndex = 0; -// -//status->msg_received = 0; -// -//switch (status->parse_state) -//{ -//case MAVLINK_PARSE_STATE_UNINIT: -//case MAVLINK_PARSE_STATE_IDLE: -// if (c == MAVLINK_STX) -// { -// status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; -// mavlink_start_checksum(rxmsg); -// } -// break; -// -//case MAVLINK_PARSE_STATE_GOT_STX: -// if (status->msg_received) -// { -// status->buffer_overrun++; -// status->parse_error++; -// status->msg_received = 0; -// status->parse_state = MAVLINK_PARSE_STATE_IDLE; -// } -// else -// { -// // NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2 -// rxmsg->len = c; -// status->packet_idx = 0; -// mavlink_update_checksum(rxmsg, c); -// status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH; -// } -// break; -// -//case MAVLINK_PARSE_STATE_GOT_LENGTH: -// rxmsg->seq = c; -// mavlink_update_checksum(rxmsg, c); -// status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ; -// break; -// -//case MAVLINK_PARSE_STATE_GOT_SEQ: -// rxmsg->sysid = c; -// mavlink_update_checksum(rxmsg, c); -// status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID; -// break; -// -//case MAVLINK_PARSE_STATE_GOT_SYSID: -// rxmsg->compid = c; -// mavlink_update_checksum(rxmsg, c); -// status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID; -// break; -// -//case MAVLINK_PARSE_STATE_GOT_COMPID: -// rxmsg->msgid = c; -// mavlink_update_checksum(rxmsg, c); -// if (rxmsg->len == 0) -// { -// status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; -// } -// else -// { -// status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID; -// } -// break; -// -//case MAVLINK_PARSE_STATE_GOT_MSGID: -// rxmsg->payload[status->packet_idx++] = c; -// mavlink_update_checksum(rxmsg, c); -// if (status->packet_idx == rxmsg->len) -// { -// status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; -// } -// break; -// -//case MAVLINK_PARSE_STATE_GOT_PAYLOAD: -// if (c != rxmsg->ck_a) -// { -// // Check first checksum byte -// status->parse_error++; -// status->msg_received = 0; -// status->parse_state = MAVLINK_PARSE_STATE_IDLE; -// } -// else + + // For future use + +// if (status->msg_received == 1) // { -// status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1; -// } -// break; -// -//case MAVLINK_PARSE_STATE_GOT_CRC1: -// if (c != rxmsg->ck_b) -// {// Check second checksum byte -// status->parse_error++; -// status->msg_received = 0; -// status->parse_state = MAVLINK_PARSE_STATE_IDLE; +// if ( r_message != NULL ) +// { +// return r_message; +// } +// else +// { +// return rxmsg; +// } // } // else // { -// // Successfully got message -// status->msg_received = 1; -// status->parse_state = MAVLINK_PARSE_STATE_IDLE; -// memcpy(r_message, rxmsg, sizeof(mavlink_message_t)); +// return NULL; // } -// break; -//} - -bufferIndex++; -// If a message has been sucessfully decoded, check index -if (status->msg_received == 1) -{ - //while(status->current_seq != rxmsg->seq) - //{ - // status->packet_rx_drop_count++; - // status->current_seq++; - //} - status->current_seq = rxmsg->seq; - // Initial condition: If no packet has been received so far, drop count is undefined - if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0; - // Count this packet as received - status->packet_rx_success_count++; -} - -r_mavlink_status->current_seq = status->current_seq+1; -r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count; -r_mavlink_status->packet_rx_drop_count = status->parse_error; -return status->msg_received; -} - */ - - -typedef union __generic_16bit -{ - uint8_t b[2]; - int16_t s; -} generic_16bit; - -typedef union __generic_32bit -{ - uint8_t b[4]; - float f; - int32_t i; - int16_t s; -} generic_32bit; - -typedef union __generic_64bit -{ - uint8_t b[8]; - int64_t ll; ///< Long long (64 bit) - double d; ///< IEEE-754 double precision floating point -} generic_64bit; - -/** - * @brief Place an unsigned byte into the buffer - * - * @param b the byte to add - * @param bindex the position in the packet - * @param buffer the packet buffer - * @return the new position of the last used byte in the buffer - */ -static inline uint8_t put_uint8_t_by_index(uint8_t b, uint8_t bindex, uint8_t* buffer) -{ - *(buffer + bindex) = b; - return sizeof(b); -} - -/** - * @brief Place a signed byte into the buffer - * - * @param b the byte to add - * @param bindex the position in the packet - * @param buffer the packet buffer - * @return the new position of the last used byte in the buffer - */ -static inline uint8_t put_int8_t_by_index(int8_t b, int8_t bindex, uint8_t* buffer) -{ - *(buffer + bindex) = (uint8_t)b; - return sizeof(b); -} - -/** - * @brief Place two unsigned bytes into the buffer - * - * @param b the bytes to add - * @param bindex the position in the packet - * @param buffer the packet buffer - * @return the new position of the last used byte in the buffer - */ -static inline uint8_t put_uint16_t_by_index(uint16_t b, const uint8_t bindex, uint8_t* buffer) -{ - buffer[bindex] = (b>>8)&0xff; - buffer[bindex+1] = (b & 0xff); - return sizeof(b); -} - -/** - * @brief Place two signed bytes into the buffer - * - * @param b the bytes to add - * @param bindex the position in the packet - * @param buffer the packet buffer - * @return the new position of the last used byte in the buffer - */ -static inline uint8_t put_int16_t_by_index(int16_t b, uint8_t bindex, uint8_t* buffer) -{ - return put_uint16_t_by_index(b, bindex, buffer); -} - -/** - * @brief Place four unsigned bytes into the buffer - * - * @param b the bytes to add - * @param bindex the position in the packet - * @param buffer the packet buffer - * @return the new position of the last used byte in the buffer - */ -static inline uint8_t put_uint32_t_by_index(uint32_t b, const uint8_t bindex, uint8_t* buffer) -{ - buffer[bindex] = (b>>24)&0xff; - buffer[bindex+1] = (b>>16)&0xff; - buffer[bindex+2] = (b>>8)&0xff; - buffer[bindex+3] = (b & 0xff); - return sizeof(b); -} - -/** - * @brief Place four signed bytes into the buffer - * - * @param b the bytes to add - * @param bindex the position in the packet - * @param buffer the packet buffer - * @return the new position of the last used byte in the buffer - */ -static inline uint8_t put_int32_t_by_index(int32_t b, uint8_t bindex, uint8_t* buffer) -{ - buffer[bindex] = (b>>24)&0xff; - buffer[bindex+1] = (b>>16)&0xff; - buffer[bindex+2] = (b>>8)&0xff; - buffer[bindex+3] = (b & 0xff); - return sizeof(b); -} - -/** - * @brief Place four unsigned bytes into the buffer - * - * @param b the bytes to add - * @param bindex the position in the packet - * @param buffer the packet buffer - * @return the new position of the last used byte in the buffer - */ -static inline uint8_t put_uint64_t_by_index(uint64_t b, const uint8_t bindex, uint8_t* buffer) -{ - buffer[bindex] = (b>>56)&0xff; - buffer[bindex+1] = (b>>48)&0xff; - buffer[bindex+2] = (b>>40)&0xff; - buffer[bindex+3] = (b>>32)&0xff; - buffer[bindex+4] = (b>>24)&0xff; - buffer[bindex+5] = (b>>16)&0xff; - buffer[bindex+6] = (b>>8)&0xff; - buffer[bindex+7] = (b & 0xff); - return sizeof(b); -} - -/** - * @brief Place four signed bytes into the buffer - * - * @param b the bytes to add - * @param bindex the position in the packet - * @param buffer the packet buffer - * @return the new position of the last used byte in the buffer - */ -static inline uint8_t put_int64_t_by_index(int64_t b, uint8_t bindex, uint8_t* buffer) -{ - return put_uint64_t_by_index(b, bindex, buffer); -} - -/** - * @brief Place a float into the buffer - * - * @param b the float to add - * @param bindex the position in the packet - * @param buffer the packet buffer - * @return the new position of the last used byte in the buffer - */ -static inline uint8_t put_float_by_index(float b, uint8_t bindex, uint8_t* buffer) -{ - generic_32bit g; - g.f = b; - return put_int32_t_by_index(g.i, bindex, buffer); -} - -/** - * @brief Place an array into the buffer - * - * @param b the array to add - * @param length size of the array (for strings: length WITH '\0' char) - * @param bindex the position in the packet - * @param buffer packet buffer - * @return new position of the last used byte in the buffer - */ -static inline uint8_t put_array_by_index(const int8_t* b, uint8_t length, uint8_t bindex, uint8_t* buffer) -{ - memcpy(buffer+bindex, b, length); - return length; -} - -/** - * @brief Place a string into the buffer - * - * @param b the string to add - * @param maxlength size of the array (for strings: length WITHOUT '\0' char) - * @param bindex the position in the packet - * @param buffer packet buffer - * @return new position of the last used byte in the buffer - */ -static inline uint8_t put_string_by_index(const char* b, uint8_t maxlength, uint8_t bindex, uint8_t* buffer) -{ - uint16_t length = 0; - // Copy string into buffer, ensuring not to exceed the buffer size - int i; - for (i = 1; i < maxlength; i++) - { - length++; - // String characters - if (i < (maxlength - 1)) - { - buffer[bindex+i] = b[i]; - // Stop at null character - if (b[i] == '\0') - { - break; - } - } - // Enforce null termination at end of buffer - else if (i == (maxlength - 1)) - { - buffer[i] = '\0'; - } - } - // Write length into first field - put_uint8_t_by_index(length, bindex, buffer); - return length; -} - -/** - * @brief Put a bitfield of length 1-32 bit into the buffer - * - * @param b the value to add, will be encoded in the bitfield - * @param bits number of bits to use to encode b, e.g. 1 for boolean, 2, 3, etc. - * @param packet_index the position in the packet (the index of the first byte to use) - * @param bit_index the position in the byte (the index of the first bit to use) - * @param buffer packet buffer to write into - * @return new position of the last used byte in the buffer - */ -static inline uint8_t put_bitfield_n_by_index(int32_t b, uint8_t bits, uint8_t packet_index, uint8_t bit_index, uint8_t* r_bit_index, uint8_t* buffer) -{ - uint16_t bits_remain = bits; - // Transform number into network order - generic_32bit bin; - generic_32bit bout; - uint8_t i_bit_index, i_byte_index, curr_bits_n; - bin.i = b; - bout.b[0] = bin.b[3]; - bout.b[1] = bin.b[2]; - bout.b[2] = bin.b[1]; - bout.b[3] = bin.b[0]; - - // buffer in - // 01100000 01000000 00000000 11110001 - // buffer out - // 11110001 00000000 01000000 01100000 - - // Existing partly filled byte (four free slots) - // 0111xxxx - - // Mask n free bits - // 00001111 = 2^0 + 2^1 + 2^2 + 2^3 = 2^n - 1 - // = ((uint32_t)(1 << n)) - 1; // = 2^n - 1 - - // Shift n bits into the right position - // out = in >> n; - - // Mask and shift bytes - i_bit_index = bit_index; - i_byte_index = packet_index; - if (bit_index > 0) - { - // If bits were available at start, they were available - // in the byte before the current index - i_byte_index--; - } - - // While bits have not been packed yet - while (bits_remain > 0) - { - // Bits still have to be packed - // there can be more than 8 bits, so - // we might have to pack them into more than one byte - - // First pack everything we can into the current 'open' byte - //curr_bits_n = bits_remain << 3; // Equals bits_remain mod 8 - //FIXME - if (bits_remain <= (8 - i_bit_index)) - { - // Enough space - curr_bits_n = bits_remain; - } - else - { - curr_bits_n = (8 - i_bit_index); - } - - // Pack these n bits into the current byte - // Mask out whatever was at that position with ones (xxx11111) - buffer[i_byte_index] &= (0xFF >> (8 - curr_bits_n)); - // Put content to this position, by masking out the non-used part - buffer[i_byte_index] |= ((0x00 << curr_bits_n) & bout.i); - - // Increment the bit index - i_bit_index += curr_bits_n; - - // Now proceed to the next byte, if necessary - bits_remain -= curr_bits_n; - if (bits_remain > 0) - { - // Offer another 8 bits / one byte - i_byte_index++; - i_bit_index = 0; - } - } - - *r_bit_index = i_bit_index; - // If a partly filled byte is present, mark this as consumed - if (i_bit_index != 7) i_byte_index++; - return i_byte_index - packet_index; + return status->msg_received; } #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS @@ -859,107 +399,9 @@ void comm_send_ch(mavlink_channel_t chan, uint8_t ch) uart1_transmit(ch); } } - */ - -static inline void mavlink_send_uart_uint8_t(mavlink_channel_t chan, uint8_t b, uint16_t* checksum) -{ - crc_accumulate(b, checksum); - comm_send_ch(chan, b); -} - -static inline void mavlink_send_uart_int8_t(mavlink_channel_t chan, int8_t b, uint16_t* checksum) -{ - crc_accumulate(b, checksum); - comm_send_ch(chan, b); -} -static inline void mavlink_send_uart_uint16_t(mavlink_channel_t chan, uint16_t b, uint16_t* checksum) -{ - char s; - s = (b>>8)&0xff; - comm_send_ch(chan, s); - crc_accumulate(s, checksum); - s = (b & 0xff); - comm_send_ch(chan, s); - crc_accumulate(s, checksum); -} - -static inline void mavlink_send_uart_int16_t(mavlink_channel_t chan, int16_t b, uint16_t* checksum) -{ - mavlink_send_uart_uint16_t(chan, b, checksum); -} -static inline void mavlink_send_uart_uint32_t(mavlink_channel_t chan, uint32_t b, uint16_t* checksum) -{ - char s; - s = (b>>24)&0xff; - comm_send_ch(chan, s); - crc_accumulate(s, checksum); - s = (b>>16)&0xff; - comm_send_ch(chan, s); - crc_accumulate(s, checksum); - s = (b>>8)&0xff; - comm_send_ch(chan, s); - crc_accumulate(s, checksum); - s = (b & 0xff); - comm_send_ch(chan, s); - crc_accumulate(s, checksum); -} - -static inline void mavlink_send_uart_int32_t(mavlink_channel_t chan, int32_t b, uint16_t* checksum) -{ - mavlink_send_uart_uint32_t(chan, b, checksum); -} - -static inline void mavlink_send_uart_uint64_t(mavlink_channel_t chan, uint64_t b, uint16_t* checksum) -{ - char s; - s = (b>>56)&0xff; - comm_send_ch(chan, s); - crc_accumulate(s, checksum); - s = (b>>48)&0xff; - comm_send_ch(chan, s); - crc_accumulate(s, checksum); - s = (b>>40)&0xff; - comm_send_ch(chan, s); - crc_accumulate(s, checksum); - s = (b>>32)&0xff; - comm_send_ch(chan, s); - crc_accumulate(s, checksum); - s = (b>>24)&0xff; - comm_send_ch(chan, s); - crc_accumulate(s, checksum); - s = (b>>16)&0xff; - comm_send_ch(chan, s); - crc_accumulate(s, checksum); - s = (b>>8)&0xff; - comm_send_ch(chan, s); - crc_accumulate(s, checksum); - s = (b & 0xff); - comm_send_ch(chan, s); - crc_accumulate(s, checksum); -} - -static inline void mavlink_send_uart_int64_t(mavlink_channel_t chan, int64_t b, uint16_t* checksum) -{ - mavlink_send_uart_uint64_t(chan, b, checksum); -} - -static inline void mavlink_send_uart_float(mavlink_channel_t chan, float b, uint16_t* checksum) -{ - generic_32bit g; - g.f = b; - mavlink_send_uart_uint32_t(chan, g.i, checksum); -} - -static inline void mavlink_send_uart_double(mavlink_channel_t chan, double b, uint16_t* checksum) -{ - generic_64bit g; - g.d = b; - mavlink_send_uart_uint32_t(chan, g.ll, checksum); -} - -static inline void mavlink_send_uart(mavlink_channel_t chan, mavlink_message_t* msg) +static inline void mavlink_send_msg(mavlink_channel_t chan, mavlink_message_t* msg) { // ARM7 MCU board implementation // Create pointer on message struct @@ -977,6 +419,21 @@ static inline void mavlink_send_uart(mavlink_channel_t chan, mavlink_message_t* comm_send_ch(chan, msg->ck_a); comm_send_ch(chan, msg->ck_b); } + +static inline void mavlink_send_mem(mavlink_channel_t chan, (uint8_t *)mem, uint8_t num) +{ + // ARM7 MCU board implementation + // Create pointer on message struct + // Send STX + for(uint16_t i = 0; i < num; i++) + { + comm_send_ch( chan, mem[i] ); + } +} + */ +static inline void mavlink_send_uart(mavlink_channel_t chan, mavlink_message_t* msg); +static inline void mavlink_send_mem(mavlink_channel_t chan, (uint8_t *)mem, uint8_t num); +#define mavlink_send_msg( a, b ) mavlink_send_uart( a, b ) #endif #endif /* _MAVLINK_PROTOCOL_H_ */ diff --git a/thirdParty/mavlink/include/slugs/mavlink.h b/thirdParty/mavlink/include/slugs/mavlink.h index bf0e146b4c4ac71ac936d8deb85044d42555ee1c..04bffe2e3147b56e2e04dfcc61d43ce45740025c 100644 --- a/thirdParty/mavlink/include/slugs/mavlink.h +++ b/thirdParty/mavlink/include/slugs/mavlink.h @@ -1,11 +1,16 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Wednesday, July 27 2011, 14:17 UTC + * Generated on Tuesday, August 9 2011, 16:16 UTC */ #ifndef MAVLINK_H #define MAVLINK_H +#pragma pack(push,1) +#include "mavlink_options.h" #include "slugs.h" - +#ifdef MAVLINK_DATA +#include "mavlink_data.h" +#endif +#pragma pack(pop) #endif diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_air_data.h b/thirdParty/mavlink/include/slugs/mavlink_msg_air_data.h index 2b212ae1ba0d55d913204ec9fea1ba0f75920701..b345be106dfdaaebd76b1780efd65bff6f8646a5 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_air_data.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_air_data.h @@ -1,6 +1,8 @@ // MESSAGE AIR_DATA PACKING #define MAVLINK_MSG_ID_AIR_DATA 171 +#define MAVLINK_MSG_ID_AIR_DATA_LEN 10 +#define MAVLINK_MSG_171_LEN 10 typedef struct __mavlink_air_data_t { @@ -10,8 +12,6 @@ typedef struct __mavlink_air_data_t } mavlink_air_data_t; - - /** * @brief Pack a air_data message * @param system_id ID of this system @@ -25,14 +25,14 @@ typedef struct __mavlink_air_data_t */ static inline uint16_t mavlink_msg_air_data_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, float dynamicPressure, float staticPressure, uint16_t temperature) { - uint16_t i = 0; + mavlink_air_data_t *p = (mavlink_air_data_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_AIR_DATA; - i += put_float_by_index(dynamicPressure, i, msg->payload); // Dynamic pressure (Pa) - i += put_float_by_index(staticPressure, i, msg->payload); // Static pressure (Pa) - i += put_uint16_t_by_index(temperature, i, msg->payload); // Board temperature + p->dynamicPressure = dynamicPressure; // float:Dynamic pressure (Pa) + p->staticPressure = staticPressure; // float:Static pressure (Pa) + p->temperature = temperature; // uint16_t:Board temperature - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AIR_DATA_LEN); } /** @@ -48,14 +48,14 @@ static inline uint16_t mavlink_msg_air_data_pack(uint8_t system_id, uint8_t comp */ static inline uint16_t mavlink_msg_air_data_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, float dynamicPressure, float staticPressure, uint16_t temperature) { - uint16_t i = 0; + mavlink_air_data_t *p = (mavlink_air_data_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_AIR_DATA; - i += put_float_by_index(dynamicPressure, i, msg->payload); // Dynamic pressure (Pa) - i += put_float_by_index(staticPressure, i, msg->payload); // Static pressure (Pa) - i += put_uint16_t_by_index(temperature, i, msg->payload); // Board temperature + p->dynamicPressure = dynamicPressure; // float:Dynamic pressure (Pa) + p->staticPressure = staticPressure; // float:Static pressure (Pa) + p->temperature = temperature; // uint16_t:Board temperature - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AIR_DATA_LEN); } /** @@ -79,13 +79,37 @@ static inline uint16_t mavlink_msg_air_data_encode(uint8_t system_id, uint8_t co * @param staticPressure Static pressure (Pa) * @param temperature Board temperature */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_air_data_send(mavlink_channel_t chan, float dynamicPressure, float staticPressure, uint16_t temperature) { - mavlink_message_t msg; - mavlink_msg_air_data_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, dynamicPressure, staticPressure, temperature); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_air_data_t payload; + uint16_t checksum; + mavlink_air_data_t *p = &payload; + + p->dynamicPressure = dynamicPressure; // float:Dynamic pressure (Pa) + p->staticPressure = staticPressure; // float:Static pressure (Pa) + p->temperature = temperature; // uint16_t:Board temperature + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_AIR_DATA_LEN; + hdr.msgid = MAVLINK_MSG_ID_AIR_DATA; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -98,12 +122,8 @@ static inline void mavlink_msg_air_data_send(mavlink_channel_t chan, float dynam */ static inline float mavlink_msg_air_data_get_dynamicPressure(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 (float)r.f; + mavlink_air_data_t *p = (mavlink_air_data_t *)&msg->payload[0]; + return (float)(p->dynamicPressure); } /** @@ -113,12 +133,8 @@ static inline float mavlink_msg_air_data_get_dynamicPressure(const mavlink_messa */ static inline float mavlink_msg_air_data_get_staticPressure(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float))[3]; - return (float)r.f; + mavlink_air_data_t *p = (mavlink_air_data_t *)&msg->payload[0]; + return (float)(p->staticPressure); } /** @@ -128,10 +144,8 @@ static inline float mavlink_msg_air_data_get_staticPressure(const mavlink_messag */ static inline uint16_t mavlink_msg_air_data_get_temperature(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float))[0]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float))[1]; - return (uint16_t)r.s; + mavlink_air_data_t *p = (mavlink_air_data_t *)&msg->payload[0]; + return (uint16_t)(p->temperature); } /** @@ -142,7 +156,5 @@ static inline uint16_t mavlink_msg_air_data_get_temperature(const mavlink_messag */ static inline void mavlink_msg_air_data_decode(const mavlink_message_t* msg, mavlink_air_data_t* air_data) { - air_data->dynamicPressure = mavlink_msg_air_data_get_dynamicPressure(msg); - air_data->staticPressure = mavlink_msg_air_data_get_staticPressure(msg); - air_data->temperature = mavlink_msg_air_data_get_temperature(msg); + memcpy( air_data, msg->payload, sizeof(mavlink_air_data_t)); } diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_cpu_load.h b/thirdParty/mavlink/include/slugs/mavlink_msg_cpu_load.h index 265aa3fceeacaa25645b0196e7a078ff7ea38d6b..f30e1d8b6216a358abe85d47bee4510f9f43ed50 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_cpu_load.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_cpu_load.h @@ -1,17 +1,17 @@ // MESSAGE CPU_LOAD PACKING #define MAVLINK_MSG_ID_CPU_LOAD 170 +#define MAVLINK_MSG_ID_CPU_LOAD_LEN 4 +#define MAVLINK_MSG_170_LEN 4 typedef struct __mavlink_cpu_load_t { + uint16_t batVolt; ///< Battery Voltage in millivolts uint8_t sensLoad; ///< Sensor DSC Load uint8_t ctrlLoad; ///< Control DSC Load - uint16_t batVolt; ///< Battery Voltage in millivolts } mavlink_cpu_load_t; - - /** * @brief Pack a cpu_load message * @param system_id ID of this system @@ -25,14 +25,14 @@ typedef struct __mavlink_cpu_load_t */ static inline uint16_t mavlink_msg_cpu_load_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t sensLoad, uint8_t ctrlLoad, uint16_t batVolt) { - uint16_t i = 0; + mavlink_cpu_load_t *p = (mavlink_cpu_load_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_CPU_LOAD; - i += put_uint8_t_by_index(sensLoad, i, msg->payload); // Sensor DSC Load - i += put_uint8_t_by_index(ctrlLoad, i, msg->payload); // Control DSC Load - i += put_uint16_t_by_index(batVolt, i, msg->payload); // Battery Voltage in millivolts + p->sensLoad = sensLoad; // uint8_t:Sensor DSC Load + p->ctrlLoad = ctrlLoad; // uint8_t:Control DSC Load + p->batVolt = batVolt; // uint16_t:Battery Voltage in millivolts - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CPU_LOAD_LEN); } /** @@ -48,14 +48,14 @@ static inline uint16_t mavlink_msg_cpu_load_pack(uint8_t system_id, uint8_t comp */ static inline uint16_t mavlink_msg_cpu_load_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t sensLoad, uint8_t ctrlLoad, uint16_t batVolt) { - uint16_t i = 0; + mavlink_cpu_load_t *p = (mavlink_cpu_load_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_CPU_LOAD; - i += put_uint8_t_by_index(sensLoad, i, msg->payload); // Sensor DSC Load - i += put_uint8_t_by_index(ctrlLoad, i, msg->payload); // Control DSC Load - i += put_uint16_t_by_index(batVolt, i, msg->payload); // Battery Voltage in millivolts + p->sensLoad = sensLoad; // uint8_t:Sensor DSC Load + p->ctrlLoad = ctrlLoad; // uint8_t:Control DSC Load + p->batVolt = batVolt; // uint16_t:Battery Voltage in millivolts - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CPU_LOAD_LEN); } /** @@ -79,13 +79,37 @@ static inline uint16_t mavlink_msg_cpu_load_encode(uint8_t system_id, uint8_t co * @param ctrlLoad Control DSC Load * @param batVolt Battery Voltage in millivolts */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_cpu_load_send(mavlink_channel_t chan, uint8_t sensLoad, uint8_t ctrlLoad, uint16_t batVolt) { - mavlink_message_t msg; - mavlink_msg_cpu_load_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, sensLoad, ctrlLoad, batVolt); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_cpu_load_t payload; + uint16_t checksum; + mavlink_cpu_load_t *p = &payload; + + p->sensLoad = sensLoad; // uint8_t:Sensor DSC Load + p->ctrlLoad = ctrlLoad; // uint8_t:Control DSC Load + p->batVolt = batVolt; // uint16_t:Battery Voltage in millivolts + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_CPU_LOAD_LEN; + hdr.msgid = MAVLINK_MSG_ID_CPU_LOAD; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -98,7 +122,8 @@ static inline void mavlink_msg_cpu_load_send(mavlink_channel_t chan, uint8_t sen */ static inline uint8_t mavlink_msg_cpu_load_get_sensLoad(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_cpu_load_t *p = (mavlink_cpu_load_t *)&msg->payload[0]; + return (uint8_t)(p->sensLoad); } /** @@ -108,7 +133,8 @@ static inline uint8_t mavlink_msg_cpu_load_get_sensLoad(const mavlink_message_t* */ static inline uint8_t mavlink_msg_cpu_load_get_ctrlLoad(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_cpu_load_t *p = (mavlink_cpu_load_t *)&msg->payload[0]; + return (uint8_t)(p->ctrlLoad); } /** @@ -118,10 +144,8 @@ static inline uint8_t mavlink_msg_cpu_load_get_ctrlLoad(const mavlink_message_t* */ static inline uint16_t mavlink_msg_cpu_load_get_batVolt(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; + mavlink_cpu_load_t *p = (mavlink_cpu_load_t *)&msg->payload[0]; + return (uint16_t)(p->batVolt); } /** @@ -132,7 +156,5 @@ static inline uint16_t mavlink_msg_cpu_load_get_batVolt(const mavlink_message_t* */ static inline void mavlink_msg_cpu_load_decode(const mavlink_message_t* msg, mavlink_cpu_load_t* cpu_load) { - cpu_load->sensLoad = mavlink_msg_cpu_load_get_sensLoad(msg); - cpu_load->ctrlLoad = mavlink_msg_cpu_load_get_ctrlLoad(msg); - cpu_load->batVolt = mavlink_msg_cpu_load_get_batVolt(msg); + memcpy( cpu_load, msg->payload, sizeof(mavlink_cpu_load_t)); } diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_ctrl_srfc_pt.h b/thirdParty/mavlink/include/slugs/mavlink_msg_ctrl_srfc_pt.h index 9221dc4f00d1032d7b9b05a30501fbc5a716a7d3..81b5fa59a29a427330222c1d1ad0490f3029e8e0 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_ctrl_srfc_pt.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_ctrl_srfc_pt.h @@ -1,16 +1,16 @@ // MESSAGE CTRL_SRFC_PT PACKING #define MAVLINK_MSG_ID_CTRL_SRFC_PT 181 +#define MAVLINK_MSG_ID_CTRL_SRFC_PT_LEN 3 +#define MAVLINK_MSG_181_LEN 3 typedef struct __mavlink_ctrl_srfc_pt_t { - uint8_t target; ///< The system setting the commands uint16_t bitfieldPt; ///< Bitfield containing the PT configuration + uint8_t target; ///< The system setting the commands } mavlink_ctrl_srfc_pt_t; - - /** * @brief Pack a ctrl_srfc_pt message * @param system_id ID of this system @@ -23,13 +23,13 @@ typedef struct __mavlink_ctrl_srfc_pt_t */ static inline uint16_t mavlink_msg_ctrl_srfc_pt_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target, uint16_t bitfieldPt) { - uint16_t i = 0; + mavlink_ctrl_srfc_pt_t *p = (mavlink_ctrl_srfc_pt_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_CTRL_SRFC_PT; - i += put_uint8_t_by_index(target, i, msg->payload); // The system setting the commands - i += put_uint16_t_by_index(bitfieldPt, i, msg->payload); // Bitfield containing the PT configuration + p->target = target; // uint8_t:The system setting the commands + p->bitfieldPt = bitfieldPt; // uint16_t:Bitfield containing the PT configuration - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CTRL_SRFC_PT_LEN); } /** @@ -44,13 +44,13 @@ static inline uint16_t mavlink_msg_ctrl_srfc_pt_pack(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_ctrl_srfc_pt_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target, uint16_t bitfieldPt) { - uint16_t i = 0; + mavlink_ctrl_srfc_pt_t *p = (mavlink_ctrl_srfc_pt_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_CTRL_SRFC_PT; - i += put_uint8_t_by_index(target, i, msg->payload); // The system setting the commands - i += put_uint16_t_by_index(bitfieldPt, i, msg->payload); // Bitfield containing the PT configuration + p->target = target; // uint8_t:The system setting the commands + p->bitfieldPt = bitfieldPt; // uint16_t:Bitfield containing the PT configuration - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CTRL_SRFC_PT_LEN); } /** @@ -73,13 +73,36 @@ static inline uint16_t mavlink_msg_ctrl_srfc_pt_encode(uint8_t system_id, uint8_ * @param target The system setting the commands * @param bitfieldPt Bitfield containing the PT configuration */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_ctrl_srfc_pt_send(mavlink_channel_t chan, uint8_t target, uint16_t bitfieldPt) { - mavlink_message_t msg; - mavlink_msg_ctrl_srfc_pt_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target, bitfieldPt); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_ctrl_srfc_pt_t payload; + uint16_t checksum; + mavlink_ctrl_srfc_pt_t *p = &payload; + + p->target = target; // uint8_t:The system setting the commands + p->bitfieldPt = bitfieldPt; // uint16_t:Bitfield containing the PT configuration + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_CTRL_SRFC_PT_LEN; + hdr.msgid = MAVLINK_MSG_ID_CTRL_SRFC_PT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -92,7 +115,8 @@ static inline void mavlink_msg_ctrl_srfc_pt_send(mavlink_channel_t chan, uint8_t */ static inline uint8_t mavlink_msg_ctrl_srfc_pt_get_target(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_ctrl_srfc_pt_t *p = (mavlink_ctrl_srfc_pt_t *)&msg->payload[0]; + return (uint8_t)(p->target); } /** @@ -102,10 +126,8 @@ static inline uint8_t mavlink_msg_ctrl_srfc_pt_get_target(const mavlink_message_ */ static inline uint16_t mavlink_msg_ctrl_srfc_pt_get_bitfieldPt(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t))[1]; - return (uint16_t)r.s; + mavlink_ctrl_srfc_pt_t *p = (mavlink_ctrl_srfc_pt_t *)&msg->payload[0]; + return (uint16_t)(p->bitfieldPt); } /** @@ -116,6 +138,5 @@ static inline uint16_t mavlink_msg_ctrl_srfc_pt_get_bitfieldPt(const mavlink_mes */ static inline void mavlink_msg_ctrl_srfc_pt_decode(const mavlink_message_t* msg, mavlink_ctrl_srfc_pt_t* ctrl_srfc_pt) { - ctrl_srfc_pt->target = mavlink_msg_ctrl_srfc_pt_get_target(msg); - ctrl_srfc_pt->bitfieldPt = mavlink_msg_ctrl_srfc_pt_get_bitfieldPt(msg); + memcpy( ctrl_srfc_pt, msg->payload, sizeof(mavlink_ctrl_srfc_pt_t)); } diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_data_log.h b/thirdParty/mavlink/include/slugs/mavlink_msg_data_log.h index cbcc0daba3804ba35822e4bc6f2b429cdeaba783..14da8b9a99688e7bb02e628c7b1a3cd32528dab2 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_data_log.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_data_log.h @@ -1,6 +1,8 @@ // MESSAGE DATA_LOG PACKING #define MAVLINK_MSG_ID_DATA_LOG 177 +#define MAVLINK_MSG_ID_DATA_LOG_LEN 24 +#define MAVLINK_MSG_177_LEN 24 typedef struct __mavlink_data_log_t { @@ -13,8 +15,6 @@ typedef struct __mavlink_data_log_t } mavlink_data_log_t; - - /** * @brief Pack a data_log message * @param system_id ID of this system @@ -31,17 +31,17 @@ typedef struct __mavlink_data_log_t */ static inline uint16_t mavlink_msg_data_log_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, float fl_1, float fl_2, float fl_3, float fl_4, float fl_5, float fl_6) { - uint16_t i = 0; + mavlink_data_log_t *p = (mavlink_data_log_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_DATA_LOG; - i += put_float_by_index(fl_1, i, msg->payload); // Log value 1 - i += put_float_by_index(fl_2, i, msg->payload); // Log value 2 - i += put_float_by_index(fl_3, i, msg->payload); // Log value 3 - i += put_float_by_index(fl_4, i, msg->payload); // Log value 4 - i += put_float_by_index(fl_5, i, msg->payload); // Log value 5 - i += put_float_by_index(fl_6, i, msg->payload); // Log value 6 + p->fl_1 = fl_1; // float:Log value 1 + p->fl_2 = fl_2; // float:Log value 2 + p->fl_3 = fl_3; // float:Log value 3 + p->fl_4 = fl_4; // float:Log value 4 + p->fl_5 = fl_5; // float:Log value 5 + p->fl_6 = fl_6; // float:Log value 6 - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA_LOG_LEN); } /** @@ -60,17 +60,17 @@ static inline uint16_t mavlink_msg_data_log_pack(uint8_t system_id, uint8_t comp */ static inline uint16_t mavlink_msg_data_log_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, float fl_1, float fl_2, float fl_3, float fl_4, float fl_5, float fl_6) { - uint16_t i = 0; + mavlink_data_log_t *p = (mavlink_data_log_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_DATA_LOG; - i += put_float_by_index(fl_1, i, msg->payload); // Log value 1 - i += put_float_by_index(fl_2, i, msg->payload); // Log value 2 - i += put_float_by_index(fl_3, i, msg->payload); // Log value 3 - i += put_float_by_index(fl_4, i, msg->payload); // Log value 4 - i += put_float_by_index(fl_5, i, msg->payload); // Log value 5 - i += put_float_by_index(fl_6, i, msg->payload); // Log value 6 + p->fl_1 = fl_1; // float:Log value 1 + p->fl_2 = fl_2; // float:Log value 2 + p->fl_3 = fl_3; // float:Log value 3 + p->fl_4 = fl_4; // float:Log value 4 + p->fl_5 = fl_5; // float:Log value 5 + p->fl_6 = fl_6; // float:Log value 6 - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA_LOG_LEN); } /** @@ -97,13 +97,40 @@ static inline uint16_t mavlink_msg_data_log_encode(uint8_t system_id, uint8_t co * @param fl_5 Log value 5 * @param fl_6 Log value 6 */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_data_log_send(mavlink_channel_t chan, float fl_1, float fl_2, float fl_3, float fl_4, float fl_5, float fl_6) { - mavlink_message_t msg; - mavlink_msg_data_log_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, fl_1, fl_2, fl_3, fl_4, fl_5, fl_6); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_data_log_t payload; + uint16_t checksum; + mavlink_data_log_t *p = &payload; + + p->fl_1 = fl_1; // float:Log value 1 + p->fl_2 = fl_2; // float:Log value 2 + p->fl_3 = fl_3; // float:Log value 3 + p->fl_4 = fl_4; // float:Log value 4 + p->fl_5 = fl_5; // float:Log value 5 + p->fl_6 = fl_6; // float:Log value 6 + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_DATA_LOG_LEN; + hdr.msgid = MAVLINK_MSG_ID_DATA_LOG; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -116,12 +143,8 @@ static inline void mavlink_msg_data_log_send(mavlink_channel_t chan, float fl_1, */ static inline float mavlink_msg_data_log_get_fl_1(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 (float)r.f; + mavlink_data_log_t *p = (mavlink_data_log_t *)&msg->payload[0]; + return (float)(p->fl_1); } /** @@ -131,12 +154,8 @@ static inline float mavlink_msg_data_log_get_fl_1(const mavlink_message_t* msg) */ static inline float mavlink_msg_data_log_get_fl_2(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float))[3]; - return (float)r.f; + mavlink_data_log_t *p = (mavlink_data_log_t *)&msg->payload[0]; + return (float)(p->fl_2); } /** @@ -146,12 +165,8 @@ static inline float mavlink_msg_data_log_get_fl_2(const mavlink_message_t* msg) */ static inline float mavlink_msg_data_log_get_fl_3(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_data_log_t *p = (mavlink_data_log_t *)&msg->payload[0]; + return (float)(p->fl_3); } /** @@ -161,12 +176,8 @@ static inline float mavlink_msg_data_log_get_fl_3(const mavlink_message_t* msg) */ static inline float mavlink_msg_data_log_get_fl_4(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_data_log_t *p = (mavlink_data_log_t *)&msg->payload[0]; + return (float)(p->fl_4); } /** @@ -176,12 +187,8 @@ static inline float mavlink_msg_data_log_get_fl_4(const mavlink_message_t* msg) */ static inline float mavlink_msg_data_log_get_fl_5(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_data_log_t *p = (mavlink_data_log_t *)&msg->payload[0]; + return (float)(p->fl_5); } /** @@ -191,12 +198,8 @@ static inline float mavlink_msg_data_log_get_fl_5(const mavlink_message_t* msg) */ static inline float mavlink_msg_data_log_get_fl_6(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_data_log_t *p = (mavlink_data_log_t *)&msg->payload[0]; + return (float)(p->fl_6); } /** @@ -207,10 +210,5 @@ static inline float mavlink_msg_data_log_get_fl_6(const mavlink_message_t* msg) */ static inline void mavlink_msg_data_log_decode(const mavlink_message_t* msg, mavlink_data_log_t* data_log) { - data_log->fl_1 = mavlink_msg_data_log_get_fl_1(msg); - data_log->fl_2 = mavlink_msg_data_log_get_fl_2(msg); - data_log->fl_3 = mavlink_msg_data_log_get_fl_3(msg); - data_log->fl_4 = mavlink_msg_data_log_get_fl_4(msg); - data_log->fl_5 = mavlink_msg_data_log_get_fl_5(msg); - data_log->fl_6 = mavlink_msg_data_log_get_fl_6(msg); + memcpy( data_log, msg->payload, sizeof(mavlink_data_log_t)); } diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_diagnostic.h b/thirdParty/mavlink/include/slugs/mavlink_msg_diagnostic.h index f3798f59bb869b8c74a4611507c8abcffb2c13db..e9aa6279b9c35c0885ae28617081441e35dd7581 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_diagnostic.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_diagnostic.h @@ -1,6 +1,8 @@ // MESSAGE DIAGNOSTIC PACKING #define MAVLINK_MSG_ID_DIAGNOSTIC 173 +#define MAVLINK_MSG_ID_DIAGNOSTIC_LEN 18 +#define MAVLINK_MSG_173_LEN 18 typedef struct __mavlink_diagnostic_t { @@ -13,8 +15,6 @@ typedef struct __mavlink_diagnostic_t } mavlink_diagnostic_t; - - /** * @brief Pack a diagnostic message * @param system_id ID of this system @@ -31,17 +31,17 @@ typedef struct __mavlink_diagnostic_t */ static inline uint16_t mavlink_msg_diagnostic_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, float diagFl1, float diagFl2, float diagFl3, int16_t diagSh1, int16_t diagSh2, int16_t diagSh3) { - uint16_t i = 0; + mavlink_diagnostic_t *p = (mavlink_diagnostic_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_DIAGNOSTIC; - i += put_float_by_index(diagFl1, i, msg->payload); // Diagnostic float 1 - i += put_float_by_index(diagFl2, i, msg->payload); // Diagnostic float 2 - i += put_float_by_index(diagFl3, i, msg->payload); // Diagnostic float 3 - i += put_int16_t_by_index(diagSh1, i, msg->payload); // Diagnostic short 1 - i += put_int16_t_by_index(diagSh2, i, msg->payload); // Diagnostic short 2 - i += put_int16_t_by_index(diagSh3, i, msg->payload); // Diagnostic short 3 + p->diagFl1 = diagFl1; // float:Diagnostic float 1 + p->diagFl2 = diagFl2; // float:Diagnostic float 2 + p->diagFl3 = diagFl3; // float:Diagnostic float 3 + p->diagSh1 = diagSh1; // int16_t:Diagnostic short 1 + p->diagSh2 = diagSh2; // int16_t:Diagnostic short 2 + p->diagSh3 = diagSh3; // int16_t:Diagnostic short 3 - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DIAGNOSTIC_LEN); } /** @@ -60,17 +60,17 @@ static inline uint16_t mavlink_msg_diagnostic_pack(uint8_t system_id, uint8_t co */ static inline uint16_t mavlink_msg_diagnostic_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, float diagFl1, float diagFl2, float diagFl3, int16_t diagSh1, int16_t diagSh2, int16_t diagSh3) { - uint16_t i = 0; + mavlink_diagnostic_t *p = (mavlink_diagnostic_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_DIAGNOSTIC; - i += put_float_by_index(diagFl1, i, msg->payload); // Diagnostic float 1 - i += put_float_by_index(diagFl2, i, msg->payload); // Diagnostic float 2 - i += put_float_by_index(diagFl3, i, msg->payload); // Diagnostic float 3 - i += put_int16_t_by_index(diagSh1, i, msg->payload); // Diagnostic short 1 - i += put_int16_t_by_index(diagSh2, i, msg->payload); // Diagnostic short 2 - i += put_int16_t_by_index(diagSh3, i, msg->payload); // Diagnostic short 3 + p->diagFl1 = diagFl1; // float:Diagnostic float 1 + p->diagFl2 = diagFl2; // float:Diagnostic float 2 + p->diagFl3 = diagFl3; // float:Diagnostic float 3 + p->diagSh1 = diagSh1; // int16_t:Diagnostic short 1 + p->diagSh2 = diagSh2; // int16_t:Diagnostic short 2 + p->diagSh3 = diagSh3; // int16_t:Diagnostic short 3 - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DIAGNOSTIC_LEN); } /** @@ -97,13 +97,40 @@ static inline uint16_t mavlink_msg_diagnostic_encode(uint8_t system_id, uint8_t * @param diagSh2 Diagnostic short 2 * @param diagSh3 Diagnostic short 3 */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_diagnostic_send(mavlink_channel_t chan, float diagFl1, float diagFl2, float diagFl3, int16_t diagSh1, int16_t diagSh2, int16_t diagSh3) { - mavlink_message_t msg; - mavlink_msg_diagnostic_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, diagFl1, diagFl2, diagFl3, diagSh1, diagSh2, diagSh3); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_diagnostic_t payload; + uint16_t checksum; + mavlink_diagnostic_t *p = &payload; + + p->diagFl1 = diagFl1; // float:Diagnostic float 1 + p->diagFl2 = diagFl2; // float:Diagnostic float 2 + p->diagFl3 = diagFl3; // float:Diagnostic float 3 + p->diagSh1 = diagSh1; // int16_t:Diagnostic short 1 + p->diagSh2 = diagSh2; // int16_t:Diagnostic short 2 + p->diagSh3 = diagSh3; // int16_t:Diagnostic short 3 + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_DIAGNOSTIC_LEN; + hdr.msgid = MAVLINK_MSG_ID_DIAGNOSTIC; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -116,12 +143,8 @@ static inline void mavlink_msg_diagnostic_send(mavlink_channel_t chan, float dia */ static inline float mavlink_msg_diagnostic_get_diagFl1(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 (float)r.f; + mavlink_diagnostic_t *p = (mavlink_diagnostic_t *)&msg->payload[0]; + return (float)(p->diagFl1); } /** @@ -131,12 +154,8 @@ static inline float mavlink_msg_diagnostic_get_diagFl1(const mavlink_message_t* */ static inline float mavlink_msg_diagnostic_get_diagFl2(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float))[3]; - return (float)r.f; + mavlink_diagnostic_t *p = (mavlink_diagnostic_t *)&msg->payload[0]; + return (float)(p->diagFl2); } /** @@ -146,12 +165,8 @@ static inline float mavlink_msg_diagnostic_get_diagFl2(const mavlink_message_t* */ static inline float mavlink_msg_diagnostic_get_diagFl3(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_diagnostic_t *p = (mavlink_diagnostic_t *)&msg->payload[0]; + return (float)(p->diagFl3); } /** @@ -161,10 +176,8 @@ static inline float mavlink_msg_diagnostic_get_diagFl3(const mavlink_message_t* */ static inline int16_t mavlink_msg_diagnostic_get_diagSh1(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[1]; - return (int16_t)r.s; + mavlink_diagnostic_t *p = (mavlink_diagnostic_t *)&msg->payload[0]; + return (int16_t)(p->diagSh1); } /** @@ -174,10 +187,8 @@ static inline int16_t mavlink_msg_diagnostic_get_diagSh1(const mavlink_message_t */ static inline int16_t mavlink_msg_diagnostic_get_diagSh2(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_diagnostic_t *p = (mavlink_diagnostic_t *)&msg->payload[0]; + return (int16_t)(p->diagSh2); } /** @@ -187,10 +198,8 @@ static inline int16_t mavlink_msg_diagnostic_get_diagSh2(const mavlink_message_t */ static inline int16_t mavlink_msg_diagnostic_get_diagSh3(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_diagnostic_t *p = (mavlink_diagnostic_t *)&msg->payload[0]; + return (int16_t)(p->diagSh3); } /** @@ -201,10 +210,5 @@ static inline int16_t mavlink_msg_diagnostic_get_diagSh3(const mavlink_message_t */ static inline void mavlink_msg_diagnostic_decode(const mavlink_message_t* msg, mavlink_diagnostic_t* diagnostic) { - diagnostic->diagFl1 = mavlink_msg_diagnostic_get_diagFl1(msg); - diagnostic->diagFl2 = mavlink_msg_diagnostic_get_diagFl2(msg); - diagnostic->diagFl3 = mavlink_msg_diagnostic_get_diagFl3(msg); - diagnostic->diagSh1 = mavlink_msg_diagnostic_get_diagSh1(msg); - diagnostic->diagSh2 = mavlink_msg_diagnostic_get_diagSh2(msg); - diagnostic->diagSh3 = mavlink_msg_diagnostic_get_diagSh3(msg); + memcpy( diagnostic, msg->payload, sizeof(mavlink_diagnostic_t)); } diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_gps_date_time.h b/thirdParty/mavlink/include/slugs/mavlink_msg_gps_date_time.h index 199cbb9ec264f0e8a15920aedb42fdcd373763d5..bab695cfa408dd0392d8c8287d056ea90a5f8478 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_gps_date_time.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_gps_date_time.h @@ -1,6 +1,8 @@ // MESSAGE GPS_DATE_TIME PACKING #define MAVLINK_MSG_ID_GPS_DATE_TIME 179 +#define MAVLINK_MSG_ID_GPS_DATE_TIME_LEN 7 +#define MAVLINK_MSG_179_LEN 7 typedef struct __mavlink_gps_date_time_t { @@ -14,8 +16,6 @@ typedef struct __mavlink_gps_date_time_t } mavlink_gps_date_time_t; - - /** * @brief Pack a gps_date_time message * @param system_id ID of this system @@ -33,18 +33,18 @@ typedef struct __mavlink_gps_date_time_t */ static inline uint16_t mavlink_msg_gps_date_time_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t year, uint8_t month, uint8_t day, uint8_t hour, uint8_t min, uint8_t sec, uint8_t visSat) { - uint16_t i = 0; + mavlink_gps_date_time_t *p = (mavlink_gps_date_time_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GPS_DATE_TIME; - i += put_uint8_t_by_index(year, i, msg->payload); // Year reported by Gps - i += put_uint8_t_by_index(month, i, msg->payload); // Month reported by Gps - i += put_uint8_t_by_index(day, i, msg->payload); // Day reported by Gps - i += put_uint8_t_by_index(hour, i, msg->payload); // Hour reported by Gps - i += put_uint8_t_by_index(min, i, msg->payload); // Min reported by Gps - i += put_uint8_t_by_index(sec, i, msg->payload); // Sec reported by Gps - i += put_uint8_t_by_index(visSat, i, msg->payload); // Visible sattelites reported by Gps + p->year = year; // uint8_t:Year reported by Gps + p->month = month; // uint8_t:Month reported by Gps + p->day = day; // uint8_t:Day reported by Gps + p->hour = hour; // uint8_t:Hour reported by Gps + p->min = min; // uint8_t:Min reported by Gps + p->sec = sec; // uint8_t:Sec reported by Gps + p->visSat = visSat; // uint8_t:Visible sattelites reported by Gps - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_DATE_TIME_LEN); } /** @@ -64,18 +64,18 @@ static inline uint16_t mavlink_msg_gps_date_time_pack(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_gps_date_time_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t year, uint8_t month, uint8_t day, uint8_t hour, uint8_t min, uint8_t sec, uint8_t visSat) { - uint16_t i = 0; + mavlink_gps_date_time_t *p = (mavlink_gps_date_time_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GPS_DATE_TIME; - i += put_uint8_t_by_index(year, i, msg->payload); // Year reported by Gps - i += put_uint8_t_by_index(month, i, msg->payload); // Month reported by Gps - i += put_uint8_t_by_index(day, i, msg->payload); // Day reported by Gps - i += put_uint8_t_by_index(hour, i, msg->payload); // Hour reported by Gps - i += put_uint8_t_by_index(min, i, msg->payload); // Min reported by Gps - i += put_uint8_t_by_index(sec, i, msg->payload); // Sec reported by Gps - i += put_uint8_t_by_index(visSat, i, msg->payload); // Visible sattelites reported by Gps + p->year = year; // uint8_t:Year reported by Gps + p->month = month; // uint8_t:Month reported by Gps + p->day = day; // uint8_t:Day reported by Gps + p->hour = hour; // uint8_t:Hour reported by Gps + p->min = min; // uint8_t:Min reported by Gps + p->sec = sec; // uint8_t:Sec reported by Gps + p->visSat = visSat; // uint8_t:Visible sattelites reported by Gps - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_DATE_TIME_LEN); } /** @@ -103,13 +103,41 @@ static inline uint16_t mavlink_msg_gps_date_time_encode(uint8_t system_id, uint8 * @param sec Sec reported by Gps * @param visSat Visible sattelites reported by Gps */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_gps_date_time_send(mavlink_channel_t chan, uint8_t year, uint8_t month, uint8_t day, uint8_t hour, uint8_t min, uint8_t sec, uint8_t visSat) { - mavlink_message_t msg; - mavlink_msg_gps_date_time_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, year, month, day, hour, min, sec, visSat); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_gps_date_time_t payload; + uint16_t checksum; + mavlink_gps_date_time_t *p = &payload; + + p->year = year; // uint8_t:Year reported by Gps + p->month = month; // uint8_t:Month reported by Gps + p->day = day; // uint8_t:Day reported by Gps + p->hour = hour; // uint8_t:Hour reported by Gps + p->min = min; // uint8_t:Min reported by Gps + p->sec = sec; // uint8_t:Sec reported by Gps + p->visSat = visSat; // uint8_t:Visible sattelites reported by Gps + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_GPS_DATE_TIME_LEN; + hdr.msgid = MAVLINK_MSG_ID_GPS_DATE_TIME; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -122,7 +150,8 @@ static inline void mavlink_msg_gps_date_time_send(mavlink_channel_t chan, uint8_ */ static inline uint8_t mavlink_msg_gps_date_time_get_year(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_gps_date_time_t *p = (mavlink_gps_date_time_t *)&msg->payload[0]; + return (uint8_t)(p->year); } /** @@ -132,7 +161,8 @@ static inline uint8_t mavlink_msg_gps_date_time_get_year(const mavlink_message_t */ static inline uint8_t mavlink_msg_gps_date_time_get_month(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_gps_date_time_t *p = (mavlink_gps_date_time_t *)&msg->payload[0]; + return (uint8_t)(p->month); } /** @@ -142,7 +172,8 @@ static inline uint8_t mavlink_msg_gps_date_time_get_month(const mavlink_message_ */ static inline uint8_t mavlink_msg_gps_date_time_get_day(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_gps_date_time_t *p = (mavlink_gps_date_time_t *)&msg->payload[0]; + return (uint8_t)(p->day); } /** @@ -152,7 +183,8 @@ static inline uint8_t mavlink_msg_gps_date_time_get_day(const mavlink_message_t* */ static inline uint8_t mavlink_msg_gps_date_time_get_hour(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_gps_date_time_t *p = (mavlink_gps_date_time_t *)&msg->payload[0]; + return (uint8_t)(p->hour); } /** @@ -162,7 +194,8 @@ static inline uint8_t mavlink_msg_gps_date_time_get_hour(const mavlink_message_t */ static inline uint8_t mavlink_msg_gps_date_time_get_min(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_gps_date_time_t *p = (mavlink_gps_date_time_t *)&msg->payload[0]; + return (uint8_t)(p->min); } /** @@ -172,7 +205,8 @@ static inline uint8_t mavlink_msg_gps_date_time_get_min(const mavlink_message_t* */ static inline uint8_t mavlink_msg_gps_date_time_get_sec(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_gps_date_time_t *p = (mavlink_gps_date_time_t *)&msg->payload[0]; + return (uint8_t)(p->sec); } /** @@ -182,7 +216,8 @@ static inline uint8_t mavlink_msg_gps_date_time_get_sec(const mavlink_message_t* */ static inline uint8_t mavlink_msg_gps_date_time_get_visSat(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_gps_date_time_t *p = (mavlink_gps_date_time_t *)&msg->payload[0]; + return (uint8_t)(p->visSat); } /** @@ -193,11 +228,5 @@ static inline uint8_t mavlink_msg_gps_date_time_get_visSat(const mavlink_message */ static inline void mavlink_msg_gps_date_time_decode(const mavlink_message_t* msg, mavlink_gps_date_time_t* gps_date_time) { - gps_date_time->year = mavlink_msg_gps_date_time_get_year(msg); - gps_date_time->month = mavlink_msg_gps_date_time_get_month(msg); - gps_date_time->day = mavlink_msg_gps_date_time_get_day(msg); - gps_date_time->hour = mavlink_msg_gps_date_time_get_hour(msg); - gps_date_time->min = mavlink_msg_gps_date_time_get_min(msg); - gps_date_time->sec = mavlink_msg_gps_date_time_get_sec(msg); - gps_date_time->visSat = mavlink_msg_gps_date_time_get_visSat(msg); + memcpy( gps_date_time, msg->payload, sizeof(mavlink_gps_date_time_t)); } diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_mid_lvl_cmds.h b/thirdParty/mavlink/include/slugs/mavlink_msg_mid_lvl_cmds.h index a0d78b1ceb9e627377ed4af66808d0c8838172dd..0436ae9498cc578ab304b9784a88fa1d57d9d8eb 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_mid_lvl_cmds.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_mid_lvl_cmds.h @@ -1,18 +1,18 @@ // MESSAGE MID_LVL_CMDS PACKING #define MAVLINK_MSG_ID_MID_LVL_CMDS 180 +#define MAVLINK_MSG_ID_MID_LVL_CMDS_LEN 13 +#define MAVLINK_MSG_180_LEN 13 typedef struct __mavlink_mid_lvl_cmds_t { - uint8_t target; ///< The system setting the commands float hCommand; ///< Commanded Airspeed float uCommand; ///< Log value 2 float rCommand; ///< Log value 3 + uint8_t target; ///< The system setting the commands } mavlink_mid_lvl_cmds_t; - - /** * @brief Pack a mid_lvl_cmds message * @param system_id ID of this system @@ -27,15 +27,15 @@ typedef struct __mavlink_mid_lvl_cmds_t */ static inline uint16_t mavlink_msg_mid_lvl_cmds_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target, float hCommand, float uCommand, float rCommand) { - uint16_t i = 0; + mavlink_mid_lvl_cmds_t *p = (mavlink_mid_lvl_cmds_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_MID_LVL_CMDS; - i += put_uint8_t_by_index(target, i, msg->payload); // The system setting the commands - i += put_float_by_index(hCommand, i, msg->payload); // Commanded Airspeed - i += put_float_by_index(uCommand, i, msg->payload); // Log value 2 - i += put_float_by_index(rCommand, i, msg->payload); // Log value 3 + p->target = target; // uint8_t:The system setting the commands + p->hCommand = hCommand; // float:Commanded Airspeed + p->uCommand = uCommand; // float:Log value 2 + p->rCommand = rCommand; // float:Log value 3 - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MID_LVL_CMDS_LEN); } /** @@ -52,15 +52,15 @@ static inline uint16_t mavlink_msg_mid_lvl_cmds_pack(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_mid_lvl_cmds_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target, float hCommand, float uCommand, float rCommand) { - uint16_t i = 0; + mavlink_mid_lvl_cmds_t *p = (mavlink_mid_lvl_cmds_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_MID_LVL_CMDS; - i += put_uint8_t_by_index(target, i, msg->payload); // The system setting the commands - i += put_float_by_index(hCommand, i, msg->payload); // Commanded Airspeed - i += put_float_by_index(uCommand, i, msg->payload); // Log value 2 - i += put_float_by_index(rCommand, i, msg->payload); // Log value 3 + p->target = target; // uint8_t:The system setting the commands + p->hCommand = hCommand; // float:Commanded Airspeed + p->uCommand = uCommand; // float:Log value 2 + p->rCommand = rCommand; // float:Log value 3 - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MID_LVL_CMDS_LEN); } /** @@ -85,13 +85,38 @@ static inline uint16_t mavlink_msg_mid_lvl_cmds_encode(uint8_t system_id, uint8_ * @param uCommand Log value 2 * @param rCommand Log value 3 */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_mid_lvl_cmds_send(mavlink_channel_t chan, uint8_t target, float hCommand, float uCommand, float rCommand) { - mavlink_message_t msg; - mavlink_msg_mid_lvl_cmds_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target, hCommand, uCommand, rCommand); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_mid_lvl_cmds_t payload; + uint16_t checksum; + mavlink_mid_lvl_cmds_t *p = &payload; + + p->target = target; // uint8_t:The system setting the commands + p->hCommand = hCommand; // float:Commanded Airspeed + p->uCommand = uCommand; // float:Log value 2 + p->rCommand = rCommand; // float:Log value 3 + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_MID_LVL_CMDS_LEN; + hdr.msgid = MAVLINK_MSG_ID_MID_LVL_CMDS; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -104,7 +129,8 @@ static inline void mavlink_msg_mid_lvl_cmds_send(mavlink_channel_t chan, uint8_t */ static inline uint8_t mavlink_msg_mid_lvl_cmds_get_target(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_mid_lvl_cmds_t *p = (mavlink_mid_lvl_cmds_t *)&msg->payload[0]; + return (uint8_t)(p->target); } /** @@ -114,12 +140,8 @@ static inline uint8_t mavlink_msg_mid_lvl_cmds_get_target(const mavlink_message_ */ static inline float mavlink_msg_mid_lvl_cmds_get_hCommand(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t))[3]; - return (float)r.f; + mavlink_mid_lvl_cmds_t *p = (mavlink_mid_lvl_cmds_t *)&msg->payload[0]; + return (float)(p->hCommand); } /** @@ -129,12 +151,8 @@ static inline float mavlink_msg_mid_lvl_cmds_get_hCommand(const mavlink_message_ */ static inline float mavlink_msg_mid_lvl_cmds_get_uCommand(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(float))[3]; - return (float)r.f; + mavlink_mid_lvl_cmds_t *p = (mavlink_mid_lvl_cmds_t *)&msg->payload[0]; + return (float)(p->uCommand); } /** @@ -144,12 +162,8 @@ static inline float mavlink_msg_mid_lvl_cmds_get_uCommand(const mavlink_message_ */ static inline float mavlink_msg_mid_lvl_cmds_get_rCommand(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_mid_lvl_cmds_t *p = (mavlink_mid_lvl_cmds_t *)&msg->payload[0]; + return (float)(p->rCommand); } /** @@ -160,8 +174,5 @@ static inline float mavlink_msg_mid_lvl_cmds_get_rCommand(const mavlink_message_ */ static inline void mavlink_msg_mid_lvl_cmds_decode(const mavlink_message_t* msg, mavlink_mid_lvl_cmds_t* mid_lvl_cmds) { - mid_lvl_cmds->target = mavlink_msg_mid_lvl_cmds_get_target(msg); - mid_lvl_cmds->hCommand = mavlink_msg_mid_lvl_cmds_get_hCommand(msg); - mid_lvl_cmds->uCommand = mavlink_msg_mid_lvl_cmds_get_uCommand(msg); - mid_lvl_cmds->rCommand = mavlink_msg_mid_lvl_cmds_get_rCommand(msg); + memcpy( mid_lvl_cmds, msg->payload, sizeof(mavlink_mid_lvl_cmds_t)); } diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_sensor_bias.h b/thirdParty/mavlink/include/slugs/mavlink_msg_sensor_bias.h index 8b40bdd67a774b6275904f2e4897370f2e69fb33..5601a2a344f36e5ed988bcb386522eae204cede0 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_sensor_bias.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_sensor_bias.h @@ -1,6 +1,8 @@ // MESSAGE SENSOR_BIAS PACKING #define MAVLINK_MSG_ID_SENSOR_BIAS 172 +#define MAVLINK_MSG_ID_SENSOR_BIAS_LEN 24 +#define MAVLINK_MSG_172_LEN 24 typedef struct __mavlink_sensor_bias_t { @@ -13,8 +15,6 @@ typedef struct __mavlink_sensor_bias_t } mavlink_sensor_bias_t; - - /** * @brief Pack a sensor_bias message * @param system_id ID of this system @@ -31,17 +31,17 @@ typedef struct __mavlink_sensor_bias_t */ static inline uint16_t mavlink_msg_sensor_bias_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, float axBias, float ayBias, float azBias, float gxBias, float gyBias, float gzBias) { - uint16_t i = 0; + mavlink_sensor_bias_t *p = (mavlink_sensor_bias_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SENSOR_BIAS; - i += put_float_by_index(axBias, i, msg->payload); // Accelerometer X bias (m/s) - i += put_float_by_index(ayBias, i, msg->payload); // Accelerometer Y bias (m/s) - i += put_float_by_index(azBias, i, msg->payload); // Accelerometer Z bias (m/s) - i += put_float_by_index(gxBias, i, msg->payload); // Gyro X bias (rad/s) - i += put_float_by_index(gyBias, i, msg->payload); // Gyro Y bias (rad/s) - i += put_float_by_index(gzBias, i, msg->payload); // Gyro Z bias (rad/s) + p->axBias = axBias; // float:Accelerometer X bias (m/s) + p->ayBias = ayBias; // float:Accelerometer Y bias (m/s) + p->azBias = azBias; // float:Accelerometer Z bias (m/s) + p->gxBias = gxBias; // float:Gyro X bias (rad/s) + p->gyBias = gyBias; // float:Gyro Y bias (rad/s) + p->gzBias = gzBias; // float:Gyro Z bias (rad/s) - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SENSOR_BIAS_LEN); } /** @@ -60,17 +60,17 @@ static inline uint16_t mavlink_msg_sensor_bias_pack(uint8_t system_id, uint8_t c */ static inline uint16_t mavlink_msg_sensor_bias_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, float axBias, float ayBias, float azBias, float gxBias, float gyBias, float gzBias) { - uint16_t i = 0; + mavlink_sensor_bias_t *p = (mavlink_sensor_bias_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SENSOR_BIAS; - i += put_float_by_index(axBias, i, msg->payload); // Accelerometer X bias (m/s) - i += put_float_by_index(ayBias, i, msg->payload); // Accelerometer Y bias (m/s) - i += put_float_by_index(azBias, i, msg->payload); // Accelerometer Z bias (m/s) - i += put_float_by_index(gxBias, i, msg->payload); // Gyro X bias (rad/s) - i += put_float_by_index(gyBias, i, msg->payload); // Gyro Y bias (rad/s) - i += put_float_by_index(gzBias, i, msg->payload); // Gyro Z bias (rad/s) + p->axBias = axBias; // float:Accelerometer X bias (m/s) + p->ayBias = ayBias; // float:Accelerometer Y bias (m/s) + p->azBias = azBias; // float:Accelerometer Z bias (m/s) + p->gxBias = gxBias; // float:Gyro X bias (rad/s) + p->gyBias = gyBias; // float:Gyro Y bias (rad/s) + p->gzBias = gzBias; // float:Gyro Z bias (rad/s) - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SENSOR_BIAS_LEN); } /** @@ -97,13 +97,40 @@ static inline uint16_t mavlink_msg_sensor_bias_encode(uint8_t system_id, uint8_t * @param gyBias Gyro Y bias (rad/s) * @param gzBias Gyro Z bias (rad/s) */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_sensor_bias_send(mavlink_channel_t chan, float axBias, float ayBias, float azBias, float gxBias, float gyBias, float gzBias) { - mavlink_message_t msg; - mavlink_msg_sensor_bias_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, axBias, ayBias, azBias, gxBias, gyBias, gzBias); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_sensor_bias_t payload; + uint16_t checksum; + mavlink_sensor_bias_t *p = &payload; + + p->axBias = axBias; // float:Accelerometer X bias (m/s) + p->ayBias = ayBias; // float:Accelerometer Y bias (m/s) + p->azBias = azBias; // float:Accelerometer Z bias (m/s) + p->gxBias = gxBias; // float:Gyro X bias (rad/s) + p->gyBias = gyBias; // float:Gyro Y bias (rad/s) + p->gzBias = gzBias; // float:Gyro Z bias (rad/s) + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_SENSOR_BIAS_LEN; + hdr.msgid = MAVLINK_MSG_ID_SENSOR_BIAS; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -116,12 +143,8 @@ static inline void mavlink_msg_sensor_bias_send(mavlink_channel_t chan, float ax */ static inline float mavlink_msg_sensor_bias_get_axBias(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 (float)r.f; + mavlink_sensor_bias_t *p = (mavlink_sensor_bias_t *)&msg->payload[0]; + return (float)(p->axBias); } /** @@ -131,12 +154,8 @@ static inline float mavlink_msg_sensor_bias_get_axBias(const mavlink_message_t* */ static inline float mavlink_msg_sensor_bias_get_ayBias(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float))[3]; - return (float)r.f; + mavlink_sensor_bias_t *p = (mavlink_sensor_bias_t *)&msg->payload[0]; + return (float)(p->ayBias); } /** @@ -146,12 +165,8 @@ static inline float mavlink_msg_sensor_bias_get_ayBias(const mavlink_message_t* */ static inline float mavlink_msg_sensor_bias_get_azBias(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_sensor_bias_t *p = (mavlink_sensor_bias_t *)&msg->payload[0]; + return (float)(p->azBias); } /** @@ -161,12 +176,8 @@ static inline float mavlink_msg_sensor_bias_get_azBias(const mavlink_message_t* */ static inline float mavlink_msg_sensor_bias_get_gxBias(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_sensor_bias_t *p = (mavlink_sensor_bias_t *)&msg->payload[0]; + return (float)(p->gxBias); } /** @@ -176,12 +187,8 @@ static inline float mavlink_msg_sensor_bias_get_gxBias(const mavlink_message_t* */ static inline float mavlink_msg_sensor_bias_get_gyBias(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_sensor_bias_t *p = (mavlink_sensor_bias_t *)&msg->payload[0]; + return (float)(p->gyBias); } /** @@ -191,12 +198,8 @@ static inline float mavlink_msg_sensor_bias_get_gyBias(const mavlink_message_t* */ static inline float mavlink_msg_sensor_bias_get_gzBias(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_sensor_bias_t *p = (mavlink_sensor_bias_t *)&msg->payload[0]; + return (float)(p->gzBias); } /** @@ -207,10 +210,5 @@ static inline float mavlink_msg_sensor_bias_get_gzBias(const mavlink_message_t* */ static inline void mavlink_msg_sensor_bias_decode(const mavlink_message_t* msg, mavlink_sensor_bias_t* sensor_bias) { - sensor_bias->axBias = mavlink_msg_sensor_bias_get_axBias(msg); - sensor_bias->ayBias = mavlink_msg_sensor_bias_get_ayBias(msg); - sensor_bias->azBias = mavlink_msg_sensor_bias_get_azBias(msg); - sensor_bias->gxBias = mavlink_msg_sensor_bias_get_gxBias(msg); - sensor_bias->gyBias = mavlink_msg_sensor_bias_get_gyBias(msg); - sensor_bias->gzBias = mavlink_msg_sensor_bias_get_gzBias(msg); + memcpy( sensor_bias, msg->payload, sizeof(mavlink_sensor_bias_t)); } diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_action.h b/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_action.h index 0664166567d84db5b432fcb1e852bb3207ba47c3..fe3399e2ecb835fb581681b901167ef52f8f8858 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_action.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_action.h @@ -1,17 +1,17 @@ // MESSAGE SLUGS_ACTION PACKING #define MAVLINK_MSG_ID_SLUGS_ACTION 183 +#define MAVLINK_MSG_ID_SLUGS_ACTION_LEN 4 +#define MAVLINK_MSG_183_LEN 4 typedef struct __mavlink_slugs_action_t { + uint16_t actionVal; ///< Value associated with the action uint8_t target; ///< The system reporting the action uint8_t actionId; ///< Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names - uint16_t actionVal; ///< Value associated with the action } mavlink_slugs_action_t; - - /** * @brief Pack a slugs_action message * @param system_id ID of this system @@ -25,14 +25,14 @@ typedef struct __mavlink_slugs_action_t */ static inline uint16_t mavlink_msg_slugs_action_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target, uint8_t actionId, uint16_t actionVal) { - uint16_t i = 0; + mavlink_slugs_action_t *p = (mavlink_slugs_action_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SLUGS_ACTION; - i += put_uint8_t_by_index(target, i, msg->payload); // The system reporting the action - i += put_uint8_t_by_index(actionId, i, msg->payload); // Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names - i += put_uint16_t_by_index(actionVal, i, msg->payload); // Value associated with the action + p->target = target; // uint8_t:The system reporting the action + p->actionId = actionId; // uint8_t:Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names + p->actionVal = actionVal; // uint16_t:Value associated with the action - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SLUGS_ACTION_LEN); } /** @@ -48,14 +48,14 @@ static inline uint16_t mavlink_msg_slugs_action_pack(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_slugs_action_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target, uint8_t actionId, uint16_t actionVal) { - uint16_t i = 0; + mavlink_slugs_action_t *p = (mavlink_slugs_action_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SLUGS_ACTION; - i += put_uint8_t_by_index(target, i, msg->payload); // The system reporting the action - i += put_uint8_t_by_index(actionId, i, msg->payload); // Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names - i += put_uint16_t_by_index(actionVal, i, msg->payload); // Value associated with the action + p->target = target; // uint8_t:The system reporting the action + p->actionId = actionId; // uint8_t:Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names + p->actionVal = actionVal; // uint16_t:Value associated with the action - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SLUGS_ACTION_LEN); } /** @@ -79,13 +79,37 @@ static inline uint16_t mavlink_msg_slugs_action_encode(uint8_t system_id, uint8_ * @param actionId Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names * @param actionVal Value associated with the action */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_slugs_action_send(mavlink_channel_t chan, uint8_t target, uint8_t actionId, uint16_t actionVal) { - mavlink_message_t msg; - mavlink_msg_slugs_action_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target, actionId, actionVal); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_slugs_action_t payload; + uint16_t checksum; + mavlink_slugs_action_t *p = &payload; + + p->target = target; // uint8_t:The system reporting the action + p->actionId = actionId; // uint8_t:Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names + p->actionVal = actionVal; // uint16_t:Value associated with the action + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_SLUGS_ACTION_LEN; + hdr.msgid = MAVLINK_MSG_ID_SLUGS_ACTION; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -98,7 +122,8 @@ static inline void mavlink_msg_slugs_action_send(mavlink_channel_t chan, uint8_t */ static inline uint8_t mavlink_msg_slugs_action_get_target(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_slugs_action_t *p = (mavlink_slugs_action_t *)&msg->payload[0]; + return (uint8_t)(p->target); } /** @@ -108,7 +133,8 @@ static inline uint8_t mavlink_msg_slugs_action_get_target(const mavlink_message_ */ static inline uint8_t mavlink_msg_slugs_action_get_actionId(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_slugs_action_t *p = (mavlink_slugs_action_t *)&msg->payload[0]; + return (uint8_t)(p->actionId); } /** @@ -118,10 +144,8 @@ static inline uint8_t mavlink_msg_slugs_action_get_actionId(const mavlink_messag */ static inline uint16_t mavlink_msg_slugs_action_get_actionVal(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; + mavlink_slugs_action_t *p = (mavlink_slugs_action_t *)&msg->payload[0]; + return (uint16_t)(p->actionVal); } /** @@ -132,7 +156,5 @@ static inline uint16_t mavlink_msg_slugs_action_get_actionVal(const mavlink_mess */ static inline void mavlink_msg_slugs_action_decode(const mavlink_message_t* msg, mavlink_slugs_action_t* slugs_action) { - slugs_action->target = mavlink_msg_slugs_action_get_target(msg); - slugs_action->actionId = mavlink_msg_slugs_action_get_actionId(msg); - slugs_action->actionVal = mavlink_msg_slugs_action_get_actionVal(msg); + memcpy( slugs_action, msg->payload, sizeof(mavlink_slugs_action_t)); } diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_navigation.h b/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_navigation.h index f1a54cb3e4408d52ec2531e22429cb567bc95a7c..dc69df79e88854f90519d88b6b00397c67236428 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_navigation.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_navigation.h @@ -1,6 +1,8 @@ // MESSAGE SLUGS_NAVIGATION PACKING #define MAVLINK_MSG_ID_SLUGS_NAVIGATION 176 +#define MAVLINK_MSG_ID_SLUGS_NAVIGATION_LEN 30 +#define MAVLINK_MSG_176_LEN 30 typedef struct __mavlink_slugs_navigation_t { @@ -16,8 +18,6 @@ typedef struct __mavlink_slugs_navigation_t } mavlink_slugs_navigation_t; - - /** * @brief Pack a slugs_navigation message * @param system_id ID of this system @@ -37,20 +37,20 @@ typedef struct __mavlink_slugs_navigation_t */ static inline uint16_t mavlink_msg_slugs_navigation_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, float u_m, float phi_c, float theta_c, float psiDot_c, float ay_body, float totalDist, float dist2Go, uint8_t fromWP, uint8_t toWP) { - uint16_t i = 0; + mavlink_slugs_navigation_t *p = (mavlink_slugs_navigation_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SLUGS_NAVIGATION; - i += put_float_by_index(u_m, i, msg->payload); // Measured Airspeed prior to the Nav Filter - i += put_float_by_index(phi_c, i, msg->payload); // Commanded Roll - i += put_float_by_index(theta_c, i, msg->payload); // Commanded Pitch - i += put_float_by_index(psiDot_c, i, msg->payload); // Commanded Turn rate - i += put_float_by_index(ay_body, i, msg->payload); // Y component of the body acceleration - i += put_float_by_index(totalDist, i, msg->payload); // Total Distance to Run on this leg of Navigation - i += put_float_by_index(dist2Go, i, msg->payload); // Remaining distance to Run on this leg of Navigation - i += put_uint8_t_by_index(fromWP, i, msg->payload); // Origin WP - i += put_uint8_t_by_index(toWP, i, msg->payload); // Destination WP + p->u_m = u_m; // float:Measured Airspeed prior to the Nav Filter + p->phi_c = phi_c; // float:Commanded Roll + p->theta_c = theta_c; // float:Commanded Pitch + p->psiDot_c = psiDot_c; // float:Commanded Turn rate + p->ay_body = ay_body; // float:Y component of the body acceleration + p->totalDist = totalDist; // float:Total Distance to Run on this leg of Navigation + p->dist2Go = dist2Go; // float:Remaining distance to Run on this leg of Navigation + p->fromWP = fromWP; // uint8_t:Origin WP + p->toWP = toWP; // uint8_t:Destination WP - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SLUGS_NAVIGATION_LEN); } /** @@ -72,20 +72,20 @@ static inline uint16_t mavlink_msg_slugs_navigation_pack(uint8_t system_id, uint */ static inline uint16_t mavlink_msg_slugs_navigation_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, float u_m, float phi_c, float theta_c, float psiDot_c, float ay_body, float totalDist, float dist2Go, uint8_t fromWP, uint8_t toWP) { - uint16_t i = 0; + mavlink_slugs_navigation_t *p = (mavlink_slugs_navigation_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SLUGS_NAVIGATION; - i += put_float_by_index(u_m, i, msg->payload); // Measured Airspeed prior to the Nav Filter - i += put_float_by_index(phi_c, i, msg->payload); // Commanded Roll - i += put_float_by_index(theta_c, i, msg->payload); // Commanded Pitch - i += put_float_by_index(psiDot_c, i, msg->payload); // Commanded Turn rate - i += put_float_by_index(ay_body, i, msg->payload); // Y component of the body acceleration - i += put_float_by_index(totalDist, i, msg->payload); // Total Distance to Run on this leg of Navigation - i += put_float_by_index(dist2Go, i, msg->payload); // Remaining distance to Run on this leg of Navigation - i += put_uint8_t_by_index(fromWP, i, msg->payload); // Origin WP - i += put_uint8_t_by_index(toWP, i, msg->payload); // Destination WP + p->u_m = u_m; // float:Measured Airspeed prior to the Nav Filter + p->phi_c = phi_c; // float:Commanded Roll + p->theta_c = theta_c; // float:Commanded Pitch + p->psiDot_c = psiDot_c; // float:Commanded Turn rate + p->ay_body = ay_body; // float:Y component of the body acceleration + p->totalDist = totalDist; // float:Total Distance to Run on this leg of Navigation + p->dist2Go = dist2Go; // float:Remaining distance to Run on this leg of Navigation + p->fromWP = fromWP; // uint8_t:Origin WP + p->toWP = toWP; // uint8_t:Destination WP - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SLUGS_NAVIGATION_LEN); } /** @@ -115,13 +115,43 @@ static inline uint16_t mavlink_msg_slugs_navigation_encode(uint8_t system_id, ui * @param fromWP Origin WP * @param toWP Destination WP */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_slugs_navigation_send(mavlink_channel_t chan, float u_m, float phi_c, float theta_c, float psiDot_c, float ay_body, float totalDist, float dist2Go, uint8_t fromWP, uint8_t toWP) { - mavlink_message_t msg; - mavlink_msg_slugs_navigation_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, u_m, phi_c, theta_c, psiDot_c, ay_body, totalDist, dist2Go, fromWP, toWP); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_slugs_navigation_t payload; + uint16_t checksum; + mavlink_slugs_navigation_t *p = &payload; + + p->u_m = u_m; // float:Measured Airspeed prior to the Nav Filter + p->phi_c = phi_c; // float:Commanded Roll + p->theta_c = theta_c; // float:Commanded Pitch + p->psiDot_c = psiDot_c; // float:Commanded Turn rate + p->ay_body = ay_body; // float:Y component of the body acceleration + p->totalDist = totalDist; // float:Total Distance to Run on this leg of Navigation + p->dist2Go = dist2Go; // float:Remaining distance to Run on this leg of Navigation + p->fromWP = fromWP; // uint8_t:Origin WP + p->toWP = toWP; // uint8_t:Destination WP + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_SLUGS_NAVIGATION_LEN; + hdr.msgid = MAVLINK_MSG_ID_SLUGS_NAVIGATION; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -134,12 +164,8 @@ static inline void mavlink_msg_slugs_navigation_send(mavlink_channel_t chan, flo */ static inline float mavlink_msg_slugs_navigation_get_u_m(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 (float)r.f; + mavlink_slugs_navigation_t *p = (mavlink_slugs_navigation_t *)&msg->payload[0]; + return (float)(p->u_m); } /** @@ -149,12 +175,8 @@ static inline float mavlink_msg_slugs_navigation_get_u_m(const mavlink_message_t */ static inline float mavlink_msg_slugs_navigation_get_phi_c(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float))[3]; - return (float)r.f; + mavlink_slugs_navigation_t *p = (mavlink_slugs_navigation_t *)&msg->payload[0]; + return (float)(p->phi_c); } /** @@ -164,12 +186,8 @@ static inline float mavlink_msg_slugs_navigation_get_phi_c(const mavlink_message */ static inline float mavlink_msg_slugs_navigation_get_theta_c(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_slugs_navigation_t *p = (mavlink_slugs_navigation_t *)&msg->payload[0]; + return (float)(p->theta_c); } /** @@ -179,12 +197,8 @@ static inline float mavlink_msg_slugs_navigation_get_theta_c(const mavlink_messa */ static inline float mavlink_msg_slugs_navigation_get_psiDot_c(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_slugs_navigation_t *p = (mavlink_slugs_navigation_t *)&msg->payload[0]; + return (float)(p->psiDot_c); } /** @@ -194,12 +208,8 @@ static inline float mavlink_msg_slugs_navigation_get_psiDot_c(const mavlink_mess */ static inline float mavlink_msg_slugs_navigation_get_ay_body(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_slugs_navigation_t *p = (mavlink_slugs_navigation_t *)&msg->payload[0]; + return (float)(p->ay_body); } /** @@ -209,12 +219,8 @@ static inline float mavlink_msg_slugs_navigation_get_ay_body(const mavlink_messa */ static inline float mavlink_msg_slugs_navigation_get_totalDist(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_slugs_navigation_t *p = (mavlink_slugs_navigation_t *)&msg->payload[0]; + return (float)(p->totalDist); } /** @@ -224,12 +230,8 @@ static inline float mavlink_msg_slugs_navigation_get_totalDist(const mavlink_mes */ static inline float mavlink_msg_slugs_navigation_get_dist2Go(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_slugs_navigation_t *p = (mavlink_slugs_navigation_t *)&msg->payload[0]; + return (float)(p->dist2Go); } /** @@ -239,7 +241,8 @@ static inline float mavlink_msg_slugs_navigation_get_dist2Go(const mavlink_messa */ static inline uint8_t mavlink_msg_slugs_navigation_get_fromWP(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; + mavlink_slugs_navigation_t *p = (mavlink_slugs_navigation_t *)&msg->payload[0]; + return (uint8_t)(p->fromWP); } /** @@ -249,7 +252,8 @@ static inline uint8_t mavlink_msg_slugs_navigation_get_fromWP(const mavlink_mess */ static inline uint8_t mavlink_msg_slugs_navigation_get_toWP(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t))[0]; + mavlink_slugs_navigation_t *p = (mavlink_slugs_navigation_t *)&msg->payload[0]; + return (uint8_t)(p->toWP); } /** @@ -260,13 +264,5 @@ static inline uint8_t mavlink_msg_slugs_navigation_get_toWP(const mavlink_messag */ static inline void mavlink_msg_slugs_navigation_decode(const mavlink_message_t* msg, mavlink_slugs_navigation_t* slugs_navigation) { - slugs_navigation->u_m = mavlink_msg_slugs_navigation_get_u_m(msg); - slugs_navigation->phi_c = mavlink_msg_slugs_navigation_get_phi_c(msg); - slugs_navigation->theta_c = mavlink_msg_slugs_navigation_get_theta_c(msg); - slugs_navigation->psiDot_c = mavlink_msg_slugs_navigation_get_psiDot_c(msg); - slugs_navigation->ay_body = mavlink_msg_slugs_navigation_get_ay_body(msg); - slugs_navigation->totalDist = mavlink_msg_slugs_navigation_get_totalDist(msg); - slugs_navigation->dist2Go = mavlink_msg_slugs_navigation_get_dist2Go(msg); - slugs_navigation->fromWP = mavlink_msg_slugs_navigation_get_fromWP(msg); - slugs_navigation->toWP = mavlink_msg_slugs_navigation_get_toWP(msg); + memcpy( slugs_navigation, msg->payload, sizeof(mavlink_slugs_navigation_t)); } diff --git a/thirdParty/mavlink/include/slugs/slugs.h b/thirdParty/mavlink/include/slugs/slugs.h index b8a6ff330ac6767501f59b0c042cd7b9966b3ab3..990857c7294df2417646ca34324fbc874843999d 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 Tuesday, August 9 2011, 16:16 UTC */ #ifndef SLUGS_H #define SLUGS_H @@ -45,10 +45,10 @@ extern "C" { #include "./mavlink_msg_slugs_action.h" -// MESSAGE LENGTHS +// MESSAGE CRC KEYS -#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 } +#undef MAVLINK_MESSAGE_KEYS +#define MAVLINK_MESSAGE_KEYS { 71, 249, 232, 226, 76, 126, 117, 186, 0, 165, 44, 249, 133, 0, 0, 0, 0, 0, 0, 0, 112, 34, 81, 152, 0, 223, 28, 99, 28, 21, 243, 240, 204, 21, 111, 43, 192, 234, 22, 197, 192, 192, 166, 34, 233, 34, 166, 158, 142, 60, 10, 75, 249, 247, 234, 157, 125, 0, 0, 0, 85, 211, 62, 75, 185, 18, 42, 38, 0, 127, 200, 0, 0, 53, 251, 20, 22, 0, 0, 0, 0, 0, 0, 0, 0, 127, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 8, 238, 165, 0, 0, 0, 0, 0, 0, 0, 218, 218, 235, 0, 0, 0, 0, 0, 0, 225, 114, 0, 0, 0, 0, 0, 0, 0, 0, 221, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 171, 122, 0, 0, 0, 0, 0, 0, 0, 92, 114, 4, 169, 10, 0, 0, 0, 0, 0, 72, 62, 83, 0, 0, 0, 0, 0, 0, 0, 202, 144, 106, 254, 0, 0, 255, 185, 0, 14, 136, 53, 0, 212, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 43, 141, 211, 166 } #ifdef __cplusplus } diff --git a/thirdParty/mavlink/include/ualberta/mavlink.h b/thirdParty/mavlink/include/ualberta/mavlink.h index 30b060f630050f4b68a710e97ca0c3eed2ca0e9b..0bb9f2f90603b286a1d2f17a2c67aee0d313c0ae 100644 --- a/thirdParty/mavlink/include/ualberta/mavlink.h +++ b/thirdParty/mavlink/include/ualberta/mavlink.h @@ -1,11 +1,16 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Wednesday, July 27 2011, 14:17 UTC + * Generated on Tuesday, August 9 2011, 16:16 UTC */ #ifndef MAVLINK_H #define MAVLINK_H +#pragma pack(push,1) +#include "mavlink_options.h" #include "ualberta.h" - +#ifdef MAVLINK_DATA +#include "mavlink_data.h" +#endif +#pragma pack(pop) #endif diff --git a/thirdParty/mavlink/include/ualberta/mavlink_msg_nav_filter_bias.h b/thirdParty/mavlink/include/ualberta/mavlink_msg_nav_filter_bias.h index a011fc1ab4f6f7b8587f8482cef6d0f3599c80a1..d1b777ee46823ec2a3e1a459d33d9376d534c18e 100644 --- a/thirdParty/mavlink/include/ualberta/mavlink_msg_nav_filter_bias.h +++ b/thirdParty/mavlink/include/ualberta/mavlink_msg_nav_filter_bias.h @@ -1,6 +1,8 @@ // MESSAGE NAV_FILTER_BIAS PACKING #define MAVLINK_MSG_ID_NAV_FILTER_BIAS 220 +#define MAVLINK_MSG_ID_NAV_FILTER_BIAS_LEN 32 +#define MAVLINK_MSG_220_LEN 32 typedef struct __mavlink_nav_filter_bias_t { @@ -14,8 +16,6 @@ typedef struct __mavlink_nav_filter_bias_t } mavlink_nav_filter_bias_t; - - /** * @brief Pack a nav_filter_bias message * @param system_id ID of this system @@ -33,18 +33,18 @@ typedef struct __mavlink_nav_filter_bias_t */ static inline uint16_t mavlink_msg_nav_filter_bias_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t usec, float accel_0, float accel_1, float accel_2, float gyro_0, float gyro_1, float gyro_2) { - uint16_t i = 0; + mavlink_nav_filter_bias_t *p = (mavlink_nav_filter_bias_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_NAV_FILTER_BIAS; - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds) - i += put_float_by_index(accel_0, i, msg->payload); // b_f[0] - i += put_float_by_index(accel_1, i, msg->payload); // b_f[1] - i += put_float_by_index(accel_2, i, msg->payload); // b_f[2] - i += put_float_by_index(gyro_0, i, msg->payload); // b_f[0] - i += put_float_by_index(gyro_1, i, msg->payload); // b_f[1] - i += put_float_by_index(gyro_2, i, msg->payload); // b_f[2] + p->usec = usec; // uint64_t:Timestamp (microseconds) + p->accel_0 = accel_0; // float:b_f[0] + p->accel_1 = accel_1; // float:b_f[1] + p->accel_2 = accel_2; // float:b_f[2] + p->gyro_0 = gyro_0; // float:b_f[0] + p->gyro_1 = gyro_1; // float:b_f[1] + p->gyro_2 = gyro_2; // float:b_f[2] - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_NAV_FILTER_BIAS_LEN); } /** @@ -64,18 +64,18 @@ static inline uint16_t mavlink_msg_nav_filter_bias_pack(uint8_t system_id, uint8 */ static inline uint16_t mavlink_msg_nav_filter_bias_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t usec, float accel_0, float accel_1, float accel_2, float gyro_0, float gyro_1, float gyro_2) { - uint16_t i = 0; + mavlink_nav_filter_bias_t *p = (mavlink_nav_filter_bias_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_NAV_FILTER_BIAS; - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds) - i += put_float_by_index(accel_0, i, msg->payload); // b_f[0] - i += put_float_by_index(accel_1, i, msg->payload); // b_f[1] - i += put_float_by_index(accel_2, i, msg->payload); // b_f[2] - i += put_float_by_index(gyro_0, i, msg->payload); // b_f[0] - i += put_float_by_index(gyro_1, i, msg->payload); // b_f[1] - i += put_float_by_index(gyro_2, i, msg->payload); // b_f[2] + p->usec = usec; // uint64_t:Timestamp (microseconds) + p->accel_0 = accel_0; // float:b_f[0] + p->accel_1 = accel_1; // float:b_f[1] + p->accel_2 = accel_2; // float:b_f[2] + p->gyro_0 = gyro_0; // float:b_f[0] + p->gyro_1 = gyro_1; // float:b_f[1] + p->gyro_2 = gyro_2; // float:b_f[2] - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_NAV_FILTER_BIAS_LEN); } /** @@ -103,13 +103,41 @@ static inline uint16_t mavlink_msg_nav_filter_bias_encode(uint8_t system_id, uin * @param gyro_1 b_f[1] * @param gyro_2 b_f[2] */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_nav_filter_bias_send(mavlink_channel_t chan, uint64_t usec, float accel_0, float accel_1, float accel_2, float gyro_0, float gyro_1, float gyro_2) { - mavlink_message_t msg; - mavlink_msg_nav_filter_bias_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, usec, accel_0, accel_1, accel_2, gyro_0, gyro_1, gyro_2); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_nav_filter_bias_t payload; + uint16_t checksum; + mavlink_nav_filter_bias_t *p = &payload; + + p->usec = usec; // uint64_t:Timestamp (microseconds) + p->accel_0 = accel_0; // float:b_f[0] + p->accel_1 = accel_1; // float:b_f[1] + p->accel_2 = accel_2; // float:b_f[2] + p->gyro_0 = gyro_0; // float:b_f[0] + p->gyro_1 = gyro_1; // float:b_f[1] + p->gyro_2 = gyro_2; // float:b_f[2] + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_NAV_FILTER_BIAS_LEN; + hdr.msgid = MAVLINK_MSG_ID_NAV_FILTER_BIAS; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -122,16 +150,8 @@ static inline void mavlink_msg_nav_filter_bias_send(mavlink_channel_t chan, uint */ static inline uint64_t mavlink_msg_nav_filter_bias_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; + mavlink_nav_filter_bias_t *p = (mavlink_nav_filter_bias_t *)&msg->payload[0]; + return (uint64_t)(p->usec); } /** @@ -141,12 +161,8 @@ static inline uint64_t mavlink_msg_nav_filter_bias_get_usec(const mavlink_messag */ static inline float mavlink_msg_nav_filter_bias_get_accel_0(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; + mavlink_nav_filter_bias_t *p = (mavlink_nav_filter_bias_t *)&msg->payload[0]; + return (float)(p->accel_0); } /** @@ -156,12 +172,8 @@ static inline float mavlink_msg_nav_filter_bias_get_accel_0(const mavlink_messag */ static inline float mavlink_msg_nav_filter_bias_get_accel_1(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; + mavlink_nav_filter_bias_t *p = (mavlink_nav_filter_bias_t *)&msg->payload[0]; + return (float)(p->accel_1); } /** @@ -171,12 +183,8 @@ static inline float mavlink_msg_nav_filter_bias_get_accel_1(const mavlink_messag */ static inline float mavlink_msg_nav_filter_bias_get_accel_2(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; + mavlink_nav_filter_bias_t *p = (mavlink_nav_filter_bias_t *)&msg->payload[0]; + return (float)(p->accel_2); } /** @@ -186,12 +194,8 @@ static inline float mavlink_msg_nav_filter_bias_get_accel_2(const mavlink_messag */ static inline float mavlink_msg_nav_filter_bias_get_gyro_0(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; + mavlink_nav_filter_bias_t *p = (mavlink_nav_filter_bias_t *)&msg->payload[0]; + return (float)(p->gyro_0); } /** @@ -201,12 +205,8 @@ static inline float mavlink_msg_nav_filter_bias_get_gyro_0(const mavlink_message */ static inline float mavlink_msg_nav_filter_bias_get_gyro_1(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; + mavlink_nav_filter_bias_t *p = (mavlink_nav_filter_bias_t *)&msg->payload[0]; + return (float)(p->gyro_1); } /** @@ -216,12 +216,8 @@ static inline float mavlink_msg_nav_filter_bias_get_gyro_1(const mavlink_message */ static inline float mavlink_msg_nav_filter_bias_get_gyro_2(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; + mavlink_nav_filter_bias_t *p = (mavlink_nav_filter_bias_t *)&msg->payload[0]; + return (float)(p->gyro_2); } /** @@ -232,11 +228,5 @@ static inline float mavlink_msg_nav_filter_bias_get_gyro_2(const mavlink_message */ static inline void mavlink_msg_nav_filter_bias_decode(const mavlink_message_t* msg, mavlink_nav_filter_bias_t* nav_filter_bias) { - nav_filter_bias->usec = mavlink_msg_nav_filter_bias_get_usec(msg); - nav_filter_bias->accel_0 = mavlink_msg_nav_filter_bias_get_accel_0(msg); - nav_filter_bias->accel_1 = mavlink_msg_nav_filter_bias_get_accel_1(msg); - nav_filter_bias->accel_2 = mavlink_msg_nav_filter_bias_get_accel_2(msg); - nav_filter_bias->gyro_0 = mavlink_msg_nav_filter_bias_get_gyro_0(msg); - nav_filter_bias->gyro_1 = mavlink_msg_nav_filter_bias_get_gyro_1(msg); - nav_filter_bias->gyro_2 = mavlink_msg_nav_filter_bias_get_gyro_2(msg); + memcpy( nav_filter_bias, msg->payload, sizeof(mavlink_nav_filter_bias_t)); } diff --git a/thirdParty/mavlink/include/ualberta/mavlink_msg_radio_calibration.h b/thirdParty/mavlink/include/ualberta/mavlink_msg_radio_calibration.h index 5907aba95bce713210549cfd6e452da1346c20fc..22b139afc655125e254c789a02652e3aa4fa8bb6 100644 --- a/thirdParty/mavlink/include/ualberta/mavlink_msg_radio_calibration.h +++ b/thirdParty/mavlink/include/ualberta/mavlink_msg_radio_calibration.h @@ -1,6 +1,8 @@ // MESSAGE RADIO_CALIBRATION PACKING #define MAVLINK_MSG_ID_RADIO_CALIBRATION 221 +#define MAVLINK_MSG_ID_RADIO_CALIBRATION_LEN 42 +#define MAVLINK_MSG_221_LEN 42 typedef struct __mavlink_radio_calibration_t { @@ -12,7 +14,6 @@ typedef struct __mavlink_radio_calibration_t uint16_t throttle[5]; ///< Throttle curve setpoints (every 25%) } mavlink_radio_calibration_t; - #define MAVLINK_MSG_RADIO_CALIBRATION_FIELD_AILERON_LEN 3 #define MAVLINK_MSG_RADIO_CALIBRATION_FIELD_ELEVATOR_LEN 3 #define MAVLINK_MSG_RADIO_CALIBRATION_FIELD_RUDDER_LEN 3 @@ -20,7 +21,6 @@ typedef struct __mavlink_radio_calibration_t #define MAVLINK_MSG_RADIO_CALIBRATION_FIELD_PITCH_LEN 5 #define MAVLINK_MSG_RADIO_CALIBRATION_FIELD_THROTTLE_LEN 5 - /** * @brief Pack a radio_calibration message * @param system_id ID of this system @@ -37,17 +37,17 @@ typedef struct __mavlink_radio_calibration_t */ static inline uint16_t mavlink_msg_radio_calibration_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const uint16_t* aileron, const uint16_t* elevator, const uint16_t* rudder, const uint16_t* gyro, const uint16_t* pitch, const uint16_t* throttle) { - uint16_t i = 0; + mavlink_radio_calibration_t *p = (mavlink_radio_calibration_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_RADIO_CALIBRATION; - i += put_array_by_index((const int8_t*)aileron, sizeof(uint16_t)*3, i, msg->payload); // Aileron setpoints: left, center, right - i += put_array_by_index((const int8_t*)elevator, sizeof(uint16_t)*3, i, msg->payload); // Elevator setpoints: nose down, center, nose up - i += put_array_by_index((const int8_t*)rudder, sizeof(uint16_t)*3, i, msg->payload); // Rudder setpoints: nose left, center, nose right - i += put_array_by_index((const int8_t*)gyro, sizeof(uint16_t)*2, i, msg->payload); // Tail gyro mode/gain setpoints: heading hold, rate mode - i += put_array_by_index((const int8_t*)pitch, sizeof(uint16_t)*5, i, msg->payload); // Pitch curve setpoints (every 25%) - i += put_array_by_index((const int8_t*)throttle, sizeof(uint16_t)*5, i, msg->payload); // Throttle curve setpoints (every 25%) + memcpy(p->aileron, aileron, sizeof(p->aileron)); // uint16_t[3]:Aileron setpoints: left, center, right + memcpy(p->elevator, elevator, sizeof(p->elevator)); // uint16_t[3]:Elevator setpoints: nose down, center, nose up + memcpy(p->rudder, rudder, sizeof(p->rudder)); // uint16_t[3]:Rudder setpoints: nose left, center, nose right + memcpy(p->gyro, gyro, sizeof(p->gyro)); // uint16_t[2]:Tail gyro mode/gain setpoints: heading hold, rate mode + memcpy(p->pitch, pitch, sizeof(p->pitch)); // uint16_t[5]:Pitch curve setpoints (every 25%) + memcpy(p->throttle, throttle, sizeof(p->throttle)); // uint16_t[5]:Throttle curve setpoints (every 25%) - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RADIO_CALIBRATION_LEN); } /** @@ -66,17 +66,17 @@ static inline uint16_t mavlink_msg_radio_calibration_pack(uint8_t system_id, uin */ static inline uint16_t mavlink_msg_radio_calibration_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const uint16_t* aileron, const uint16_t* elevator, const uint16_t* rudder, const uint16_t* gyro, const uint16_t* pitch, const uint16_t* throttle) { - uint16_t i = 0; + mavlink_radio_calibration_t *p = (mavlink_radio_calibration_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_RADIO_CALIBRATION; - i += put_array_by_index((const int8_t*)aileron, sizeof(uint16_t)*3, i, msg->payload); // Aileron setpoints: left, center, right - i += put_array_by_index((const int8_t*)elevator, sizeof(uint16_t)*3, i, msg->payload); // Elevator setpoints: nose down, center, nose up - i += put_array_by_index((const int8_t*)rudder, sizeof(uint16_t)*3, i, msg->payload); // Rudder setpoints: nose left, center, nose right - i += put_array_by_index((const int8_t*)gyro, sizeof(uint16_t)*2, i, msg->payload); // Tail gyro mode/gain setpoints: heading hold, rate mode - i += put_array_by_index((const int8_t*)pitch, sizeof(uint16_t)*5, i, msg->payload); // Pitch curve setpoints (every 25%) - i += put_array_by_index((const int8_t*)throttle, sizeof(uint16_t)*5, i, msg->payload); // Throttle curve setpoints (every 25%) + memcpy(p->aileron, aileron, sizeof(p->aileron)); // uint16_t[3]:Aileron setpoints: left, center, right + memcpy(p->elevator, elevator, sizeof(p->elevator)); // uint16_t[3]:Elevator setpoints: nose down, center, nose up + memcpy(p->rudder, rudder, sizeof(p->rudder)); // uint16_t[3]:Rudder setpoints: nose left, center, nose right + memcpy(p->gyro, gyro, sizeof(p->gyro)); // uint16_t[2]:Tail gyro mode/gain setpoints: heading hold, rate mode + memcpy(p->pitch, pitch, sizeof(p->pitch)); // uint16_t[5]:Pitch curve setpoints (every 25%) + memcpy(p->throttle, throttle, sizeof(p->throttle)); // uint16_t[5]:Throttle curve setpoints (every 25%) - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RADIO_CALIBRATION_LEN); } /** @@ -103,13 +103,40 @@ static inline uint16_t mavlink_msg_radio_calibration_encode(uint8_t system_id, u * @param pitch Pitch curve setpoints (every 25%) * @param throttle Throttle curve setpoints (every 25%) */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_radio_calibration_send(mavlink_channel_t chan, const uint16_t* aileron, const uint16_t* elevator, const uint16_t* rudder, const uint16_t* gyro, const uint16_t* pitch, const uint16_t* throttle) { - mavlink_message_t msg; - mavlink_msg_radio_calibration_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, aileron, elevator, rudder, gyro, pitch, throttle); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_radio_calibration_t payload; + uint16_t checksum; + mavlink_radio_calibration_t *p = &payload; + + memcpy(p->aileron, aileron, sizeof(p->aileron)); // uint16_t[3]:Aileron setpoints: left, center, right + memcpy(p->elevator, elevator, sizeof(p->elevator)); // uint16_t[3]:Elevator setpoints: nose down, center, nose up + memcpy(p->rudder, rudder, sizeof(p->rudder)); // uint16_t[3]:Rudder setpoints: nose left, center, nose right + memcpy(p->gyro, gyro, sizeof(p->gyro)); // uint16_t[2]:Tail gyro mode/gain setpoints: heading hold, rate mode + memcpy(p->pitch, pitch, sizeof(p->pitch)); // uint16_t[5]:Pitch curve setpoints (every 25%) + memcpy(p->throttle, throttle, sizeof(p->throttle)); // uint16_t[5]:Throttle curve setpoints (every 25%) + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_RADIO_CALIBRATION_LEN; + hdr.msgid = MAVLINK_MSG_ID_RADIO_CALIBRATION; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -120,11 +147,12 @@ static inline void mavlink_msg_radio_calibration_send(mavlink_channel_t chan, co * * @return Aileron setpoints: left, center, right */ -static inline uint16_t mavlink_msg_radio_calibration_get_aileron(const mavlink_message_t* msg, uint16_t* r_data) +static inline uint16_t mavlink_msg_radio_calibration_get_aileron(const mavlink_message_t* msg, uint16_t* aileron) { + mavlink_radio_calibration_t *p = (mavlink_radio_calibration_t *)&msg->payload[0]; - memcpy(r_data, msg->payload, sizeof(uint16_t)*3); - return sizeof(uint16_t)*3; + memcpy(aileron, p->aileron, sizeof(p->aileron)); + return sizeof(p->aileron); } /** @@ -132,11 +160,12 @@ static inline uint16_t mavlink_msg_radio_calibration_get_aileron(const mavlink_m * * @return Elevator setpoints: nose down, center, nose up */ -static inline uint16_t mavlink_msg_radio_calibration_get_elevator(const mavlink_message_t* msg, uint16_t* r_data) +static inline uint16_t mavlink_msg_radio_calibration_get_elevator(const mavlink_message_t* msg, uint16_t* elevator) { + mavlink_radio_calibration_t *p = (mavlink_radio_calibration_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint16_t)*3, sizeof(uint16_t)*3); - return sizeof(uint16_t)*3; + memcpy(elevator, p->elevator, sizeof(p->elevator)); + return sizeof(p->elevator); } /** @@ -144,11 +173,12 @@ static inline uint16_t mavlink_msg_radio_calibration_get_elevator(const mavlink_ * * @return Rudder setpoints: nose left, center, nose right */ -static inline uint16_t mavlink_msg_radio_calibration_get_rudder(const mavlink_message_t* msg, uint16_t* r_data) +static inline uint16_t mavlink_msg_radio_calibration_get_rudder(const mavlink_message_t* msg, uint16_t* rudder) { + mavlink_radio_calibration_t *p = (mavlink_radio_calibration_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint16_t)*3+sizeof(uint16_t)*3, sizeof(uint16_t)*3); - return sizeof(uint16_t)*3; + memcpy(rudder, p->rudder, sizeof(p->rudder)); + return sizeof(p->rudder); } /** @@ -156,11 +186,12 @@ static inline uint16_t mavlink_msg_radio_calibration_get_rudder(const mavlink_me * * @return Tail gyro mode/gain setpoints: heading hold, rate mode */ -static inline uint16_t mavlink_msg_radio_calibration_get_gyro(const mavlink_message_t* msg, uint16_t* r_data) +static inline uint16_t mavlink_msg_radio_calibration_get_gyro(const mavlink_message_t* msg, uint16_t* gyro) { + mavlink_radio_calibration_t *p = (mavlink_radio_calibration_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint16_t)*3+sizeof(uint16_t)*3+sizeof(uint16_t)*3, sizeof(uint16_t)*2); - return sizeof(uint16_t)*2; + memcpy(gyro, p->gyro, sizeof(p->gyro)); + return sizeof(p->gyro); } /** @@ -168,11 +199,12 @@ static inline uint16_t mavlink_msg_radio_calibration_get_gyro(const mavlink_mess * * @return Pitch curve setpoints (every 25%) */ -static inline uint16_t mavlink_msg_radio_calibration_get_pitch(const mavlink_message_t* msg, uint16_t* r_data) +static inline uint16_t mavlink_msg_radio_calibration_get_pitch(const mavlink_message_t* msg, uint16_t* pitch) { + mavlink_radio_calibration_t *p = (mavlink_radio_calibration_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint16_t)*3+sizeof(uint16_t)*3+sizeof(uint16_t)*3+sizeof(uint16_t)*2, sizeof(uint16_t)*5); - return sizeof(uint16_t)*5; + memcpy(pitch, p->pitch, sizeof(p->pitch)); + return sizeof(p->pitch); } /** @@ -180,11 +212,12 @@ static inline uint16_t mavlink_msg_radio_calibration_get_pitch(const mavlink_mes * * @return Throttle curve setpoints (every 25%) */ -static inline uint16_t mavlink_msg_radio_calibration_get_throttle(const mavlink_message_t* msg, uint16_t* r_data) +static inline uint16_t mavlink_msg_radio_calibration_get_throttle(const mavlink_message_t* msg, uint16_t* throttle) { + mavlink_radio_calibration_t *p = (mavlink_radio_calibration_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint16_t)*3+sizeof(uint16_t)*3+sizeof(uint16_t)*3+sizeof(uint16_t)*2+sizeof(uint16_t)*5, sizeof(uint16_t)*5); - return sizeof(uint16_t)*5; + memcpy(throttle, p->throttle, sizeof(p->throttle)); + return sizeof(p->throttle); } /** @@ -195,10 +228,5 @@ static inline uint16_t mavlink_msg_radio_calibration_get_throttle(const mavlink_ */ static inline void mavlink_msg_radio_calibration_decode(const mavlink_message_t* msg, mavlink_radio_calibration_t* radio_calibration) { - mavlink_msg_radio_calibration_get_aileron(msg, radio_calibration->aileron); - mavlink_msg_radio_calibration_get_elevator(msg, radio_calibration->elevator); - mavlink_msg_radio_calibration_get_rudder(msg, radio_calibration->rudder); - mavlink_msg_radio_calibration_get_gyro(msg, radio_calibration->gyro); - mavlink_msg_radio_calibration_get_pitch(msg, radio_calibration->pitch); - mavlink_msg_radio_calibration_get_throttle(msg, radio_calibration->throttle); + memcpy( radio_calibration, msg->payload, sizeof(mavlink_radio_calibration_t)); } diff --git a/thirdParty/mavlink/include/ualberta/mavlink_msg_request_rc_channels.h b/thirdParty/mavlink/include/ualberta/mavlink_msg_request_rc_channels.h deleted file mode 100644 index 64202ffe269eb434aa80960924793f92c38e25d2..0000000000000000000000000000000000000000 --- a/thirdParty/mavlink/include/ualberta/mavlink_msg_request_rc_channels.h +++ /dev/null @@ -1,101 +0,0 @@ -// MESSAGE REQUEST_RC_CHANNELS PACKING - -#define MAVLINK_MSG_ID_REQUEST_RC_CHANNELS 221 - -typedef struct __mavlink_request_rc_channels_t -{ - uint8_t enabled; ///< True: start sending data; False: stop sending data - -} mavlink_request_rc_channels_t; - - - -/** - * @brief Pack a request_rc_channels 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 True: start sending data; False: stop sending data - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_request_rc_channels_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t enabled) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_REQUEST_RC_CHANNELS; - - i += put_uint8_t_by_index(enabled, i, msg->payload); // True: start sending data; False: stop sending data - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -/** - * @brief Pack a request_rc_channels 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 True: start sending data; False: stop sending data - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_request_rc_channels_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t enabled) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_REQUEST_RC_CHANNELS; - - i += put_uint8_t_by_index(enabled, i, msg->payload); // True: start sending data; False: stop sending data - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); -} - -/** - * @brief Encode a request_rc_channels 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 request_rc_channels C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_request_rc_channels_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_request_rc_channels_t* request_rc_channels) -{ - return mavlink_msg_request_rc_channels_pack(system_id, component_id, msg, request_rc_channels->enabled); -} - -/** - * @brief Send a request_rc_channels message - * @param chan MAVLink channel to send the message - * - * @param enabled True: start sending data; False: stop sending data - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_request_rc_channels_send(mavlink_channel_t chan, uint8_t enabled) -{ - mavlink_message_t msg; - mavlink_msg_request_rc_channels_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, enabled); - mavlink_send_uart(chan, &msg); -} - -#endif -// MESSAGE REQUEST_RC_CHANNELS UNPACKING - -/** - * @brief Get field enabled from request_rc_channels message - * - * @return True: start sending data; False: stop sending data - */ -static inline uint8_t mavlink_msg_request_rc_channels_get_enabled(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload)[0]; -} - -/** - * @brief Decode a request_rc_channels message into a struct - * - * @param msg The message to decode - * @param request_rc_channels C-struct to decode the message contents into - */ -static inline void mavlink_msg_request_rc_channels_decode(const mavlink_message_t* msg, mavlink_request_rc_channels_t* request_rc_channels) -{ - request_rc_channels->enabled = mavlink_msg_request_rc_channels_get_enabled(msg); -} diff --git a/thirdParty/mavlink/include/ualberta/mavlink_msg_ualberta_sys_status.h b/thirdParty/mavlink/include/ualberta/mavlink_msg_ualberta_sys_status.h index 50e8f7d02ca58dccec28614af2fed7d4fe0c1466..16e2ff6853ca2559b1d0c99e013f38df910347e3 100644 --- a/thirdParty/mavlink/include/ualberta/mavlink_msg_ualberta_sys_status.h +++ b/thirdParty/mavlink/include/ualberta/mavlink_msg_ualberta_sys_status.h @@ -1,6 +1,8 @@ // MESSAGE UALBERTA_SYS_STATUS PACKING #define MAVLINK_MSG_ID_UALBERTA_SYS_STATUS 222 +#define MAVLINK_MSG_ID_UALBERTA_SYS_STATUS_LEN 3 +#define MAVLINK_MSG_222_LEN 3 typedef struct __mavlink_ualberta_sys_status_t { @@ -10,8 +12,6 @@ typedef struct __mavlink_ualberta_sys_status_t } mavlink_ualberta_sys_status_t; - - /** * @brief Pack a ualberta_sys_status message * @param system_id ID of this system @@ -25,14 +25,14 @@ typedef struct __mavlink_ualberta_sys_status_t */ static inline uint16_t mavlink_msg_ualberta_sys_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t mode, uint8_t nav_mode, uint8_t pilot) { - uint16_t i = 0; + mavlink_ualberta_sys_status_t *p = (mavlink_ualberta_sys_status_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_UALBERTA_SYS_STATUS; - i += put_uint8_t_by_index(mode, i, msg->payload); // System mode, see UALBERTA_AUTOPILOT_MODE ENUM - i += put_uint8_t_by_index(nav_mode, i, msg->payload); // Navigation mode, see UALBERTA_NAV_MODE ENUM - i += put_uint8_t_by_index(pilot, i, msg->payload); // Pilot mode, see UALBERTA_PILOT_MODE + p->mode = mode; // uint8_t:System mode, see UALBERTA_AUTOPILOT_MODE ENUM + p->nav_mode = nav_mode; // uint8_t:Navigation mode, see UALBERTA_NAV_MODE ENUM + p->pilot = pilot; // uint8_t:Pilot mode, see UALBERTA_PILOT_MODE - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_UALBERTA_SYS_STATUS_LEN); } /** @@ -48,14 +48,14 @@ static inline uint16_t mavlink_msg_ualberta_sys_status_pack(uint8_t system_id, u */ static inline uint16_t mavlink_msg_ualberta_sys_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t mode, uint8_t nav_mode, uint8_t pilot) { - uint16_t i = 0; + mavlink_ualberta_sys_status_t *p = (mavlink_ualberta_sys_status_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_UALBERTA_SYS_STATUS; - i += put_uint8_t_by_index(mode, i, msg->payload); // System mode, see UALBERTA_AUTOPILOT_MODE ENUM - i += put_uint8_t_by_index(nav_mode, i, msg->payload); // Navigation mode, see UALBERTA_NAV_MODE ENUM - i += put_uint8_t_by_index(pilot, i, msg->payload); // Pilot mode, see UALBERTA_PILOT_MODE + p->mode = mode; // uint8_t:System mode, see UALBERTA_AUTOPILOT_MODE ENUM + p->nav_mode = nav_mode; // uint8_t:Navigation mode, see UALBERTA_NAV_MODE ENUM + p->pilot = pilot; // uint8_t:Pilot mode, see UALBERTA_PILOT_MODE - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_UALBERTA_SYS_STATUS_LEN); } /** @@ -79,13 +79,37 @@ static inline uint16_t mavlink_msg_ualberta_sys_status_encode(uint8_t system_id, * @param nav_mode Navigation mode, see UALBERTA_NAV_MODE ENUM * @param pilot Pilot mode, see UALBERTA_PILOT_MODE */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_ualberta_sys_status_send(mavlink_channel_t chan, uint8_t mode, uint8_t nav_mode, uint8_t pilot) { - mavlink_message_t msg; - mavlink_msg_ualberta_sys_status_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, mode, nav_mode, pilot); - mavlink_send_uart(chan, &msg); + mavlink_header_t hdr; + mavlink_ualberta_sys_status_t payload; + uint16_t checksum; + mavlink_ualberta_sys_status_t *p = &payload; + + p->mode = mode; // uint8_t:System mode, see UALBERTA_AUTOPILOT_MODE ENUM + p->nav_mode = nav_mode; // uint8_t:Navigation mode, see UALBERTA_NAV_MODE ENUM + p->pilot = pilot; // uint8_t:Pilot mode, see UALBERTA_PILOT_MODE + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_UALBERTA_SYS_STATUS_LEN; + hdr.msgid = MAVLINK_MSG_ID_UALBERTA_SYS_STATUS; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -98,7 +122,8 @@ static inline void mavlink_msg_ualberta_sys_status_send(mavlink_channel_t chan, */ static inline uint8_t mavlink_msg_ualberta_sys_status_get_mode(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_ualberta_sys_status_t *p = (mavlink_ualberta_sys_status_t *)&msg->payload[0]; + return (uint8_t)(p->mode); } /** @@ -108,7 +133,8 @@ static inline uint8_t mavlink_msg_ualberta_sys_status_get_mode(const mavlink_mes */ static inline uint8_t mavlink_msg_ualberta_sys_status_get_nav_mode(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_ualberta_sys_status_t *p = (mavlink_ualberta_sys_status_t *)&msg->payload[0]; + return (uint8_t)(p->nav_mode); } /** @@ -118,7 +144,8 @@ static inline uint8_t mavlink_msg_ualberta_sys_status_get_nav_mode(const mavlink */ static inline uint8_t mavlink_msg_ualberta_sys_status_get_pilot(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_ualberta_sys_status_t *p = (mavlink_ualberta_sys_status_t *)&msg->payload[0]; + return (uint8_t)(p->pilot); } /** @@ -129,7 +156,5 @@ static inline uint8_t mavlink_msg_ualberta_sys_status_get_pilot(const mavlink_me */ static inline void mavlink_msg_ualberta_sys_status_decode(const mavlink_message_t* msg, mavlink_ualberta_sys_status_t* ualberta_sys_status) { - ualberta_sys_status->mode = mavlink_msg_ualberta_sys_status_get_mode(msg); - ualberta_sys_status->nav_mode = mavlink_msg_ualberta_sys_status_get_nav_mode(msg); - ualberta_sys_status->pilot = mavlink_msg_ualberta_sys_status_get_pilot(msg); + memcpy( ualberta_sys_status, msg->payload, sizeof(mavlink_ualberta_sys_status_t)); } diff --git a/thirdParty/mavlink/include/ualberta/ualberta.h b/thirdParty/mavlink/include/ualberta/ualberta.h index b492ff62d65eead6b12bd73363dcbb4b22791ae2..c0c03d82799e60c4cd307de501f3a1ec6038107f 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 Tuesday, August 9 2011, 16:16 UTC */ #ifndef UALBERTA_H #define UALBERTA_H @@ -30,7 +30,7 @@ extern "C" { // ENUM DEFINITIONS -/** @brief Available autopilot modes for ualberta uav */ +/** @brief Available autopilot modes for ualberta uav */ enum UALBERTA_AUTOPILOT_MODE { MODE_MANUAL_DIRECT=0, /* */ @@ -41,7 +41,7 @@ enum UALBERTA_AUTOPILOT_MODE UALBERTA_AUTOPILOT_MODE_ENUM_END }; -/** @brief Navigation filter mode */ +/** @brief Navigation filter mode */ enum UALBERTA_NAV_MODE { NAV_AHRS_INIT=0, @@ -51,7 +51,7 @@ enum UALBERTA_NAV_MODE UALBERTA_NAV_MODE_ENUM_END }; -/** @brief Mode currently commanded by pilot */ +/** @brief Mode currently commanded by pilot */ enum UALBERTA_PILOT_MODE { PILOT_MANUAL=0, /* */ @@ -68,10 +68,10 @@ enum UALBERTA_PILOT_MODE #include "./mavlink_msg_ualberta_sys_status.h" -// MESSAGE LENGTHS +// MESSAGE CRC KEYS -#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 } +#undef MAVLINK_MESSAGE_KEYS +#define MAVLINK_MESSAGE_KEYS { 71, 249, 232, 226, 76, 126, 117, 186, 0, 165, 44, 249, 133, 0, 0, 0, 0, 0, 0, 0, 112, 34, 81, 152, 0, 223, 28, 99, 28, 21, 243, 240, 204, 21, 111, 43, 192, 234, 22, 197, 192, 192, 166, 34, 233, 34, 166, 158, 142, 60, 10, 75, 249, 247, 234, 157, 125, 0, 0, 0, 85, 211, 62, 75, 185, 18, 42, 38, 0, 127, 200, 0, 0, 53, 251, 20, 22, 0, 0, 0, 0, 0, 0, 0, 0, 127, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 8, 238, 165, 0, 0, 0, 0, 0, 0, 0, 218, 218, 235, 0, 0, 0, 0, 0, 0, 225, 114, 0, 0, 0, 0, 0, 0, 0, 0, 221, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 171, 122, 0, 0, 0, 0, 0, 0, 0, 92, 114, 4, 169, 10, 0, 0, 0, 0, 0, 72, 62, 83, 0, 0, 0, 0, 0, 0, 0, 202, 144, 106, 254, 0, 0, 255, 185, 0, 14, 136, 53, 0, 212, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 255, 73, 239, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 43, 141, 211, 166 } #ifdef __cplusplus } diff --git a/thirdParty/mavlink/message_definitions/ardupilotmega.xml b/thirdParty/mavlink/message_definitions/ardupilotmega.xml index 39d96eab515cf9fb19b6fdf0bf167e369be00a50..72bf88a7cf8ade6a6ee2cc3e7ac63795021053a5 100644 --- a/thirdParty/mavlink/message_definitions/ardupilotmega.xml +++ b/thirdParty/mavlink/message_definitions/ardupilotmega.xml @@ -1,6 +1,6 @@ - common.xml - - + common.xml + + diff --git a/thirdParty/mavlink/message_definitions/common.xml b/thirdParty/mavlink/message_definitions/common.xml index 5df9ba827ee9112b0755dd4ece6918f48a71bbb7..4d4c1e52a519c762a35d975fa76b8fe92c98fee7 100644 --- a/thirdParty/mavlink/message_definitions/common.xml +++ b/thirdParty/mavlink/message_definitions/common.xml @@ -1,892 +1,1204 @@ - + -2 - - - - Commands to be executed by the MAV. They can be executed on user request, + 3 + + + Micro air vehicle / autopilot classes. This identifies the individual model. + + Generic autopilot, full support for everything + + + PIXHAWK autopilot, http://pixhawk.ethz.ch + + + SLUGS autopilot, http://slugsuav.soe.ucsc.edu + + + ArduPilotMega / ArduCopter, http://diydrones.com + + + OpenPilot, http://openpilot.org + + + Generic autopilot only supporting simple waypoints + + + Generic autopilot supporting waypoints and other simple navigation commands + + + Generic autopilot supporting the full mission command set + + + No valid autopilot, e.g. a GCS or other MAVLink component + + + PPZ UAV - http://nongnu.org/paparazzi + + + UAV Dev Board + + + FlexiPilot + + + + + + System is in undefined state + + + Motors are blocked, system is safe + + + System is allowed to be active, under manual (RC) control + + + System is allowed to be active, under autonomous control, manual setpoint + + + System is allowed to be active, under autonomous control and navigation + + + Generic test mode, for custom use + + + Generic test mode, for custom use + + + Generic test mode, for custom use + + + System is ready, motors are unblocked, but controllers are inactive + + + System is blocked, only RC valued are read and reported back + + + + + System is currently on ground. + + + System is during liftoff, not in normal navigation mode yet. + + + System is holding its current position (rotorcraft or rover / boat). + + + System is navigating towards the next waypoint. + + + System is flying at a defined course and speed. + + + System is return straight to home position. + + + System is landing. + + + System lost its position input and is in attitude / flight stabilization only. + + + System is loitering in wait position. DO NOT USE THIS FOR WAYPOINT LOITER! + + + + + Uninitialized system, state is unknown. + + + System is booting up. + + + System is calibrating and not flight-ready. + + + System is grounded and on standby. It can be launched any time. + + + System is active and might be already airborne. Motors are engaged. + + + System is in a non-normal flight mode. It can however still navigate. + + + System is in a non-normal flight mode. It lost control over parts or over the whole airframe. It is in mayday and going down. + + + System just initialized its power-down sequence, will shut down now. + + + + 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. + + + + + Generic micro air vehicle. + + + Fixed wing aircraft. + + + Quadrotor + + + Coaxial helicopter + + + Normal helicopter with tail rotor. + + + Ground installation + + + Operator control unit / ground control station + + + Airship, controlled + + + Free balloon, uncontrolled + + + Rocket + + + Ground rover + + + Surface vessel, boat, ship + + + Submarine + + + Hexarotor + + + Octorotor + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Global coordinate frame, WGS84 coordinate system. + + + Local coordinate frame, Z-up (x: north, y: east, z: down). + + + NOT a coordinate frame, indicates a mission command. + + + Global coordinate frame, WGS84 coordinate system, relative altitude over ground. + + + Local coordinate frame, Z-down (x: east, y: north, z: up) + + + + + + + + + + + + + + + + + + + + + + + + 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 - 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 -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 -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 - 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 -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 - - - + 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 + + + + + + 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 MAV_CMD ID + 0: Action DENIED, 1: Action executed + + + 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) above MSL + GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + GPS ground speed (m/s * 100). If unknown, set to: 65535 + Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + Number of satellites visible. If unknown, set to 255 + + + 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) + + + IMPORTANT: Please use GPS_RAW_INT for maximum precision. The max FLOAT resolution limits the resolution to about 1.8 m on some places on the earth. 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 + Number of satellites visible + + + 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 + + + Set roll, pitch and yaw. + System ID + Component ID + Desired roll angle in radians + Desired pitch angle in radians + Desired yaw angle in radians + + + 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 + + + Setpoint in roll, pitch, yaw currently active on the system. + Timestamp in milliseconds since system boot + Desired roll angle in radians + Desired pitch angle in radians + Desired yaw angle in radians + + + Setpoint in rollspeed, pitchspeed, yawspeed currently active on the system. + Timestamp in milliseconds since system boot + Desired roll angular speed in rad/s + Desired pitch angular speed in rad/s + Desired yaw angular speed in rad/s + + + 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 + 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. + + + This packet is useful for high throughput + applications such as hardware in the loop. + + 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) + + + 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), above MSL + 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 + Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + + + 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 + + + + Send raw controller memory. The use of this message is discouraged for normal packets, but a quite efficient way for testing new messages and getting experimental debug output. + Starting address of the debug variables + Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below + Type code of the memory variables. for ver = 1: 0=16 x int16_t, 1=16 x uint16_t, 2=16 x Q15, 3=16 x 1Q14 + Memory contents at specified address + + + 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 + + +>>>>>>> 26a845613229fbb0cb8bee589c12f477fd30a42f 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..0eb3bb638e85b1098cd16207e00f537fdc5b7c22 100644 --- a/thirdParty/mavlink/message_definitions/pixhawk.xml +++ b/thirdParty/mavlink/message_definitions/pixhawk.xml @@ -1,262 +1,235 @@ - + -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 - - - + 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 + + 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..60b653a7982396c3e21ecf2c204e19e86c7b2307 --- /dev/null +++ b/thirdParty/mavlink/missionlib/testing/main.c @@ -0,0 +1,1221 @@ +/******************************************************************************* + + 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 + +#include +<<<<<<< HEAD +#include +======= +>>>>>>> 26a845613229fbb0cb8bee589c12f477fd30a42f + +#include <../mavlink_types.h> + +mavlink_system_t mavlink_system; + +/* This assumes you have the mavlink headers on your include path + or in the same folder as this source file */ +#include + + +<<<<<<< HEAD +======= +/* Provide the interface functions for the waypoint manager */ + +/* + * @brief Sends a MAVLink message over UDP + */ +void mavlink_wpm_send_message(mavlink_message_t* msg) +{ + 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); +} + + +#include + + +>>>>>>> 26a845613229fbb0cb8bee589c12f477fd30a42f +#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(); + + +<<<<<<< HEAD +// 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; + +mavlink_wpm_storage wpm; + +bool debug = true; +bool verbose = true; + + +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 a MAVLink message over UDP + */ +void mavlink_wpm_send_message(mavlink_message_t* msg) +{ + 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; +} + +/* + * @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) + { + //printf("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system); + 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; +} + + +static void mavlink_wpm_message_handler(const mavlink_message_t* msg) +{ + // Handle param messages + //paramClient->handleMAVLinkPacket(msg); + + //check for timed-out operations + struct timeval tv; + gettimeofday(&tv, NULL); + uint64_t now = ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec; + if (now-wpm.timestamp_lastaction > wpm.timeout && wpm.current_state != MAVLINK_WPM_STATE_IDLE) + { + if (verbose) printf("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm.current_state); + 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) + { + if (verbose) printf("Received Ack after having sent last waypoint, going to state MAVLINK_WPM_STATE_IDLE\n"); + wpm.current_state = MAVLINK_WPM_STATE_IDLE; + wpm.current_wp_id = 0; + } + } + } + else + { + if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); + } + 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; + } + } + if (verbose) printf("New current waypoint %u\n", wpm.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; + } + else + { + if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: Index out of bounds\n"); + } + } + else + { + if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE NOT IN IDLE STATE\n"); + } + } + else + { + if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); + } + 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 (verbose && wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) 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); + if (verbose && wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm.current_wp_id + 1) 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); + if (verbose && wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm.current_wp_id) 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); + + 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)) { printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because i'm doing something else already (state=%i).\n", wpm.current_state); break; } + else if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) + { + if (wpr.seq != 0) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the first requested waypoint ID (%u) was not 0.\n", wpr.seq); + } + else if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) + { + if (wpr.seq != wpm.current_wp_id && wpr.seq != wpm.current_wp_id + 1) 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); + else if (wpr.seq >= wpm.size) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was out of bounds.\n", wpr.seq); + } + else printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST - FIXME: missed error description\n"); + } + } + } + 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)) + { + if (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); + } + else + { + if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); + } + + } + 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 (verbose && wpm.current_state == MAVLINK_WPM_STATE_IDLE) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST\n", wpc.count, msg->sysid); + if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) again from %u\n", wpc.count, msg->sysid); + + 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; + + printf("clearing receive buffer and readying for receiving waypoints\n"); + 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) + { + printf("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE\n"); + 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 + { + if (verbose) printf("Ignoring MAVLINK_MSG_ID_WAYPOINT_COUNT from %u with count of %u\n", msg->sysid, wpc.count); + } + } + else + { + if (verbose && !(wpm.current_state == MAVLINK_WPM_STATE_IDLE || wpm.current_state == MAVLINK_WPM_STATE_GETLIST)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm doing something else already (state=%i).\n", wpm.current_state); + else if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST && wpm.current_wp_id != 0) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm already receiving waypoint %u.\n", wpm.current_wp_id); + else printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT - FIXME: missed error description\n"); + } + } + else + { + if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); + } + + } + 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; + } +} + + + + + + + + +======= +>>>>>>> 26a845613229fbb0cb8bee589c12f477fd30a42f +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_TYPE_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)); + +<<<<<<< HEAD +// /* 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)); +// +======= + /* 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)); + +>>>>>>> 26a845613229fbb0cb8bee589c12f477fd30a42f + + 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); + sleep(1); // 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/udp.c b/thirdParty/mavlink/missionlib/testing/udp.c new file mode 100644 index 0000000000000000000000000000000000000000..dc096be51bd2b38c58240eb833703e90af31c54f --- /dev/null +++ b/thirdParty/mavlink/missionlib/testing/udp.c @@ -0,0 +1,1073 @@ +/******************************************************************************* + + 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 + +/* This assumes you have the mavlink headers on your include path + or in the same folder as this source file */ +#include + + +#define BUFFER_LENGTH 2041 // minimum buffer size that can be used with qnx (I don't know why) + +uint64_t microsSinceEpoch(); + + +// 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 ///< Report back status information as text +#define MAVLINK_WPM_SYSTEM_ID 1 +#define MAVLINK_WPM_COMPONENT_ID 1 + +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 count; + MAVLINK_WPM_STATES current_state; +} mavlink_wpm_storage; + + +void mavlink_wpm_init(mavlink_wpm_storage* state) +{ + // Set all waypoints to zero + + // Set count to zero + state->count = 0; + state->current_state = MAVLINK_WPM_STATE_IDLE; +} + + +PX_WAYPOINTPLANNER_STATES current_state = PX_WPP_IDLE; +uint16_t protocol_current_wp_id = 0; +uint16_t protocol_current_count = 0; +uint8_t protocol_current_partner_systemid = 0; +uint8_t protocol_current_partner_compid = 0; +uint64_t protocol_timestamp_lastaction = 0; + +uint64_t timestamp_last_send_setpoint = 0; + + +/* + * @brief Sends an waypoint ack message + */ +void mavlink_wpm_send_waypoint_ack(uint8_t target_systemid, uint8_t target_compid, uint8_t type) +{ + mavlink_message_t msg; + mavlink_waypoint_ack_t wpa; + + wpa.target_system = target_systemid; + wpa.target_component = target_compid; + wpa.type = type; + + mavlink_msg_waypoint_ack_encode(systemid, compid, &msg, &wpa); + mavlink_message_t_publish(lcm, "MAVLINK", &msg); + + usleep(paramClient->getParamValue("PROTOCOLDELAY")); + + if (verbose) printf("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system); +} + +/* + * @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 < waypoints->size()) + { + mavlink_waypoint_t *cur = waypoints->at(seq); + + mavlink_message_t msg; + mavlink_waypoint_current_t wpc; + + wpc.seq = cur->seq; + + mavlink_msg_waypoint_current_encode(systemid, compid, &msg, &wpc); + mavlink_message_t_publish(lcm, "MAVLINK", &msg); + + usleep(paramClient->getParamValue("PROTOCOLDELAY")); + + if (verbose) printf("Broadcasted new current waypoint %u\n", wpc.seq); + } + else + { + if (verbose) printf("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 < waypoints->size()) + { + mavlink_waypoint_t *cur = waypoints->at(seq); + + mavlink_message_t msg; + mavlink_local_position_setpoint_set_t PControlSetPoint; + + // send new set point to local IMU + if (cur->frame == 1) + { + PControlSetPoint.target_system = systemid; + PControlSetPoint.target_component = MAV_COMP_ID_IMU; + PControlSetPoint.x = cur->x; + PControlSetPoint.y = cur->y; + PControlSetPoint.z = cur->z; + PControlSetPoint.yaw = cur->param4; + + mavlink_msg_local_position_setpoint_set_encode(systemid, compid, &msg, &PControlSetPoint); + mavlink_message_t_publish(lcm, "MAVLINK", &msg); + + usleep(paramClient->getParamValue("PROTOCOLDELAY")); + } + else + { + if (verbose) printf("No new set point sent to IMU because the new waypoint %u had no local coordinates\n", cur->seq); + } + + struct timeval tv; + gettimeofday(&tv, NULL); + uint64_t now = ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec; + timestamp_last_send_setpoint = now; + } + else + { + if (verbose) printf("ERROR: index out of bounds\n"); + } +} + +void mavlink_wpm_send_waypoint_count(uint8_t target_systemid, uint8_t target_compid, uint16_t count) +{ + mavlink_message_t msg; + mavlink_waypoint_count_t wpc; + + wpc.target_system = target_systemid; + wpc.target_component = target_compid; + wpc.count = count; + + mavlink_msg_waypoint_count_encode(systemid, compid, &msg, &wpc); + mavlink_message_t_publish(lcm, "MAVLINK", &msg); + + if (verbose) printf("Sent waypoint count (%u) to ID %u\n", wpc.count, wpc.target_system); + + usleep(paramClient->getParamValue("PROTOCOLDELAY")); +} + +void mavlink_wpm_send_waypoint(uint8_t target_systemid, uint8_t target_compid, uint16_t seq) +{ + if (seq < waypoints->size()) + { + mavlink_message_t msg; + mavlink_waypoint_t *wp = waypoints->at(seq); + wp->target_system = target_systemid; + wp->target_component = target_compid; + mavlink_msg_waypoint_encode(systemid, compid, &msg, wp); + mavlink_message_t_publish(lcm, "MAVLINK", &msg); + if (verbose) printf("Sent waypoint %u to ID %u\n", wp->seq, wp->target_system); + + usleep(paramClient->getParamValue("PROTOCOLDELAY")); + } + else + { + if (verbose) printf("ERROR: index out of bounds\n"); + } +} + +void mavlink_wpm_send_waypoint_request(uint8_t target_systemid, uint8_t target_compid, uint16_t seq) +{ + if (seq < waypoints->size()) + { + mavlink_message_t msg; + mavlink_waypoint_request_t wpr; + wpr.target_system = target_systemid; + wpr.target_component = target_compid; + wpr.seq = seq; + mavlink_msg_waypoint_request_encode(systemid, compid, &msg, &wpr); + mavlink_message_t_publish(lcm, "MAVLINK", &msg); + if (verbose) printf("Sent waypoint request %u to ID %u\n", wpr.seq, wpr.target_system); + + usleep(paramClient->getParamValue("PROTOCOLDELAY")); + } + else + { + if (verbose) printf("ERROR: index out of bounds\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(systemid, compid, &msg, &wp_reached); + mavlink_message_t_publish(lcm, "MAVLINK", &msg); + + if (verbose) printf("Sent waypoint %u reached message\n", wp_reached.seq); + + usleep(paramClient->getParamValue("PROTOCOLDELAY")); +} + +float mavlink_wpm_distance_to_segment(uint16_t seq, float x, float y, float z) +{ + if (seq < waypoints->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) < waypoints->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 < waypoints->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; +} + + +static void mavlink_wpm_mavlink_handler(const lcm_recv_buf_t *rbuf, const char * channel, const mavlink_message_t* msg, void * user) +{ + // Handle param messages + paramClient->handleMAVLinkPacket(msg); + + //check for timed-out operations + struct timeval tv; + gettimeofday(&tv, NULL); + uint64_t now = ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec; + if (now-protocol_timestamp_lastaction > paramClient->getParamValue("PROTOCOLTIMEOUT") && current_state != PX_WPP_IDLE) + { + if (verbose) printf("Last operation (state=%u) timed out, changing state to PX_WPP_IDLE\n", current_state); + current_state = PX_WPP_IDLE; + protocol_current_count = 0; + protocol_current_partner_systemid = 0; + protocol_current_partner_compid = 0; + protocol_current_wp_id = -1; + + if(waypoints->size() == 0) + { + current_active_wp_id = -1; + } + } + + if(now-timestamp_last_send_setpoint > paramClient->getParamValue("SETPOINTDELAY") && current_active_wp_id < waypoints->size()) + { + send_setpoint(current_active_wp_id); + } + + switch(msg->msgid) + { + case MAVLINK_MSG_ID_ATTITUDE: + { + if(msg->sysid == systemid && current_active_wp_id < waypoints->size()) + { + mavlink_waypoint_t *wp = waypoints->at(current_active_wp_id); + if(wp->frame == 1) + { + mavlink_attitude_t att; + mavlink_msg_attitude_decode(msg, &att); + float yaw_tolerance = paramClient->getParamValue("YAWTOLERANCE"); + //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) + yawReached = 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) + yawReached = true; + } + else + { + float upperBound = att.yaw + yaw_tolerance - 2.f*M_PI; + if (att.yaw - yaw_tolerance < wp->param4 || wp->param4 < upperBound) + yawReached = true; + } + } + } + break; + } + + case MAVLINK_MSG_ID_LOCAL_POSITION: + { + if(msg->sysid == systemid && current_active_wp_id < waypoints->size()) + { + mavlink_waypoint_t *wp = waypoints->at(current_active_wp_id); + + if(wp->frame == 1) + { + 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); + + posReached = false; + + // compare current position (given in message) with current waypoint + float orbit = wp->param1; + + float dist; + if (wp->param2 == 0) + { + dist = distanceToSegment(current_active_wp_id, pos.x, pos.y, pos.z); + } + else + { + dist = distanceToPoint(current_active_wp_id, pos.x, pos.y, pos.z); + } + + if (dist >= 0.f && dist <= orbit && yawReached) + { + posReached = 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 == systemid) + { + 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 (waypoints->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 == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid) && (wpa.target_system == systemid && wpa.target_component == compid)) + { + protocol_timestamp_lastaction = now; + + if (current_state == PX_WPP_SENDLIST || current_state == PX_WPP_SENDLIST_SENDWPS) + { + if (protocol_current_wp_id == waypoints->size()-1) + { + if (verbose) printf("Received Ack after having sent last waypoint, going to state PX_WPP_IDLE\n"); + current_state = PX_WPP_IDLE; + protocol_current_wp_id = 0; + } + } + } + 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 == systemid && wpc.target_component == compid) + { + protocol_timestamp_lastaction = now; + + if (current_state == PX_WPP_IDLE) + { + if (wpc.seq < waypoints->size()) + { + if (verbose) printf("Received MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT\n"); + current_active_wp_id = wpc.seq; + uint32_t i; + for(i = 0; i < waypoints->size(); i++) + { + if (i == current_active_wp_id) + { + waypoints->at(i)->current = true; + } + else + { + waypoints->at(i)->current = false; + } + } + if (verbose) printf("New current waypoint %u\n", current_active_wp_id); + yawReached = false; + posReached = false; + send_waypoint_current(current_active_wp_id); + send_setpoint(current_active_wp_id); + timestamp_firstinside_orbit = 0; + } + else + { + if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: Index out of bounds\n"); + } + } + } + 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 == systemid && wprl.target_component == compid) + { + protocol_timestamp_lastaction = now; + + if (current_state == PX_WPP_IDLE || current_state == PX_WPP_SENDLIST) + { + if (waypoints->size() > 0) + { + if (verbose && current_state == PX_WPP_IDLE) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u changing state to PX_WPP_SENDLIST\n", msg->sysid); + if (verbose && current_state == PX_WPP_SENDLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST again from %u staying in state PX_WPP_SENDLIST\n", msg->sysid); + current_state = PX_WPP_SENDLIST; + protocol_current_wp_id = 0; + protocol_current_partner_systemid = msg->sysid; + protocol_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); + } + protocol_current_count = waypoints->size(); + send_waypoint_count(msg->sysid,msg->compid, protocol_current_count); + } + else + { + if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST because i'm doing something else already (state=%i).\n", current_state); + } + } + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_REQUEST: + { + mavlink_waypoint_request_t wpr; + mavlink_msg_waypoint_request_decode(msg, &wpr); + if(msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid && wpr.target_system == systemid && wpr.target_component == compid) + { + protocol_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 ((current_state == PX_WPP_SENDLIST && wpr.seq == 0) || (current_state == PX_WPP_SENDLIST_SENDWPS && (wpr.seq == protocol_current_wp_id || wpr.seq == protocol_current_wp_id + 1) && wpr.seq < waypoints->size())) + { + if (verbose && current_state == PX_WPP_SENDLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u changing state to PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); + if (verbose && current_state == PX_WPP_SENDLIST_SENDWPS && wpr.seq == protocol_current_wp_id + 1) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u staying in state PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); + if (verbose && current_state == PX_WPP_SENDLIST_SENDWPS && wpr.seq == protocol_current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u (again) from %u staying in state PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); + + current_state = PX_WPP_SENDLIST_SENDWPS; + protocol_current_wp_id = wpr.seq; + send_waypoint(protocol_current_partner_systemid, protocol_current_partner_compid, wpr.seq); + } + else + { + if (verbose) + { + if (!(current_state == PX_WPP_SENDLIST || current_state == PX_WPP_SENDLIST_SENDWPS)) { printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because i'm doing something else already (state=%i).\n", current_state); break; } + else if (current_state == PX_WPP_SENDLIST) + { + if (wpr.seq != 0) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the first requested waypoint ID (%u) was not 0.\n", wpr.seq); + } + else if (current_state == PX_WPP_SENDLIST_SENDWPS) + { + if (wpr.seq != protocol_current_wp_id && wpr.seq != protocol_current_wp_id + 1) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).\n", wpr.seq, protocol_current_wp_id, protocol_current_wp_id+1); + else if (wpr.seq >= waypoints->size()) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was out of bounds.\n", wpr.seq); + } + else printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST - FIXME: missed error description\n"); + } + } + } + else + { + //we we're target but already communicating with someone else + if((wpr.target_system == systemid && wpr.target_component == compid) && !(msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid)) + { + if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST from ID %u because i'm already talking to ID %u.\n", msg->sysid, protocol_current_partner_systemid); + } + } + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_COUNT: + { + mavlink_waypoint_count_t wpc; + mavlink_msg_waypoint_count_decode(msg, &wpc); + if(wpc.target_system == systemid && wpc.target_component == compid) + { + protocol_timestamp_lastaction = now; + + if (current_state == PX_WPP_IDLE || (current_state == PX_WPP_GETLIST && protocol_current_wp_id == 0)) + { + if (wpc.count > 0) + { + if (verbose && current_state == PX_WPP_IDLE) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) from %u changing state to PX_WPP_GETLIST\n", wpc.count, msg->sysid); + if (verbose && current_state == PX_WPP_GETLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) again from %u\n", wpc.count, msg->sysid); + + current_state = PX_WPP_GETLIST; + protocol_current_wp_id = 0; + protocol_current_partner_systemid = msg->sysid; + protocol_current_partner_compid = msg->compid; + protocol_current_count = wpc.count; + + printf("clearing receive buffer and readying for receiving waypoints\n"); + while(waypoints_receive_buffer->size() > 0) + { + delete waypoints_receive_buffer->back(); + waypoints_receive_buffer->pop_back(); + } + + send_waypoint_request(protocol_current_partner_systemid, protocol_current_partner_compid, protocol_current_wp_id); + } + else if (wpc.count == 0) + { + printf("got waypoint count of 0, clearing waypoint list and staying in state PX_WPP_IDLE\n"); + while(waypoints_receive_buffer->size() > 0) + { + delete waypoints->back(); + waypoints->pop_back(); + } + current_active_wp_id = -1; + yawReached = false; + posReached = false; + break; + + } + else + { + if (verbose) printf("Ignoring MAVLINK_MSG_ID_WAYPOINT_COUNT from %u with count of %u\n", msg->sysid, wpc.count); + } + } + else + { + if (verbose && !(current_state == PX_WPP_IDLE || current_state == PX_WPP_GETLIST)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm doing something else already (state=%i).\n", current_state); + else if (verbose && current_state == PX_WPP_GETLIST && protocol_current_wp_id != 0) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm already receiving waypoint %u.\n", protocol_current_wp_id); + else printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT - FIXME: missed error description\n"); + } + } + break; + } + + case MAVLINK_MSG_ID_WAYPOINT: + { + mavlink_waypoint_t wp; + mavlink_msg_waypoint_decode(msg, &wp); + + if((msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid) && (wp.target_system == systemid && wp.target_component == compid)) + { + protocol_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 ((current_state == PX_WPP_GETLIST && wp.seq == 0) || (current_state == PX_WPP_GETLIST_GETWPS && wp.seq == protocol_current_wp_id && wp.seq < protocol_current_count)) + { + if (verbose && current_state == PX_WPP_GETLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u changing state to PX_WPP_GETLIST_GETWPS\n", wp.seq, msg->sysid); + if (verbose && current_state == PX_WPP_GETLIST_GETWPS && wp.seq == protocol_current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u\n", wp.seq, msg->sysid); + if (verbose && current_state == PX_WPP_GETLIST_GETWPS && wp.seq-1 == protocol_current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT %u (again) from %u\n", wp.seq, msg->sysid); + + current_state = PX_WPP_GETLIST_GETWPS; + protocol_current_wp_id = wp.seq + 1; + mavlink_waypoint_t* newwp = new mavlink_waypoint_t; + memcpy(newwp, &wp, sizeof(mavlink_waypoint_t)); + waypoints_receive_buffer->push_back(newwp); + + 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(protocol_current_wp_id == protocol_current_count && current_state == PX_WPP_GETLIST_GETWPS) + { + if (verbose) printf("Got all %u waypoints, changing state to PX_WPP_IDLE\n", protocol_current_count); + + send_waypoint_ack(protocol_current_partner_systemid, protocol_current_partner_compid, 0); + + if (current_active_wp_id > waypoints_receive_buffer->size()-1) + { + current_active_wp_id = waypoints_receive_buffer->size() - 1; + } + + // switch the waypoints list + std::vector* waypoints_temp = waypoints; + waypoints = waypoints_receive_buffer; + waypoints_receive_buffer = waypoints_temp; + + //get the new current waypoint + uint32_t i; + for(i = 0; i < waypoints->size(); i++) + { + if (waypoints->at(i)->current == 1) + { + current_active_wp_id = i; + //if (verbose) printf("New current waypoint %u\n", current_active_wp_id); + yawReached = false; + posReached = false; + send_waypoint_current(current_active_wp_id); + send_setpoint(current_active_wp_id); + timestamp_firstinside_orbit = 0; + break; + } + } + + if (i == waypoints->size()) + { + current_active_wp_id = -1; + yawReached = false; + posReached = false; + timestamp_firstinside_orbit = 0; + } + + current_state = PX_WPP_IDLE; + } + else + { + send_waypoint_request(protocol_current_partner_systemid, protocol_current_partner_compid, protocol_current_wp_id); + } + } + else + { + if (current_state == PX_WPP_IDLE) + { + //we're done receiving waypoints, answer with ack. + send_waypoint_ack(protocol_current_partner_systemid, protocol_current_partner_compid, 0); + printf("Received MAVLINK_MSG_ID_WAYPOINT while state=PX_WPP_IDLE, answered with WAYPOINT_ACK.\n"); + } + if (verbose) + { + if (!(current_state == PX_WPP_GETLIST || current_state == PX_WPP_GETLIST_GETWPS)) { printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u because i'm doing something else already (state=%i).\n", wp.seq, current_state); break; } + else if (current_state == PX_WPP_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 (current_state == PX_WPP_GETLIST_GETWPS) + { + if (!(wp.seq == protocol_current_wp_id)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was not the expected %u.\n", wp.seq, protocol_current_wp_id); + else if (!(wp.seq < protocol_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 == systemid && wp.target_component == compid) && !(msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid) && current_state != PX_WPP_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, protocol_current_partner_systemid); + } + else if(wp.target_system == systemid && wp.target_component == 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 == systemid && wpca.target_component == compid && current_state == PX_WPP_IDLE) + { + protocol_timestamp_lastaction = now; + + if (verbose) printf("Got MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u deleting all waypoints\n", msg->sysid); + while(waypoints->size() > 0) + { + delete waypoints->back(); + waypoints->pop_back(); + } + current_active_wp_id = -1; + yawReached = false; + posReached = false; + } + else if (wpca.target_system == systemid && wpca.target_component == compid && current_state != PX_WPP_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, current_state); + } + break; + } + + default: + { + if (debug) std::cerr << "Waypoint: received message of unknown type" << std::endl; + break; + } + } + + //check if the current waypoint was reached + if ((posReached && /*yawReached &&*/ !idle)) + { + if (current_active_wp_id < waypoints->size()) + { + mavlink_waypoint_t *cur_wp = waypoints->at(current_active_wp_id); + + if (timestamp_firstinside_orbit == 0) + { + // Announce that last waypoint was reached + if (verbose) printf("*** Reached waypoint %u ***\n", cur_wp->seq); + send_waypoint_reached(cur_wp->seq); + 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-timestamp_firstinside_orbit >= cur_wp->param2*1000) + { + if (cur_wp->autocontinue) + { + cur_wp->current = 0; + if (current_active_wp_id == waypoints->size() - 1 && waypoints->size() > 1) + { + //the last waypoint was reached, if auto continue is + //activated restart the waypoint list from the beginning + current_active_wp_id = 1; + } + else + { + if ((uint16_t)(current_active_wp_id + 1) < waypoints->size()) + current_active_wp_id++; + } + + // Fly to next waypoint + timestamp_firstinside_orbit = 0; + send_waypoint_current(current_active_wp_id); + send_setpoint(current_active_wp_id); + waypoints->at(current_active_wp_id)->current = true; + posReached = false; + yawReached = false; + if (verbose) printf("Set new waypoint (%u)\n", current_active_wp_id); + } + } + } + } + else + { + timestamp_lastoutside_orbit = now; + } +} + + + + + + + + +int main(int argc, char* argv[]) +{ + + char help[] = "--help"; + + + char target_ip[100]; + + float position[6] = {}; + int sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP); + struct sockaddr_in gcAddr; + struct sockaddr_in locAddr; + //struct sockaddr_in fromAddr; + uint8_t buf[BUFFER_LENGTH]; + ssize_t recsize; + socklen_t fromlen; + int bytes_sent; + mavlink_message_t msg; + uint16_t len; + int i = 0; + //int success = 0; + unsigned int temp = 0; + + // 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(1, 200, &msg, MAV_TYPE_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(1, 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(1, 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); + } + } + printf("\n"); + } + memset(buf, 0, BUFFER_LENGTH); + sleep(1); // 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/waypoints.c b/thirdParty/mavlink/missionlib/waypoints.c new file mode 100644 index 0000000000000000000000000000000000000000..d0e7621f758db0741b191abda23d765d5cfd1f50 --- /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 +#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..2c3416a821289dcce894cb151bd4e20b584f091e --- /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 30 +#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);